Diff: Robot_Library/robot.cpp

Revision:

78:d53f1d68ca65

Parent:

77:19cf9072bc22

--- a/Robot_Library/robot.cpp	Wed Jun 01 08:18:10 2022 +0200
+++ b/Robot_Library/robot.cpp	Wed Jun 01 16:18:25 2022 +0200
@@ -17,10 +17,11 @@
       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),
-      theta(0.0f), target_theta(0.0f),
-      robot_x(0.0f), target_x(0.0f),
-      robot_y(0.0f), target_y(0.0f) {
+      encoder_M1(PA_6, PC_7), encoder_M2(PB_6, PB_7)
+      // theta(0.0f), target_theta(0.0f),
+      // robot_x(0.0f), target_x(0.0f),
+      // robot_y(0.0f), target_y(0.0f)
+{
   // initialize all variables
   wheel_to_robot << WHEEL_RADIUS / 2.0f, -WHEEL_RADIUS / 2.0f,
       -WHEEL_RADIUS / DISTANCE_BETWEEN_WHEELS,
@@ -85,9 +86,6 @@
   case FOLLOWING_LINE:
     FollowingLine();
     break;
-  case TARGETING:
-    MoveToTarget();
-    break;
   case AVOIDING_OBSTACLE:
     AvoidObstacle();
     break;
@@ -100,13 +98,6 @@
     return;
   }
 
-  float dt = controller.Period();
-  float v = robot_speed_actual(0);
-  float omega = robot_speed_actual(1);
-  theta += omega * dt;
-  robot_x += cos(theta) * v * dt;
-  robot_y -= sin(theta) * v * dt; // since theta increases clockwise
-
   wheel_speed_desired = robot_to_wheel * robot_speed_desired;
 
   // smooth desired wheel_speeds
@@ -149,17 +140,10 @@
 void Robot::FollowingLine() // Updates once per cycle.
 {
   uint8_t raw = line_sensor.getRaw();
-  // if ((raw & 0xF0) == 0xF0) {
-  //   state = LEFT_TURN;
-  //   target_x = robot_x + 40e-3;
-  //   target_theta = theta + M_PI / 2;
-  //   return;
-  // } else if ((raw & 0xF) == 0xF) {
-  //   state = RIGHT_TURN;
-  //   target_x = robot_x + 40e-3;
-  //   target_theta = theta - M_PI / 2;
-  //   return;
-  // }
+  if (raw == 0xFF) {
+    raw = 0x80; // turn left at crossroads
+    // raw = 0x01; // turn right at crossroads
+  }
   float angle = ComputeAngle(raw, previous_error_value);
 
   // if(IsSharpTurn(binary_sensor_data)) { return; } // check if the sensor
@@ -171,31 +155,6 @@
                         // calculation and stuff?
 }
 
-void Robot::MoveToTarget()
-{
-  float dx = target_x - robot_x;
-  float dy = target_y - robot_y;
-  float motion_angle = -atan2(dy, dx);
-  float diff = motion_angle - theta;
-  if (abs(robot_x - target_x) <= 0.005 && abs(robot_y - target_y) <= 0.005) {
-    if (abs(theta - target_theta) <= 0.05) {
-      state = FOLLOWING_LINE;
-    } else {
-      float orient = target_theta - theta;
-      int sign = (0 < orient) - (orient < 0);
-      robot_speed_desired(0) = 0.0f;
-      robot_speed_desired(1) = sign * ROTATIONAL_VELOCITY;
-    }
-  } else if (abs(theta - motion_angle) <= 0.05) {
-    robot_speed_desired(0) = TRANSLATIONAL_VELOCITY;
-    robot_speed_desired(1) = 0.0f;
-  } else {
-    robot_speed_desired(0) = 0.0f;
-    int sign = (0 < diff) - (diff < 0);
-    robot_speed_desired(1) = sign * ROTATIONAL_VELOCITY;
-  }
-}
-
 void Robot::AvoidObstacle() {
   // TODO
 }