File content as of revision 78:d53f1d68ca65:

#include "robot.h"
#include "EncoderCounter.h"
#include "PeripheralNames.h"
#include "PinNames.h"
#include <array>
#include <cstdint>
#include <cstdio>
#include <cmath>

static float ComputeAngle(std::uint8_t raw, float default_val);
static float Multiplier(float angle);

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), 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)
{
  // 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 = wheel_to_robot * wheel_speed_actual;

  switch (state) {
  case INITIAL:
    Initial();
    break;
  case IDLE:
    Idle();
    break;
  case FOLLOWING_LINE:
    FollowingLine();
    break;
  case AVOIDING_OBSTACLE:
    AvoidObstacle();
    break;
  default:
    state = IDLE; // on default, stop the car
  }

  if (!controller.GetTurnedOn()) {
    state = IDLE;
    return;
  }

  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() {
  // initialize the robot.
  enable_motors = 0;
  // 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
  {
    enable_motors = 1;
    state = FOLLOWING_LINE;
  }
}

void Robot::Idle() {
  enable_motors = 0;
  robot_speed_desired(0) = 0.0f; // set speed and rotational velocity to zero
  robot_speed_desired(1) = 0.0f;
  if (controller.GetTurnedOn()) {
    enable_motors = 1;
    state = FOLLOWING_LINE;
  }
}

void Robot::FollowingLine() // Updates once per cycle.
{
  uint8_t raw = line_sensor.getRaw();
  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
  // 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(angle); // move the robot smoothly with error
                        // calculation and stuff?
}

void Robot::AvoidObstacle() {
  // TODO
}

void Robot::PID_Move(float errval) // for following smooth lines ONLY
{
  float dt = controller.Period();

  integral_error += dt * errval;

  float integral_term = k_int * integral_error;
  float derivative_term = k_deriv * robot_speed_actual(1);

  float control_input = k_prop * errval + integral_term + derivative_term;
  float multiplier = sqrt(Multiplier(errval));

  robot_speed_desired(0) = multiplier * TRANSLATIONAL_VELOCITY;
  robot_speed_desired(1) = control_input * ROTATIONAL_VELOCITY / multiplier;
  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
}

static float ComputeAngle(std::uint8_t raw, float default_val) {
  const int line_sensor_count = 8;
  uint8_t line_sensor_bits_count;
  std::array<float, line_sensor_count> line_sensor_weight{
      3.5, 2.5, 1.5, 0.5, -0.5, -1.5, -2.5, -3.5}; // [m]
  std::array<float, line_sensor_count> line_sensor_value{};
  float line_sensor_position;                 // [m]
  const float line_sensor_distance = 12.5e-3; // [m]

  // fill bits into the sensor array
  // also count the sensors detecting a black line
  line_sensor_bits_count = 0;
  for (int i = 0; i < line_sensor_count; i++) {
    line_sensor_value[i] = (raw >> i) & 0x01;
    line_sensor_bits_count += line_sensor_value[i];
  }

  // getting the line position as a weighted mean
  if (line_sensor_bits_count == 8) {
    return default_val;
  } else if (line_sensor_bits_count > 0) {
    for (int i = 0; i < line_sensor_count; i++) {
      line_sensor_position += line_sensor_value[i] * line_sensor_weight[i] /
                              float(line_sensor_bits_count);
    }
  } else { // line_sensor_bits_count = 0
    return default_val;
  }
  line_sensor_position *= line_sensor_distance;

  float line_sensor_angle;                            // [rad]
  const float line_sensor_center_distance = 40e-3;    // [m] Set this parameter yourself

  // getting an angle out of the position
  line_sensor_angle = atan2(line_sensor_position, line_sensor_center_distance);
  return line_sensor_angle;
}

static float Multiplier(float angle)
{
  return 1 / (1 + 1.5 * angle * angle);
}