Merge pull request #12 from Pera-Swarm/8-turning-the-bot-90-degree

8 - turning the bot 90 degree

src/components/gyro.cpp

@@ -1,9 +1,8 @@
 #include "gyro.h"
 
-Gyro::Gyro(int MPU, float *angle, float *GyroErrorX)
+Gyro::Gyro(int MPU, float *angle)
 {
     this->MPU = MPU;
-    this->GyroErrorXP = GyroErrorX;
     this->angleP = angle;
 }
 
@@ -18,6 +17,7 @@ void Gyro::updateGyro()
     float AvgGyroZ = 0;
     for (int i = 0; i < AVGINTER; i++)
     {
+
         Wire.beginTransmission(MPU);
         Wire.write(0x43); // Gyro data first register address 0x43
         Wire.endTransmission(false);
@@ -32,7 +32,7 @@ void Gyro::updateGyro()
     // to get the avg value
     GyroZ = AvgGyroZ / AVGINTER;
 
-    GyroZ = GyroZ - *GyroErrorXP; // GyroErrorX; // GyroErrorX ~(-0.56)
+    GyroZ = GyroZ - GyroErrorXP; // GyroErrorX; // GyroErrorX ~(-0.56)
 
     // deg/s * s = deg
     *angleP = *angleP + GyroZ * elapsedTime;
@@ -55,6 +55,7 @@ void Gyro::calculate_IMU_error()
 
     // initialize c to 0
     int c = 0;
+    GyroErrorXP = 0;
     // Read gyro values 200 times
     while (c < ERRORINTER)
     {
@@ -67,13 +68,12 @@ void Gyro::calculate_IMU_error()
         GyroZ = Wire.read() << 8 | Wire.read();
 
         // Sum all readings
-        *GyroErrorXP = *GyroErrorXP + (GyroZ / 32.75);
+        GyroErrorXP = GyroErrorXP + (GyroZ / 32.75);
         c++;
-
     }
 
     // Divide the sum by ERRORINTER to get the error value
-    *GyroErrorXP = *GyroErrorXP / ERRORINTER;
+    GyroErrorXP = GyroErrorXP / ERRORINTER;
 
     // Print the error values on the Serial Monitor
     //  Serial.print("GyroErrorZ: ");
@@ -82,5 +82,11 @@ void Gyro::calculate_IMU_error()
 
 float Gyro::getAngle()
 {
+    // return radToDegree(*angleP);
     return *angleP;
 }
+
+double radToDegree(double rads)
+{
+    return (float)(rads * 180 / PI);
+}

src/components/gyro.h

@@ -4,7 +4,7 @@
 #include <Arduino.h>
 #include <Wire.h>
 
-// define the number of iterations 
+// define the number of iterations
 #define AVGINTER 10
 #define ERRORINTER 500
 
@@ -14,20 +14,21 @@ class Gyro
 {
 
 public:
-    Gyro(int MPU, float *angle, float *GyroErrorX);
+    Gyro(int MPU, float *angle);
 
-    void updateGyro();  // update the angle
+    void updateGyro();          // update the angle
     void calculate_IMU_error(); // calculate the error in yaw
-    float getAngle();   // return the angle
+    float getAngle();           // return the angle
 
 private:
     int MPU;
-    float *GyroErrorXP; // Gyro error
-    float *angleP;      // Gyro angle
+    float GyroErrorXP = 0; // Gyro error
+    float *angleP;         // Gyro angle
     float GyroX, GyroY, GyroZ;
     float elapsedTime, currentTime, previousTime; // time stamps for gyro calculaions
     int c = 0;                                    // temp
-
 };
 
+double radToDegree(double rads);
+
 #endif

src/components/motors.cpp

@@ -10,10 +10,10 @@ void Motor::setup_motors()
     pid.SetSampleTime(pid_const.SET_TIME_FORWARD); // refresh rate of the PID
     pid.SetMode(AUTOMATIC);
 
-    // set the EEPROM
-    setPIDConstToEEPROM(0.5, 0.005, 0.005, 0.02, 0.005, 0.005, 20);
+    // set the eerpom
+    setPIDConstToEEPROM(0.5, 0.005, 0.005, 0.02, 0.005, 0.005, 20, 70, 70, 90);
 
-    // update the PID values from EEPROM
+    // update the pid values from eeprom
     getPIDConstFromEEPROM();
 
     pinMode(EN_R, OUTPUT);
@@ -47,7 +47,7 @@ void Motor::MR(int16_t val)
 void Motor::motorWrite(int16_t leftSpeed, int16_t rightSpeed)
 {
 
-    if (leftSpeed == rightSpeed)
+    if ((leftSpeed == rightSpeed) && (leftSpeed != 0))
     {
         tunning(leftSpeed, rightSpeed);
         updateOutput();
@@ -58,25 +58,25 @@ void Motor::motorWrite(int16_t leftSpeed, int16_t rightSpeed)
     }
     else
     {
-        goingStraight = false;
+        motorState = RANDOM;
     }
 
     ML(leftSpeed);
     MR(rightSpeed);
 }
 
+// update the output variable
 // update the angle and use it with PID
 void Motor::updateOutput()
 {
 
-    if (!goingStraight)
+    if (motorState != GOING_STRAIGHT)
     {
-        gyro.updateGyro();
-        setPoint = (double)gyro.getAngle();
-        goingStraight = true;
+        updateSetPoint();
+        motorState = GOING_STRAIGHT;
     }
     else
-        goingStraight = true;
+        motorState = GOING_STRAIGHT;
 
     gyro.updateGyro();
     input = (double)gyro.getAngle();
@@ -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);
 }
@@ -117,6 +120,24 @@ bool Motor::getPIDConstFromEEPROM()
     return EEPROM_read_struct(ADDRESS, pid_const);
 }
 
+void Motor::updateSetPoint()
+{
+    gyro.updateGyro();
+    setPoint = (double)gyro.getAngle();
+}
+
+void Motor::setState(int state)
+{
+    this->motorState = state;
+}
+
+void Motor::stop()
+{
+    ML(0);
+    MR(0);
+    setState(STOP);
+}
+
 void pulse(int pulsetime, int time)
 {
     digitalWrite(ML_A1, HIGH);
@@ -134,4 +155,66 @@ void pulse(int pulsetime, int time)
         digitalWrite(EN_R, LOW);
         delayMicroseconds(pulsetime * 9 / 10);
     }
-}
+}
+
+void Motor::turn90DegLeft()
+{
+    if (motorState != TURNING)
+    {
+        setPoint = setPoint + pid_const.ANGLE_90;
+        motorState = TURNING;
+    }
+
+    if (motorState == TURNING)
+    {
+        gyro.updateGyro();
+        double startangle = (double)gyro.getAngle();
+        double err = setPoint - startangle;
+
+        if ((err > 1.0) || (err < -1.0))
+        {
+            input = startangle;
+            tunning(pid_const.TURING_SPEED, pid_const.TURING_SPEED);
+            pid.Compute();
+
+            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
+        {
+            motorState = STOP;
+            stop();
+        }
+    }
+}
+
+void Motor::turn90DegRight()
+{
+    if (motorState != TURNING)
+    {
+        setPoint = setPoint - pid_const.ANGLE_90;
+        motorState = TURNING;
+    }
+
+    if (motorState == TURNING)
+    {
+        gyro.updateGyro();
+        double startangle = (double)gyro.getAngle();
+        double err = startangle - setPoint;
+
+        if ((err > 1.0) || (err < -1.0))
+        {
+            input = startangle;
+            tunning(pid_const.TURING_SPEED, pid_const.TURING_SPEED);
+            pid.Compute();
+
+            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
+        {
+            motorState = STOP;
+            stop();
+        }
+    }
+}

src/components/motors.h

@@ -4,7 +4,13 @@
 #include "define.h"
 
 // starting address of EEPROM
-#define ADDRESS 0
+#define ADDRESS 1
+
+// states of the bot
+#define STOP 0
+#define GOING_STRAIGHT 1
+#define TURNING 2
+#define RANDOM 3
 
 // constants for PID
 struct PID_CONST
@@ -19,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
@@ -36,13 +46,21 @@ 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 getPIDConstFromEEPROM();                                                                                                                    // get the PID const from 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
+    void setState(int state);
+
+    void stop();
+    void turn90DegRight(); // turn 90 degrees to the right
+    void turn90DegLeft();  // turn 90 degrees to the right
+
+    int motorState = STOP;
 
 private:
     PID_CONST pid_const; // values instance
 
-    bool goingStraight = false; // check if the bot is going staight forward
     double setPoint, input, output;
 
     PID pid = PID(&input, &output, &setPoint, pid_const.KP_FOWARD, pid_const.KI_FOWARD, pid_const.KD_FOWARD, DIRECT); // PID instance for the motors

src/define.h

@@ -12,10 +12,11 @@
 #include <PID_v1.h>
 
 // Implementations
+#include "components/leds.h"
 #include "functions/utility.h"
 #include "components/motors.h"
-#include "components/leds.h"
 #include "functions/eeprom.h"
 #include "components/gyro.h"
+#include "services/hc12.h"
 
 extern Gyro gyro;

src/functions/eeprom.cpp

@@ -42,4 +42,4 @@ bool EEPROM_address_check(uint16_t address)
 
 // Explicit instantiation for the types you expect to use
 template bool EEPROM_read_struct(uint16_t address, PID_CONST &t);
-template bool EEPROM_write_struct(uint16_t address, const PID_CONST &t);
+template bool EEPROM_write_struct(uint16_t address, const PID_CONST &t);

src/functions/eeprom.h

@@ -12,4 +12,4 @@ template <typename T>
 bool EEPROM_write_struct(uint16_t address, const T &t);
 
 template <typename T>
-bool EEPROM_read_struct(uint16_t address, T &t);
+bool EEPROM_read_struct(uint16_t address, T &t);

src/functions/utility.cpp

@@ -1,10 +1,5 @@
 #include "utility.h"
 
-double radToDegree(double rads)
-{
-    return (float)(rads * 180 / PI);
-}
-
 void intShow()
 {
     LED(COLOR_BLUE);

src/functions/utility.h

@@ -2,6 +2,4 @@
 
 #include "define.h"
 
-double radToDegree(double rads);
-
 void intShow();