changing function heading

src/components/motors.cpp

@@ -63,7 +63,7 @@ void Motor::motorWrite(int16_t leftSpeed, int16_t rightSpeed)
 
 // update the output variable 
 // update the angle and use it with pid
-double Motor::updateOutput(){
+void Motor::updateOutput(){
 
     if (!goingStraight)
     {

src/components/motors.h

@@ -3,38 +3,37 @@
 
 #include "define.h"
 
-// define the const for the pid 
-#define KP_FOWARD 0.5 // 0.6
+// define the const for the pid
+#define KP_FOWARD 0.5   // 0.6
 #define KI_FOWARD 0.005 // 0.01
 #define KD_FOWARD 0.005 // 0.01
 // define the rates of kp, ki and kd
 #define KP_RATE 0.02
-#define KI_RATE 0.005 
-#define KD_RATE 0.005 
+#define KI_RATE 0.005
+#define KD_RATE 0.005
 
 #define SET_TIME_FOWARD 20
 
 // motor class that contains basic functionality for the motors
 class Motor
 {
-    public:
-        Motor();
+public:
+    Motor();
 
-        void setup_motors();    // initalize the left and right motor
-        void ML(int16_t val);   // write to left motor
-        void MR(int16_t val);   // write to right motor
+    void setup_motors();  // initalize the left and right motor
+    void ML(int16_t val); // write to left motor
+    void MR(int16_t val); // write to right motor
 
-        void motorWrite(int16_t leftSpeed, int16_t rightSpeed);   // write to left motor and right motor
+    void motorWrite(int16_t leftSpeed, int16_t rightSpeed); // write to left motor and right motor
 
-        double 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
+    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
 
-    private:
-        bool goingStraight = false; // check if the bot is going staight forward
-        double setPoint, input, output;
+private:
+    bool goingStraight = false; // check if the bot is going staight forward
+    double setPoint, input, output;
 
-        PID pid = PID(&input, &output, &setPoint, KP_FOWARD, KI_FOWARD, KD_FOWARD, DIRECT);     // pid instance for the motors
-
+    PID pid = PID(&input, &output, &setPoint, KP_FOWARD, KI_FOWARD, KD_FOWARD, DIRECT); // pid instance for the motors
 };
 
 void pulse(int pulsetime, int time);

src/main.cpp

@@ -213,9 +213,18 @@ void setup()
 void loop()
 {
     // Serial.println("Loop");
-    motor.motorWrite(80, 80);
-    delay(5000);
-    motor.motorWrite(-80,-80);
-    delay(5000);
+    unsigned long startTime = millis();
+    while ((millis() - startTime) < 3000)
+    {
+        motor.motorWrite(80, 80);
+        delay(10);
+    }
+
+    startTime = millis();
 
+    while ((millis() - startTime) < 3000)
+    {
+        motor.motorWrite(-80, -80);
+        delay(10);
+    }
 }