File content as of revision 0:caa8ee3bd882:

#include "main.h"

// = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
// UI implementation
// = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =

void ui::StatusDisplay()
{
    switch (robot::state) {
        case robot::OFF: // continuous red;
            rgb_led[0] = LED_ON;
            rgb_led[1] = !LED_ON;
            rgb_led[2] = !LED_ON;
            break;
        case robot::CALIBRATION: // alternating red-green; busy
            rgb_led[0] = !rgb_led[0];
            rgb_led[1] = !rgb_led[0];
            rgb_led[2] = !LED_ON;
            break;
        case robot::HOMING: // alternating green-blue; busy
            rgb_led[0] = !LED_ON;
            rgb_led[1] = !rgb_led[1];
            rgb_led[2] = !rgb_led[1];
            break;
        case robot::READY: // continuous green
            rgb_led[0] = !LED_ON;
            rgb_led[1] = LED_ON;
            rgb_led[2] = !LED_ON;
            break;
        case robot::DEMO: // alternating red-blue; busy
            rgb_led[0] = !rgb_led[0];
            rgb_led[1] = !LED_ON;
            rgb_led[2] = !rgb_led[0];
            break;
        case robot::MANUAL: // alternating green-purple
            rgb_led[0] = !rgb_led[0];
            rgb_led[1] = !rgb_led[0];
            rgb_led[2] = rgb_led[0];
            break;
    }
}



void ui::SwitchState(State new_state)
{
    ui::serial.printf("* Switching Input State to: %s\r\n",StateNames[new_state]);
    ui::state = new_state;
}

void robot::SwitchState(State new_state)
{
    ui::serial.printf("* Switching Robot State to: %s\r\n",StateNames[new_state]);
    state = new_state;
}

void ui::InterruptSwitch2()
{
    if (robot::has_power()) {
        robot::Stop();
    } else {
        robot::Start();
    }
}

void ui::InterruptSwitch3()
{
    switch (state) {
        case STATE_SWITCHING:
            robot::GoToNextState();
            break;
    }
}




// = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
// Robot implementation
// = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =


void robot::GoToNextState()
{
    switch (state) {
        case OFF:
            SwitchState(CALIBRATION);
            ui::SwitchState(ui::IGNORE);
            Start();
            break;
        case CALIBRATION:
            SwitchState(HOMING);
            Start();
            break;
        case HOMING:
            SwitchState(DEMO);
            //ui::SwitchState(ui::STATE_SWITCHING);
            break;
        case READY:
            SwitchState(DEMO);
            break;
        case DEMO:
            SwitchState(MANUAL);
            break;
        default:
            SwitchState(OFF);
            robot::is_calibrated = false;
            Stop();
    }
}

bool robot::has_power()
{
    return m1.has_power() or m2.has_power();
}

double robot::get_angle(int i)
{
    switch(i) {
        case 1:
            return m1.get_angle();
        case 2:
            return m2.get_angle();
        default:
            return NULL;
    }
}

double robot::ForwardKinematicsX(double theta_1, double theta_2)
{
    return kL1*cos(deg2rad(theta_1)) + kL2*cos(deg2rad(theta_2));
}

double robot::ForwardKinematicsY(double theta_1, double theta_2)
{
    return kL1*sin(deg2rad(theta_1)) + kL2*sin(deg2rad(theta_2));
}

double robot::get_x()
{
    return ForwardKinematicsX(get_angle(1),get_angle(2));
}

double robot::get_y()
{
    return ForwardKinematicsY(get_angle(1),get_angle(2));
}

double robot::InverseKinematicsTheta(double x, double y, int i)
{
    double r = sqrt(pow(x,2) + pow(y,2));
    double dir_angle = rad2deg( atan2(y,x) );
    
    double int_angle_1 = rad2deg( acos((pow(kL1,2) + pow(r,2) - pow(kL2,2))/(2*kL1*r)) );
    double int_angle_2 = rad2deg( acos((pow(kL2,2) + pow(r,2) - pow(kL1,2))/(2*kL2*r)) );
    
    double theta_1 = dir_angle + int_angle_1;
    double theta_2 = dir_angle - int_angle_2;
    
    switch (i) {
        case 1: return theta_1;
        case 2: return theta_2;
        default: return NULL;
    }
}

void robot::Start()
{
    ui::serial.printf("  Setting current position as reference.\r\n");
    ref::SetPositionAsReference();

    ui::serial.printf("  Resetting controllers.\r\n");
    c1.Reset();
    c2.Reset();

    ui::serial.printf("  Starting motors.\r\n");
    m1.Start();
    m2.Start();
}

void robot::Stop()
{
    ui::serial.printf("  Stopping motors.\r\n");
    m1.Stop();
    m2.Stop();
}

void robot::ControlLoop()
{
    m1.set_pwm(c1.Update( ref::theta_1 - get_angle(1) ));
    m2.set_pwm(c2.Update( ref::theta_2 - get_angle(2) ));
}

void robot::SetCalibrationAngles()
{
    m1.set_angle(kCalibAngleMotor1);
    m2.set_angle(kCalibAngleMotor2);
    is_calibrated = true;
}

// = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
// Reference implementation
// = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =

double ref::get_ref_x()
{
    return robot::ForwardKinematicsX(theta_1,theta_2);
}

double ref::get_ref_y()
{
    return robot::ForwardKinematicsY(theta_1,theta_2);
}

void ref::UpdateReference()
{
    switch(robot::state) {
        case robot::CALIBRATION:
            CalibrationReference();
            break;
        case robot::HOMING:
            HomingReference();
            break;
        case robot::DEMO:
            DemoReference();
            break;
    }
}

void ref::CalibrationReference()
{
    // Calibration process
    bool is_ready_1 = (theta_1 - robot::get_angle(1) > kCalibrationError);
    bool is_ready_2 = (theta_2 - robot::get_angle(2) > kCalibrationError);

    if (is_ready_1) {
        robot::m1.Stop();
    } else {
        theta_1 += kCalibrationOmegaStep;
    }

    if (is_ready_2) {
        robot::m2.Stop();
    } else {
        theta_2 += kCalibrationOmegaStep;
    }

    if (is_ready_1 & is_ready_2) {
        robot::Stop();
        ui::serial.printf("+ Both motors at end-stop.\r\n");
        robot::SetCalibrationAngles();
        robot::GoToNextState();
    }
}

void ref::HomingReference()
{
    double dist_1 = kOriginTheta1 - theta_1;

    if (abs(dist_1)<=kCalibrationOmegaStep) {
        // motor 1 ready
        theta_1 = kOriginTheta1;
        
        double dist_2 = kOriginTheta2 - theta_2;

        if (abs(dist_2)<=kCalibrationOmegaStep) {
            // motor 2 also ready
            theta_2 = kOriginTheta2;
            
            ui::serial.printf("  Robot at home position.\r\n");
            robot::GoToNextState();
        } else {
            // motor 2 still to move
            theta_2 += dist_2 / abs(dist_2) * kCalibrationOmegaStep;
        }


    } else {
        // motor 1 still to move
        theta_1 += dist_1 / abs(dist_1) * kCalibrationOmegaStep;
    }


}

void ref::DemoReference()
{
    double x = robot::ForwardKinematicsX(theta_1,theta_2);
    double y = robot::ForwardKinematicsY(theta_1,theta_2);
    
    double x_goal = kDemoCoords[0][i_demo_coord];
    double y_goal = kDemoCoords[1][i_demo_coord];
    
    double delta_x = x_goal - x;
    double delta_y = y_goal - y;
    
    double dist = sqrt(pow(delta_x,2) + pow(delta_y,2));
    
    double x_new = x;
    double y_new = y;
    
    if (dist<kMaxStep) {
        x_new = x_goal;
        y_new = y_goal;
        
        ui::serial.printf("  Reached Demo Coord %d\r\n",i_demo_coord);
        i_demo_coord = ++i_demo_coord % kNumDemoCoords;
        
        if (i_demo_coord == 0) {
            robot::is_calibrated = false;
            robot::SwitchState(robot::CALIBRATION);
        }
    } else {
        x_new += delta_x / dist * kMaxStep;
        y_new += delta_y / dist * kMaxStep;
    }
    
    theta_1 = robot::InverseKinematicsTheta(x_new, y_new, 1);
    theta_2 = robot::InverseKinematicsTheta(x_new, y_new, 2);
    
    
}

void ref::SetPositionAsReference()
{
    ref::theta_1 = robot::get_angle(1);
    ref::theta_2 = robot::get_angle(2);
}