From 7eb5d4284d123f36d1195321b59bb7a0c36d81bd Mon Sep 17 00:00:00 2001
From: DashyFox <DashyFox@yandex.ru>
Date: Sun, 1 Sep 2024 22:16:17 +0300
Subject: [PATCH] RobotFunctions

---
 Core/Inc/RobotFunctions.h |  13 ++--
 Core/Src/RobotFunctions.c | 144 ++++++++++++++++++++++++++++++++++++++
 2 files changed, 152 insertions(+), 5 deletions(-)
 create mode 100644 Core/Src/RobotFunctions.c

diff --git a/Core/Inc/RobotFunctions.h b/Core/Inc/RobotFunctions.h
index 56b00d7..66eac73 100644
--- a/Core/Inc/RobotFunctions.h
+++ b/Core/Inc/RobotFunctions.h
@@ -9,23 +9,26 @@
 #define INC_ROBOTFUNCTIONS_H_
 
 #include "pca9685.h"
+#include "EEPROM.h"
 
 
-void doShot();
-void doShotForever();
+void doShot(Shot*);
+void doShotForever(uint8_t number);
 
-void prepareShot();
+uint8_t prepareShot(uint8_t number);
 
+void startShooting();
 void stopShooting();
 
 void setPos();
 void setPosDefault();
 
 // 0 .. 100
-void setShneckSpeed(uint8_t speed);
+void setScrewkSpeed(uint8_t speed);
+
 
 //(-v) 0 .. 100(stop) .. 200(+v)
-void setRollersSpeed(uint8_t speed); //(-v) 0 . 100(stop) . 200(+v)
+void setRollersSpeed(uint8_t up, uint8_t down); //(-v) 0 . 100(stop) . 200(+v)
 
 void startProgram(); // shot sequence
 void startMacro(); // shot sequence
diff --git a/Core/Src/RobotFunctions.c b/Core/Src/RobotFunctions.c
new file mode 100644
index 0000000..a2527f6
--- /dev/null
+++ b/Core/Src/RobotFunctions.c
@@ -0,0 +1,144 @@
+/*
+ * RobotFunctions.c
+ *
+ *  Created on: Aug 28, 2024
+ *      Author: DashyFox
+ */
+
+#include "RobotFunctions.h"
+#include "EEPROM.h"
+#include "Print.h"
+
+
+typedef enum Mode {
+	NoneMode,
+	ShotMode,
+	ProgramMode,
+	MacroMode,
+} Mode;
+
+uint8_t isPause = 0;
+uint8_t isShooting = 0;
+
+typedef struct CurrentProgram{
+	Program program;
+	uint8_t shot_index;
+} CurrentProgram;
+
+typedef struct CurrentMacro{
+	Macro macro;
+	uint8_t program_index;
+} CurrentMacro;
+
+typedef struct Current {
+	Mode mode;
+	Shot shot;
+	CurrentProgram program;
+	CurrentMacro macro;
+} Current;
+
+Current current;
+
+
+extern uint8_t Vz1;
+extern uint8_t Vz2;
+void doShot(Shot* shot){
+	SetServo(0, shot->rotationHorizontal);
+	SetServo(1, shot->rotationVertical);
+	SetServo(2, shot->rotationAxial);
+	Vz1 = shot->speedRollerTop;
+	Vz2 = shot->speedRollerBottom;
+	setScrewkSpeed(shot->speedScrew);
+}
+
+void startShooting(){
+	switch (current.mode) {
+		case ShotMode:
+			print("StartShooting\n");
+			if(current.shot.isExist){
+				print("Fire!\n");
+
+				print("isExist             ");printNumber(current.shot.isExist);
+		        print("countRepeatShot;    ");printNumber(current.shot.countRepeatShot);
+		        print("speedRollerTop;     ");printNumber(current.shot.speedRollerTop);
+		        print("speedRollerBottom;  ");printNumber(current.shot.speedRollerBottom);
+		        print("speedScrew;         ");printNumber(current.shot.speedScrew);
+		        print("rotationAxial;      ");printNumber(current.shot.rotationAxial);
+		        print("rotationHorizontal; ");printNumber(current.shot.rotationHorizontal);
+		        print("rotationVertical;   ");printNumber(current.shot.rotationVertical);
+
+
+				isShooting = 1;
+				doShot(&current.shot);
+			} else {
+				print("Current Shot is NULL\n");
+				// TODO: sound_ERR(); ledFX_ERR();
+			}
+			break;
+		case ProgramMode:
+
+			break;
+		case MacroMode:
+
+			break;
+		default:
+			break;
+	}
+}
+
+void stopShooting(){
+	isShooting = 0;
+	isPause = 0;
+
+	setPosDefault();
+	Vz1 = 100;
+	Vz2 = 100;
+	TIM1->CCR1 = 0;
+	TIM1->CCR2 = 0;
+}
+
+void doShotForever(uint8_t number){
+
+}
+
+uint8_t prepareShot(uint8_t number){
+	Shot shot = GetShot(number);
+	if(shot.isExist){
+		current.mode = ShotMode;
+		current.shot = shot;
+		return 1;
+	} else {
+		// TODO: sound_ERR(); ledFX_ERR();
+		return 0;
+	}
+}
+
+
+void setPos(){
+
+}
+void setPosDefault(){
+	SetServo(0, 90);
+	SetServo(1, 90);
+	SetServo(2, 90);
+}
+
+// 0 .. 100
+void setScrewkSpeed(uint8_t speed){
+	TIM1->CCR1 = 0;
+	TIM1->CCR2 = (uint16_t)(40 * speed);
+}
+
+//(-v) 0 .. 100(stop) .. 200(+v)
+void setRollersSpeed(uint8_t up, uint8_t down){
+	Vz1 = up;
+	Vz2 = down;
+}
+// shot sequence
+void startProgram(){
+
+}
+// shot sequence
+void startMacro(){
+
+}