File content as of revision 71:350ae163ffa5:

#include "robot.h"
#include "EncoderCounter.h"
#include "PeripheralNames.h"
#include "PinNames.h"
#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_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

  // 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);

  // 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);
}

Robot::~Robot() {
  delete trajectoryPlanners[0];
  delete trajectoryPlanners[1];
  delete speedControllers[0];
  delete speedControllers[1];
}

void Robot::Update() {

  controller.Update();

  wheel_speed_actual << speedControllers[0]->getSpeedRPS(), speedControllers[1]->getSpeedRPS();
  robot_speed_actual = Cwheel2robot * 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;
  default:
    state = IDLE; // on default, stop the car
  }
  
  wheel_speed_desired = robot_to_wheel * robot_speed_desired;

  // 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();

  // command speedController objects
  speedControllers[0]->setDesiredSpeedRPS(wheel_speed_smooth(0));
  speedControllers[1]->setDesiredSpeedRPS(wheel_speed_smooth(1));
}

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;
  }
}

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.

  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::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
{

  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 proportional_term = k_prop * errval;
  float integral_term = integral_error / t_int;
  float derivative_term = t_deriv * (errval - previous_error_value) / dt;

  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;
}

void Robot::PID_Delay(int ms) {
  // add in delay ?
  // implement
}