Diff: Robot_Library/robot.cpp

Revision:

77:19cf9072bc22

Parent:

76:2302f2b51e63

Child:

78:d53f1d68ca65

--- a/Robot_Library/robot.cpp	Tue May 31 10:44:16 2022 +0200
+++ b/Robot_Library/robot.cpp	Wed Jun 01 08:18:10 2022 +0200
@@ -18,7 +18,9 @@
       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) {
+      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,
@@ -83,11 +85,8 @@
   case FOLLOWING_LINE:
     FollowingLine();
     break;
-  case RIGHT_TURN:
-    RightTurn();
-    break;
-  case LEFT_TURN:
-    LeftTurn();
+  case TARGETING:
+    MoveToTarget();
     break;
   case AVOIDING_OBSTACLE:
     AvoidObstacle();
@@ -101,7 +100,12 @@
     return;
   }
 
-  theta += controller.Period() * robot_speed_desired(1); // theta_new = theta_old + omega * dt
+  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;
 
@@ -147,10 +151,12 @@
   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;
   // }
@@ -165,26 +171,29 @@
                         // calculation and stuff?
 }
 
-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;
+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;
   }
-  robot_speed_desired(0) = 0.0f;
-  robot_speed_desired(1) = ROTATIONAL_VELOCITY;
-}
-
-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() {