Diff: Robot_Library/robot.cpp

Revision:

76:2302f2b51e63

Parent:

74:76c7a805f63d

Child:

77:19cf9072bc22

--- a/Robot_Library/robot.cpp	Mon May 30 20:21:15 2022 +0200
+++ b/Robot_Library/robot.cpp	Tue May 31 10:44:16 2022 +0200
@@ -17,9 +17,10 @@
       i2c2(PB_9, PB_8),                       // line sensor
       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) {
+      encoder_M1(PA_6, PC_7), encoder_M2(PB_6, PB_7),
+      theta(0.0f), target_theta(0.0f) {
   // initialize all variables
-  wheel_to_robot << -WHEEL_RADIUS / 2.0f, WHEEL_RADIUS / 2.0f,
+  wheel_to_robot << WHEEL_RADIUS / 2.0f, -WHEEL_RADIUS / 2.0f,
       -WHEEL_RADIUS / DISTANCE_BETWEEN_WHEELS,
       -WHEEL_RADIUS / DISTANCE_BETWEEN_WHEELS; // transformation matrix
   robot_to_wheel = wheel_to_robot.inverse();
@@ -82,11 +83,11 @@
   case FOLLOWING_LINE:
     FollowingLine();
     break;
-  case RIGHT_TURN_90:
-    RightTurn_90();
+  case RIGHT_TURN:
+    RightTurn();
     break;
-  case LEFT_TURN_90:
-    LeftTurn_90();
+  case LEFT_TURN:
+    LeftTurn();
     break;
   case AVOIDING_OBSTACLE:
     AvoidObstacle();
@@ -95,6 +96,13 @@
     state = IDLE; // on default, stop the car
   }
 
+  if (!controller.GetTurnedOn()) {
+    state = IDLE;
+    return;
+  }
+
+  theta += controller.Period() * robot_speed_desired(1); // theta_new = theta_old + omega * dt
+
   wheel_speed_desired = robot_to_wheel * robot_speed_desired;
 
   // smooth desired wheel_speeds
@@ -136,12 +144,16 @@
 
 void Robot::FollowingLine() // Updates once per cycle.
 {
-  if (!controller.GetTurnedOn()) {
-    state = IDLE;
-    return;
-  }
-
   uint8_t raw = line_sensor.getRaw();
+  // if ((raw & 0xF0) == 0xF0) {
+  //   state = LEFT_TURN;
+  //   target_theta = theta + M_PI / 2;
+  //   return;
+  // } else if ((raw & 0xF) == 0xF) {
+  //   state = RIGHT_TURN;
+  //   target_theta = theta - M_PI / 2;
+  //   return;
+  // }
   float angle = ComputeAngle(raw, previous_error_value);
 
   // if(IsSharpTurn(binary_sensor_data)) { return; } // check if the sensor
@@ -153,16 +165,26 @@
                         // calculation and stuff?
 }
 
-void Robot::RightTurn_90() {
+void Robot::RightTurn() {
   // 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
+  if (abs(theta - target_theta) <= 0.05) {
+    state = FOLLOWING_LINE;
+  }
+  robot_speed_desired(0) = 0.0f;
+  robot_speed_desired(1) = ROTATIONAL_VELOCITY;
 }
 
-void Robot::LeftTurn_90() {
+void Robot::LeftTurn() {
   // 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
+  if (abs(theta - target_theta) <= 0.05) {
+    state = FOLLOWING_LINE;
+  }
+  robot_speed_desired(0) = 0.0f;
+  robot_speed_desired(1) = -ROTATIONAL_VELOCITY;
 }
 
 void Robot::AvoidObstacle() {
@@ -182,7 +204,7 @@
   float multiplier = sqrt(Multiplier(errval));
 
   robot_speed_desired(0) = multiplier * TRANSLATIONAL_VELOCITY;
-  robot_speed_desired(1) = control_input / multiplier;
+  robot_speed_desired(1) = control_input * ROTATIONAL_VELOCITY / multiplier;
   previous_error_value = errval;
 
   // Delay total_error/2. not exactly sure why.
@@ -222,8 +244,8 @@
       line_sensor_position += line_sensor_value[i] * line_sensor_weight[i] /
                               float(line_sensor_bits_count);
     }
-  } else {
-    line_sensor_position = 0.0;
+  } else { // line_sensor_bits_count = 0
+    return default_val;
   }
   line_sensor_position *= line_sensor_distance;
 
@@ -237,5 +259,5 @@
 
 static float Multiplier(float angle)
 {
-  return 1 / (1 + 2 * angle * angle);
+  return 1 / (1 + 1.5 * angle * angle);
 }