Diff: Robot_Library/robot.cpp

Revision:

60:85a40e69ced6

Parent:

59:f2d7ef0db12b

Child:

61:9b1fc2bc7172

Child:

67:90f378806cbe

--- a/Robot_Library/robot.cpp	Mon May 30 09:10:01 2022 +0200
+++ b/Robot_Library/robot.cpp	Mon May 30 09:34:12 2022 +0200
@@ -5,196 +5,185 @@
 #include <cstdint>
 #include <cstdio>
 
+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), pwm_M1(PA_9),        // motors + encoders
+      pwm_M2(PA_8), 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,
+      -WHEEL_RADIUS / DISTANCE_BETWEEN_WHEELS,
+      -WHEEL_RADIUS / DISTANCE_BETWEEN_WHEELS; // transformation matrix
+  robot_to_wheel = wheel_to_robot.inverse();
 
-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),
-                 pwm_M1(PA_9), // motors + encoders
-                 pwm_M2(PA_8),
-                 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,
-                      -WHEEL_RADIUS / DISTANCE_BETWEEN_WHEELS, -WHEEL_RADIUS / DISTANCE_BETWEEN_WHEELS; // transformation matrix
-    robot_to_wheel = wheel_to_robot.inverse();
+  robot_speed_desired.setZero(); // zero out all speeds
+  wheel_speed_desired.setZero();
+  wheel_speed_smooth.setZero();
+  robot_speed_actual.setZero();
+  wheel_speed_actual.setZero();
+
+  // MOTORS + MOTION
 
-    robot_speed_desired.setZero(); // zero out all speeds
-    wheel_speed_desired.setZero();
-    wheel_speed_smooth.setZero();
-    robot_speed_actual.setZero();
-    wheel_speed_actual.setZero();
+  // TRAJECTORY PLANNERS
+  trajectoryPlanners[0] = new Motion();
+  trajectoryPlanners[1] = new Motion();
 
-    // MOTORS + MOTION 
-
-    // TRAJECTORY PLANNERS
-    trajectoryPlanners[0] = new Motion();
-    trajectoryPlanners[1] = new Motion();
+  trajectoryPlanners[0]->setProfileVelocity(MAX_MOTOR_VOLTAGE * KN / 60.0f);
+  trajectoryPlanners[1]->setProfileVelocity(MAX_MOTOR_VOLTAGE * KN / 60.0f);
+  trajectoryPlanners[0]->setProfileAcceleration(10.0f);
+  trajectoryPlanners[1]->setProfileAcceleration(10.0f);
+  trajectoryPlanners[0]->setProfileDeceleration(10.0f);
+  trajectoryPlanners[1]->setProfileDeceleration(10.0f);
 
-    trajectoryPlanners[0]->setProfileVelocity(MAX_MOTOR_VOLTAGE * KN / 60.0f);
-    trajectoryPlanners[1]->setProfileVelocity(MAX_MOTOR_VOLTAGE * KN / 60.0f);
-    trajectoryPlanners[0]->setProfileAcceleration(10.0f);
-    trajectoryPlanners[1]->setProfileAcceleration(10.0f);
-    trajectoryPlanners[0]->setProfileDeceleration(10.0f);
-    trajectoryPlanners[1]->setProfileDeceleration(10.0f);
+  // SPEED CONTROLLERS
+  speedControllers[0] = new SpeedController(
+      COUNTS_PER_TURN, KN, MAX_MOTOR_VOLTAGE, pwm_M1, encoder_M1);
+  speedControllers[1] = new SpeedController(
+      COUNTS_PER_TURN, KN, MAX_MOTOR_VOLTAGE, pwm_M2, encoder_M2);
 
-    // SPEED CONTROLLERS
-    speedControllers[0] = new SpeedController(COUNTS_PER_TURN, KN, MAX_MOTOR_VOLTAGE, pwm_M1, encoder_M1);
-    speedControllers[1] = new SpeedController(COUNTS_PER_TURN, KN, MAX_MOTOR_VOLTAGE, pwm_M2, encoder_M2);
-
-    speedControllers[0]->setSpeedCntrlGain(0.04f);          // adjust speed controller gains
-    speedControllers[1]->setSpeedCntrlGain(0.04f);
-    speedControllers[0]->setMaxAccelerationRPS(999.0f);     // adjust max. acceleration for smooth movement
-    speedControllers[1]->setMaxAccelerationRPS(999.0f);
-    
+  speedControllers[0]->setSpeedCntrlGain(
+      0.04f); // adjust speed controller gains
+  speedControllers[1]->setSpeedCntrlGain(0.04f);
+  speedControllers[0]->setMaxAccelerationRPS(
+      999.0f); // adjust max. acceleration for smooth movement
+  speedControllers[1]->setMaxAccelerationRPS(999.0f);
 }
 
-Robot::~Robot()
-{
-    delete trajectoryPlanners[0];
-    delete trajectoryPlanners[1];
-    delete speedControllers[0];
-    delete speedControllers[1];
+Robot::~Robot() {
+  delete trajectoryPlanners[0];
+  delete trajectoryPlanners[1];
+  delete speedControllers[0];
+  delete speedControllers[1];
 }
 
 void Robot::Update() {
 
-    controller.Update();
+  controller.Update();
 
-    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;
-        default: state = IDLE; // on default, stop the car
-    }
+  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;
+  default:
+    state = IDLE; // on default, stop the car
+  }
 }
 
-void Robot::Initial() 
-{
-    printf("Initial State\n");  // TODO: REMOVE PRINT 
-    // initialize the robot.
-    // enable_motors = 1;
-    // motors_enabled = false;
-    robot_speed_desired(0) = 0.0f; // set speed and rotational velocity to zero
-    robot_speed_desired(1) = 0.0f;
+void Robot::Initial() {
+  printf("Initial State\n"); // TODO: REMOVE PRINT
+  // initialize the robot.
+  // enable_motors = 1;
+  // motors_enabled = false;
+  robot_speed_desired(0) = 0.0f; // set speed and rotational velocity to zero
+  robot_speed_desired(1) = 0.0f;
 
-    if(controller.GetTurnedOn()) // check to see if blue button is toggled
-    {
-        state = FOLLOWING_LINE;
-    }
+  if (controller.GetTurnedOn()) // check to see if blue button is toggled
+  {
+    state = FOLLOWING_LINE;
+  }
 }
 
-void Robot::Idle() 
-{
-    printf("Idle\n"); // TODO: REMOVE PRINT
-    robot_speed_desired(0) = 0.0f; // set speed and rotational velocity to zero
-    robot_speed_desired(1) = 0.0f;
+void Robot::Idle() {
+  printf("Idle\n");              // TODO: REMOVE PRINT
+  robot_speed_desired(0) = 0.0f; // set speed and rotational velocity to zero
+  robot_speed_desired(1) = 0.0f;
 }
 
 void Robot::FollowingLine() // Updates once per cycle.
 {
-    if(!controller.GetTurnedOn()) 
-    {
-        state = IDLE;
-        return;
-    }
-    
-    printf("FollowingLine\n");  // TODO: REMOVE PRINT
-    printf("%d", 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
-     
-    // 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.
+  if (!controller.GetTurnedOn()) {
+    state = IDLE;
+    return;
+  }
 
-    PID_Move(binary_sensor_data); // move the robot smoothly with error calculation and stuff?
+  printf("FollowingLine\n");          // TODO: REMOVE PRINT
+  printf("%d", 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
+
+  // 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::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::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
+void Robot::PID_Move(std::uint8_t s_binary) // for following smooth lines ONLY
 {
 
-    int errval = 0;
+  float 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;
+  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];
 
-    int principle_error = kP * errval;
-    integral_error = kI * (integral_error + errval);
-    int derivative_error = kD * (errval - previous_error_value);
+  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];
 
-    int total_error = (principle_error + integral_error + derivative_error) / 10; // TODO: find out why we divide by 10
+  float dt = controller.Period();
+
+  integral_error += dt * errval;
 
-    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 velocity
-    }
-    else if(total_error < 0)
-    {
-        // go slightly left
-        robot_speed_desired(0) = TRANSLATIONAL_VELOCITY; // forward velocity
-        robot_speed_desired(1) = -ROTATIONAL_VELOCITY; // rotational velocity
-    }
-    else 
-    {
-        robot_speed_desired(0) = TRANSLATIONAL_VELOCITY; // forward velocity
-        robot_speed_desired(1) = 0.0f; // rotational velocity
-    }
+  float proportional_term = k_prop * errval;
+  float integral_term = integral_error / t_int;
+  float derivative_term = t_deriv * (errval - previous_error_value) / dt;
 
-    // Delay total_error/2. not exactly sure why.
+  float control_input =
+    (proportional_term + integral_term + derivative_term) / error_scale;
+
+  robot_speed_desired(0) = TRANSLATIONAL_VELOCITY;
+  robot_speed_desired(1) = ROTATIONAL_VELOCITY * control_input;
+  previous_error_value = errval;
+
+  // Delay total_error/2. not exactly sure why.
 }
 
-bool Robot::IsSharpTurn(int binary_sensor_data)
-{
-    return binary_sensor_data&0b11110000 || binary_sensor_data&0b00001111;
+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 ?
- // implement
+void Robot::PID_Delay(int ms) {
+  // add in delay ?
+  // implement
 }