setting the turning parameter from EEPROM

Pera-Swarm · Sep 17, 2023 · 7982533 · 7982533
1 parent 5258d34
commit 7982533
Show file tree

Hide file tree

Showing 5 changed files with 32 additions and 24 deletions.
diff --git a/src/components/motors.cpp b/src/components/motors.cpp
@@ -11,7 +11,7 @@ void Motor::setup_motors()
     pid.SetMode(AUTOMATIC);
 
     // set the eerpom
-    setPIDConstToEEPROM(0.5, 0.005, 0.005, 0.02, 0.005, 0.005, 20);
+    setPIDConstToEEPROM(0.5, 0.005, 0.005, 0.02, 0.005, 0.005, 20, 70, 70, 90);
 
     // update the pid values from eeprom
     getPIDConstFromEEPROM();
@@ -98,7 +98,7 @@ void Motor::tunning(int16_t leftSpeed, int16_t rightSpeed)
 }
 
 // store the pid_const in EEPROM
-bool Motor::setPIDConstToEEPROM(double kpForward, double kiForward, double kdForward, double kpRate, double kiRate, double kdRate, int setTimeForward)
+bool Motor::setPIDConstToEEPROM(double kpForward, double kiForward, double kdForward, double kpRate, double kiRate, double kdRate, int setTimeForward, int turningSpeed, int turningSpeedRes, int angle)
 {
     pid_const.KP_FOWARD = kpForward;
     pid_const.KD_FOWARD = kdForward;
@@ -107,6 +107,9 @@ bool Motor::setPIDConstToEEPROM(double kpForward, double kiForward, double kdFor
     pid_const.KI_RATE = kiRate;
     pid_const.KD_RATE = kdRate;
     pid_const.SET_TIME_FORWARD = setTimeForward;
+    pid_const.TURING_SPEED = turningSpeed;
+    pid_const.TURING_SPEED_REVERSE = turningSpeedRes;
+    pid_const.ANGLE_90 = angle;
 
     return EEPROM_write_struct(ADDRESS, pid_const);
 }
@@ -154,11 +157,11 @@ void pulse(int pulsetime, int time)
     }
 }
 
-void Motor::turn90DegRight()
+void Motor::turn90DegLeft()
 {
     if (motorState != TURNING)
     {
-        setPoint = setPoint + ANGLE_90;
+        setPoint = setPoint + pid_const.ANGLE_90;
         motorState = TURNING;
     }
 
@@ -171,11 +174,11 @@ void Motor::turn90DegRight()
         if ((err > 1.0) || (err < -1.0))
         {
             input = startangle;
-            tunning(10, 10);
+            tunning(pid_const.TURING_SPEED, pid_const.TURING_SPEED);
             pid.Compute();
 
-            ML((err >= -1.0) ? -TURING_SPEED : TURING_SPEED_REVERSE - output);
-            MR((err >= -1.0) ? TURING_SPEED : -TURING_SPEED_REVERSE + output);
+            ML((err >= -1.0) ? -pid_const.TURING_SPEED : pid_const.TURING_SPEED_REVERSE - output);
+            MR((err >= -1.0) ? pid_const.TURING_SPEED : -pid_const.TURING_SPEED_REVERSE + output);
         }
         else
         {
@@ -185,11 +188,11 @@ void Motor::turn90DegRight()
     }
 }
 
-void Motor::turn90DegLeft()
+void Motor::turn90DegRight()
 {
     if (motorState != TURNING)
     {
-        setPoint = setPoint - ANGLE_90;
+        setPoint = setPoint - pid_const.ANGLE_90;
         motorState = TURNING;
     }
 
@@ -202,11 +205,11 @@ void Motor::turn90DegLeft()
         if ((err > 1.0) || (err < -1.0))
         {
             input = startangle;
-            tunning(10, 10);
+            tunning(pid_const.TURING_SPEED, pid_const.TURING_SPEED);
             pid.Compute();
 
-            ML((err >= -1.0) ? TURING_SPEED : -TURING_SPEED_REVERSE - output);
-            MR((err >= -1.0) ? -TURING_SPEED : TURING_SPEED_REVERSE + output);
+            ML((err >= -1.0) ? pid_const.TURING_SPEED : -pid_const.TURING_SPEED_REVERSE - output);
+            MR((err >= -1.0) ? -pid_const.TURING_SPEED : pid_const.TURING_SPEED_REVERSE + output);
         }
         else
         {

diff --git a/src/components/motors.h b/src/components/motors.h
@@ -12,10 +12,6 @@
 #define TURNING 2
 #define RANDOM 3
 
-#define TURING_SPEED 70
-#define TURING_SPEED_REVERSE 70
-#define ANGLE_90 90
-
 // constants for PID
 struct PID_CONST
 {
@@ -29,6 +25,10 @@ struct PID_CONST
     double KD_RATE = 0.005;
 
     int SET_TIME_FORWARD = 20;
+
+    int TURING_SPEED = 70;
+    int TURING_SPEED_REVERSE = 70;
+    int ANGLE_90 = 90;
 };
 
 // motor class that contains basic functionality for the motors
@@ -46,7 +46,7 @@ class Motor
     void updateOutput();                                 // update to output respect to the angle error and const
     void tunning(int16_t leftSpeed, int16_t rightSpeed); // tune the Kp, KI and Kd
 
-    bool setPIDConstToEEPROM(double kpForward, double kiForward, double kdForward, double kpRate, double kiRate, double kdRate, int setTimeForward); // update the PID const in EEPROM
+    bool setPIDConstToEEPROM(double kpForward, double kiForward, double kdForward, double kpRate, double kiRate, double kdRate, int setTimeForward, int turningSpeed, int turningSpeedRes, int angle); // update the PID const in EEPROM
     bool getPIDConstFromEEPROM();
 
     void updateSetPoint(); // get the PID const from EEPROM

diff --git a/src/main.cpp b/src/main.cpp
@@ -1,8 +1,10 @@
 #include "define.h"
 
+#define ADDRESS_MYID 0
+
 // id of the bot
 // TODO: Store this in the EEPROM of the microcontroller
-String myID = "1";
+int myID = 1;
 
 // create Motor instance
 Motor motor;
@@ -126,6 +128,9 @@ void algorithm()
 
 void setup()
 {
+    // EEPROM_write_int(ADDRESS, 1);
+    // myID = EEPROM_read_int(ADDRESS);
+
     // begining the serial commiunication
     Serial.begin(9600);
 
@@ -145,13 +150,13 @@ void setup()
     Serial.println("Bot initiated");
 
     motor.updateSetPoint();
-    motor.turn90DegLeft();
+    motor.turn90DegRight();
 }
 
 void loop()
 {
     if (motor.motorState == TURNING)
-        motor.turn90DegLeft();
+        motor.turn90DegRight();
 
     delay(10);
     // unsigned long startTime = millis();

diff --git a/src/services/hc12.cpp b/src/services/hc12.cpp
@@ -1,6 +1,6 @@
 #include "hc12.h"
 
-HC12::HC12(double *startAngle, double *endAngle, double *travelDis, bool *newData, int *tcount, String myID)
+HC12::HC12(double *startAngle, double *endAngle, double *travelDis, bool *newData, int *tcount, int myID)
 {
     this->startAngle = startAngle;
     this->endAngle = endAngle;
@@ -60,7 +60,7 @@ void HC12::dataDecoder(char c)
 
             if (idflag) // if id is getting
             {
-                if (id == myID)
+                if (id == String(myID))
                 {
                     LED(1);      // blue
                     good = true; // id is good

diff --git a/src/services/hc12.h b/src/services/hc12.h
@@ -7,15 +7,15 @@
 class HC12
 {
 public:
-    HC12(double *startAngle, double *endAngle, double *travelDis, bool *newData, int *tcount, String myID);
+    HC12(double *startAngle, double *endAngle, double *travelDis, bool *newData, int *tcount, int myID);
 
     void listen();
     String getReceivedData();
 
     void dataDecoder(char c);
 
 private:
-    String myID = "1";
+    int myID = 1;
 
     String reciveData = "";