File content as of revision 56:3fce0a9bb6df:

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


static int ToBinary(uint8_t n);

Robot::Robot() : dist(PB_1),  // iniitalize all of the physical ports
                 bit0(PH_1), 
                 bit1(PC_2), 
                 bit2(PC_3), 
                 ir_sensor_0(dist, bit0, bit1, bit2, 0), // one IR sendor
                 i2c2(PB_9, PB_8),  // line sensor
                 line_sensor(i2c2),
                 pwm_M1(PA_9), // mototors + 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 matri 
    robot_to_wheel = wheel_to_robot.inverse();

    robot_speed_desired.setZero(); // zero out all speed
    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 speedcontroller gains
    speedControllers[1]->setSpeedCntrlGain(0.04f);
    speedControllers[0]->setMaxAccelerationRPS(999.0f);     // adjust max. acceleration for smooth movement
    speedControllers[1]->setMaxAccelerationRPS(999.0f);
    
}

void Robot::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
    }
}

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 = ToBinary(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
{

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

    int principle_error = kP * errval;
    intergal_error = kI * (intergal_error + errval);
    int derivative_error = kD * (errval - previous_error_value);

    int total_error = (principle_error + intergal_error + derivative_error) / 10; // TODO: find out why we divide by 10

    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 vecloty
    }
    else if(total_error < 0)
    {
        // go slightly left
        robot_speed_desired(0) = TRANSLATIONAL_VELOCITY; // forward velocity
        robot_speed_desired(1) = -ROTATIONAL_VELOCITY; // rotational vecloty
    }
    else 
    {
        robot_speed_desired(0) = TRANSLATIONAL_VELOCITY; // forward velocity
        robot_speed_desired(1) = 0.0f; // rotational vecloty
    }

    // 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 ?
 // implemennt
}

static int ToBinary(std::uint8_t s)
{
    uint8_t s_binary = 0, remainder, product = 1;

    // simple binary conversion algorithm.
    while(s != 0)
    {
        remainder = s % 2;
        s_binary = s_binary + (remainder * product);
        s = s / 2;
        product *= 10;
    }

    return s_binary;
}