File content as of revision 49:7da71f479dac:

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

Robot::Robot() : dist(PB_1), 
                 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(i2c2),
                 pwm_M1(PA_9),  
                 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();
            break;
        case LEFT_TURN_90:
            LeftTurn();
            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() 
{
    printf("FollowingLine\n");  // TODO: REMOVE PRINT
    robot_speed_desired(0) = TRANSLATIONAL_VELOCITY;
    robot_speed_desired(1) = 0.0f; // set 

    printf("%d", line_sensor.getRaw());


    // somehow make veloity relative to the angle of the line?

}