Diff: Robot_Library/robot.cpp

Branch:

distance-sensor

Revision:

73:667d568da72a

Parent:

72:9325748d2d02

Child:

74:76c7a805f63d

--- a/Robot_Library/robot.cpp	Mon May 30 15:08:21 2022 +0200
+++ b/Robot_Library/robot.cpp	Mon May 30 17:02:27 2022 +0200
@@ -2,17 +2,20 @@
 #include "EncoderCounter.h"
 #include "PeripheralNames.h"
 #include "PinNames.h"
+#include <array>
 #include <cstdint>
 #include <cstdio>
+#include <cmath>
+
+static float ComputeAngle(std::uint8_t raw);
 
 Robot::Robot()
     : dist(PB_1), // initialize all of the physical ports
       bit0(PH_1), bit1(PC_2), bit2(PC_3),
       ir_sensor_0(dist, bit0, bit1, bit2, 0), // one IR sensor
       i2c2(PB_9, PB_8),                       // line sensor
-      line_sensor(i2c2),
-      enable_motors(PB_15),
-      pwm_M1(PA_8), pwm_M2(PA_9),       // motors + encoders
+      line_sensor(i2c2), enable_motors(PB_15), pwm_M1(PB_13),
+      pwm_M2(PA_9), // motors + encoders
       encoder_M1(PA_6, PC_7), encoder_M2(PB_6, PB_7) {
   // initialize all variables
   wheel_to_robot << -WHEEL_RADIUS / 2.0f, WHEEL_RADIUS / 2.0f,
@@ -62,43 +65,51 @@
 
 void Robot::Update() {
 
+  printf("\n\n===UPDATE===\n");
+
   controller.Update();
-  
-  wheel_speed_actual << speedControllers[0]->getSpeedRPS(), speedControllers[1]->getSpeedRPS();
+
+  wheel_speed_actual << speedControllers[0]->getSpeedRPS(),
+      speedControllers[1]->getSpeedRPS();
   robot_speed_actual = wheel_to_robot * wheel_speed_actual;
 
-  printf("STATE: %d \r\n", state);
   switch (state) {
-    case INITIAL:
-      Initial();
-      break;
-    case IDLE:
-      Idle();
-      break;
-    case FOLLOWING_LINE:
-      FollowingLine();
-      break;
-    case RIGHT_TURN_90:
-      RightTurn_90();
-      break;
-    case LEFT_TURN_90:
-      LeftTurn_90();
-      break;
-    case AVOIDING_OBSTACLE:
-      AvoidObstacle();
-      break;
-    default:
-      state = IDLE; // on default, stop the car
+  case INITIAL:
+    Initial();
+    break;
+  case IDLE:
+    Idle();
+    break;
+  case FOLLOWING_LINE:
+    FollowingLine();
+    break;
+  case RIGHT_TURN_90:
+    RightTurn_90();
+    break;
+  case LEFT_TURN_90:
+    LeftTurn_90();
+    break;
+  case AVOIDING_OBSTACLE:
+    AvoidObstacle();
+    break;
+  default:
+    state = IDLE; // on default, stop the car
   }
-  
-  printf("robot speed: [%.2f, %.2f]\n", robot_speed_desired(0), robot_speed_desired(1));
+
+  printf("state: %d\n", state);
+  printf("robot speed: [%.2f, %.2f]\n", robot_speed_desired(0),
+         robot_speed_desired(1));
   wheel_speed_desired = robot_to_wheel * robot_speed_desired;
-  printf("wheel speed: [%.2f, %.2f]\n", wheel_speed_desired(0), wheel_speed_desired(1));
+  printf("wheel speed: [%.2f, %.2f]\n", wheel_speed_desired(0),
+         wheel_speed_desired(1));
 
   // smooth desired wheel_speeds
-  trajectoryPlanners[0]->incrementToVelocity(wheel_speed_desired(0) / (2.0f * M_PI), controller.Period());
-  trajectoryPlanners[1]->incrementToVelocity(wheel_speed_desired(1) / (2.0f * M_PI), controller.Period());
-  wheel_speed_smooth << trajectoryPlanners[0]->getVelocity(), trajectoryPlanners[1]->getVelocity();
+  trajectoryPlanners[0]->incrementToVelocity(
+      wheel_speed_desired(0) / (2.0f * M_PI), controller.Period());
+  trajectoryPlanners[1]->incrementToVelocity(
+      wheel_speed_desired(1) / (2.0f * M_PI), controller.Period());
+  wheel_speed_smooth << trajectoryPlanners[0]->getVelocity(),
+      trajectoryPlanners[1]->getVelocity();
 
   // command speedController objects
   speedControllers[0]->setDesiredSpeedRPS(wheel_speed_smooth(0));
@@ -106,7 +117,6 @@
 }
 
 void Robot::Initial() {
-  printf("Initial State\n"); // TODO: REMOVE PRINT
   // initialize the robot.
   enable_motors = 0;
   // motors_enabled = false;
@@ -121,12 +131,10 @@
 }
 
 void Robot::Idle() {
-  printf("Idle\n");              // TODO: REMOVE PRINT
   enable_motors = 0;
   robot_speed_desired(0) = 0.0f; // set speed and rotational velocity to zero
   robot_speed_desired(1) = 0.0f;
-  if (controller.GetTurnedOn())
-  {
+  if (controller.GetTurnedOn()) {
     enable_motors = 1;
     state = FOLLOWING_LINE;
   }
@@ -139,20 +147,16 @@
     return;
   }
 
-  printf("FollowingLine\n");          // TODO: REMOVE PRINT
-  printf("enable_motors: %d\n", enable_motors.read());
-  printf("%d\n", line_sensor.getRaw()); // print raw line sensor data
-  uint8_t binary_sensor_data =
-      line_sensor.getRaw(); // convert line sensor data into binary
-                            // representation of it
+  uint8_t raw = line_sensor.getRaw();
+  float angle = ComputeAngle(raw);
 
   // 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?
+  PID_Move(angle); // move the robot smoothly with error
+                        // calculation and stuff?
 }
 
 void Robot::RightTurn_90() {
@@ -167,45 +171,23 @@
   // first
 }
 
-void Robot::AvoidObstacle()
-{
+void Robot::AvoidObstacle() {
   // TODO
 }
 
-void Robot::PID_Move(std::uint8_t s_binary) // for following smooth lines ONLY
+void Robot::PID_Move(float errval) // for following smooth lines ONLY
 {
-
-  float errval = 0;
-
-  if (s_binary & 0b00000001)
-    errval += error_angles[3];
-  else if (s_binary & 0b00000010)
-    errval += error_angles[2];
-  else if (s_binary & 0b00000100)
-    errval += error_angles[1];
-  else if (s_binary & 0b00001000)
-    errval += error_angles[0];
-
-  if (s_binary & 0b10000000)
-    errval -= error_angles[3];
-  else if (s_binary & 0b01000000)
-    errval -= error_angles[2];
-  else if (s_binary & 0b00100000)
-    errval -= error_angles[1];
-  else if (s_binary & 0b00010000)
-    errval -= error_angles[0];
-
   float dt = controller.Period();
 
   integral_error += dt * errval;
 
-  float integral_term = integral_error / t_int;
-  float derivative_term = t_deriv * (errval - previous_error_value) / dt;
+  float integral_term = k_int * integral_error;
+  float derivative_term = k_deriv * (errval - previous_error_value) / dt;
 
-  float control_input = k_prop * (errval + integral_term + derivative_term);
+  float control_input = k_prop * errval + integral_term + derivative_term;
 
   robot_speed_desired(0) = TRANSLATIONAL_VELOCITY;
-  robot_speed_desired(1) = ROTATIONAL_VELOCITY * control_input;
+  robot_speed_desired(1) = control_input;
   previous_error_value = errval;
 
   // Delay total_error/2. not exactly sure why.
@@ -219,3 +201,40 @@
   // add in delay ?
   // implement
 }
+
+static float ComputeAngle(std::uint8_t raw) {
+  const int line_sensor_count = 8;
+  uint8_t line_sensor_bits_count;
+  std::array<float, line_sensor_count> line_sensor_weight{
+      3.5, 2.5, 1.5, 0.5, -0.5, -1.5, -2.5, -3.5}; // [m]
+  std::array<float, line_sensor_count> line_sensor_value{};
+  float line_sensor_position;                 // [m]
+  const float line_sensor_distance = 12.5e-3; // [m]
+
+  // fill bits into the sensor array
+  // also count the sensors detecting a black line
+  line_sensor_bits_count = 0;
+  for (int i = 0; i < line_sensor_count; i++) {
+    line_sensor_value[i] = (raw >> i) & 0x01;
+    line_sensor_bits_count += line_sensor_value[i];
+  }
+
+  // getting the line position as a weighted mean
+  line_sensor_position = 0.0;
+  for (int i = 0; i < line_sensor_count; i++) {
+    if (line_sensor_bits_count > 0) {
+      line_sensor_position += line_sensor_value[i] * line_sensor_weight[i] /
+                              float(line_sensor_bits_count);
+    } else {
+      line_sensor_position = 0.0;
+    }
+  }
+  line_sensor_position *= line_sensor_distance;
+
+  float line_sensor_angle;                            // [rad]
+  const float line_sensor_center_distance = 40e-3;    // [m] Set this parameter yourself
+
+  // getting an angle out of the position
+  line_sensor_angle = atan2(line_sensor_position, line_sensor_center_distance);
+  return line_sensor_angle;
+}