Diff: Robot_Library/robot.cpp

Revision:

56:3fce0a9bb6df

Parent:

49:7da71f479dac

Child:

57:8bf0b5a70065

--- a/Robot_Library/robot.cpp	Thu May 26 16:33:16 2022 +0200
+++ b/Robot_Library/robot.cpp	Mon May 30 08:38:03 2022 +0200
@@ -2,16 +2,20 @@
 #include "EncoderCounter.h"
 #include "PeripheralNames.h"
 #include "PinNames.h"
+#include <cstdint>
 #include <cstdio>
 
-Robot::Robot() : dist(PB_1), 
+
+static int ToBinary(uint8_t n);
+
+Robot::Robot() : dist(PB_1),  // iniitalize all of the physical ports
                  bit0(PH_1), 
                  bit1(PC_2), 
                  bit2(PC_3), 
                  ir_sensor_0(dist, bit0, bit1, bit2, 0), // one IR sendor
-                 i2c2(PB_9, PB_8), 
+                 i2c2(PB_9, PB_8),  // line sensor
                  line_sensor(i2c2),
-                 pwm_M1(PA_9),  
+                 pwm_M1(PA_9), // mototors + encoders
                  pwm_M2(PA_8),
                  encoder_M1(PA_6, PC_7),
                  encoder_M2(PB_6,PB_7)
@@ -67,10 +71,10 @@
             FollowingLine();
             break;
         case RIGHT_TURN_90:
-            RightTurn();
+            RightTurn_90();
             break;
         case LEFT_TURN_90:
-            LeftTurn();
+            LeftTurn_90();
             break;
         default: state = IDLE; // on default, stop the car
     }
@@ -98,18 +102,113 @@
     robot_speed_desired(1) = 0.0f;
 }
 
-void Robot::FollowingLine() 
+void Robot::FollowingLine() // Updates once per cycle.
 {
+    if(!controller.GetTurnedOn()) 
+    {
+        state = IDLE;
+        return;
+    }
+    
     printf("FollowingLine\n");  // TODO: REMOVE PRINT
-    robot_speed_desired(0) = TRANSLATIONAL_VELOCITY;
-    robot_speed_desired(1) = 0.0f; // set 
+    printf("%d", line_sensor.getRaw()); // print raw line sensor data
+    uint8_t binary_sensor_data = ToBinary(line_sensor.getRaw()); // convert line sensor data into binary representation of it
+     
+    // if(IsSharpTurn(binary_sensor_data)) { return; } // check if the sensor reads in any sharp turns. if so, exit the PID movement and turn sharply.
+    // first test PID movement. it is possible that PID movement works just as well.
+
+    PID_Move(binary_sensor_data); // move the robot smoothly with error calculation and stuff?
+}
+
+void Robot::RightTurn_90() 
+{
+    // count encoder values and turn until the motor has rotated ~ 90 degrees
+    // im actually not sure if we need this, try testing with just the PID system first
+}
+
+void Robot::LeftTurn_90() 
+{
+    // count encoder values and turn until the motor has rotated ~ 90 degrees
+    // im actually not sure if we need this, try testing with just the PID system first  
+}
+
+void Robot::PID_Move (std::uint8_t s_binary) // for following smooth lines ONLY
+{
+
+    int errval = 0;
+
+	if (s_binary&0b00000001)
+		errval += 3;
+	else if (s_binary&0b00000010)
+		errval += 2;
+	else if (s_binary&0b00000100)
+		errval += 1;
+	else if (s_binary&0b00001000)
+		errval += 0;
+
+	if (s_binary&0b10000000)
+		errval -= 3;
+	else if (s_binary&0b01000000)
+		errval -= 2;
+	else if (s_binary&0b00100000)
+		errval -= 1;
+	else if (s_binary&0b00010000)
+		errval -= 0;
 
-    printf("%d", line_sensor.getRaw());
+    int principle_error = kP * errval;
+    intergal_error = kI * (intergal_error + errval);
+    int derivative_error = kD * (errval - previous_error_value);
+
+    int total_error = (principle_error + intergal_error + derivative_error) / 10; // TODO: find out why we divide by 10
 
+    if(total_error > 0) 
+    { // call this every frame until it does not need it? set rotational velocity relative to the calculated error?
+        // go slightly right
+        robot_speed_desired(0) = TRANSLATIONAL_VELOCITY; // forward velocity
+        robot_speed_desired(1) = ROTATIONAL_VELOCITY; // rotational vecloty
+    }
+    else if(total_error < 0)
+    {
+        // go slightly left
+        robot_speed_desired(0) = TRANSLATIONAL_VELOCITY; // forward velocity
+        robot_speed_desired(1) = -ROTATIONAL_VELOCITY; // rotational vecloty
+    }
+    else 
+    {
+        robot_speed_desired(0) = TRANSLATIONAL_VELOCITY; // forward velocity
+        robot_speed_desired(1) = 0.0f; // rotational vecloty
+    }
 
-    // somehow make veloity relative to the angle of the line?
+    // Delay total_error/2. not exactly sure why.
+}
+
+bool Robot::IsSharpTurn(int binary_sensor_data)
+{
+    return binary_sensor_data&0b11110000 || binary_sensor_data&0b00001111;
+}
+
+void Robot::PID_Delay(int ms)
+{
+ // add in delay ?
+ // implemennt
+}
 
+static int ToBinary(std::uint8_t s)
+{
+    uint8_t s_binary = 0, remainder, product = 1;
+
+    // simple binary conversion algorithm.
+    while(s != 0)
+    {
+        remainder = s % 2;
+        s_binary = s_binary + (remainder * product);
+        s = s / 2;
+        product *= 10;
+    }
+
+    return s_binary;
 }
 
 
 
+