File content as of revision 2:df0c6af898ac:

#include "main.h"

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

/**
 * Display robot state using (flashing) LEDs
 */
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;
    }
}


/**
 * Switching UI state to a new state
 */
void ui::SwitchState(State new_state)
{
    ui::serial.printf("* Switching INPUT State to: %s\r\n",StateNames[new_state]);
    ui::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
// = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =

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

/**
 * State flow // MANUAL state not implemented
 */
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;
        default:
            SwitchState(OFF);
            robot::is_calibrated = false;
            Stop();
    }
}

/**
 * Check whether any of the motors is running
 */
bool robot::has_power()
{
    return m1.has_power() or m2.has_power();
}

/**
 * Get motor angle in [deg] for motor i=1,2
 */
double robot::get_angle(int i)
{
    switch(i) {
        case 1:
            return m1.get_angle();
        case 2:
            return m2.get_angle();
        default:
            return NULL;
    }
}

/**
 * Compute end-effector x-coordinate [m] for given motor angles [deg].
 */
double robot::ForwardKinematicsX(double theta_1, double theta_2)
{
    return kL1*cos(deg2rad(theta_1)) + kL2*cos(deg2rad(theta_2));
}

/**
 * Compute end-effector y-coordinate [m] for given motor angles [deg].
 */
double robot::ForwardKinematicsY(double theta_1, double theta_2)
{
    return kL1*sin(deg2rad(theta_1)) + kL2*sin(deg2rad(theta_2));
}

/**
 * Compute end-effector x-coordinate [m] for current motor angles.
 */
double robot::get_x()
{
    return ForwardKinematicsX(get_angle(1),get_angle(2));
}

/**
 * Compute end-effector y-coordinate [m] for current motor angles.
 */
double robot::get_y()
{
    return ForwardKinematicsY(get_angle(1),get_angle(2));
}

/**
 * Compute motor angle [deg] for motor i=1,2 for given end-effector x and y
 * coordinate [m]
 */
double robot::InverseKinematicsTheta(double x, double y, int i)
{
    // vector to end point
    double r = sqrt(pow(x,2) + pow(y,2));
    double dir_angle = rad2deg( atan2(y,x) );

    // law of cosines for internal angles
    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)) );

    // absolute motor angles w.r.t x,y frame
    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;
    }
}

/**
 * Start robot:
 * - set reference position to current position s.t. no 'explosion'
 * - start motors
 * - reset controller
 */
void robot::Start()
{
    ui::serial.printf("! Starting robot.\r\n");
    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 robot.\r\n");
    ui::serial.printf("  - Stopping motors.\r\n");
    m1.Stop();
    m2.Stop();
}

/**
 * Control loop; negative feedback control on error with reference postion
 */
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) ));
}

/**
 * Set the motor angles to the values corresponding to the end-stops.
 * Only call this method when the robot is at its end-stops!
 */
void robot::SetCalibrationAngles()
{
    m1.set_angle(kCalibAngleMotor1);
    m2.set_angle(kCalibAngleMotor2);
    is_calibrated = true;
}

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

/**
 * Get reference x-coordinate using reference angles and forward kinematics
 */
double ref::get_ref_x()
{
    return robot::ForwardKinematicsX(theta_1,theta_2);
}

/**
 * Get reference x-coordinate using reference angles and forward kinematics
 */
double ref::get_ref_y()
{
    return robot::ForwardKinematicsY(theta_1,theta_2);
}

/**
 * Update reference signal depending on robot state
 */
void ref::UpdateReference()
{
    switch(robot::state) {
        case robot::CALIBRATION:
            CalibrationReference();
            break;
        case robot::HOMING:
            HomingReference();
            break;
        case robot::DEMO:
            DemoReference();
            break;
    }
}

/**
 * Generate reference profile to bring robotto calibration position;
 * at start-up the motor angles are '0' (relative encoders)
 */
void ref::CalibrationReference()
{
    // Check when motors can no longer move farther
    bool is_ready_1 = (theta_1 - robot::get_angle(1) > kCalibrationError);
    bool is_ready_2 = (theta_2 - robot::get_angle(2) > kCalibrationError);

    // Turn off motor if end-stop reached
    // Motor 2 _should_ arrive first
    if (is_ready_1) {
        robot::m1.Stop();
    } else {
        theta_1 += kCalibrationOmegaStep;
    }

    // Turn off motor if end-stop reached
    if (is_ready_2) {
        robot::m2.Stop();
    } else {
        theta_2 += kCalibrationOmegaStep;
    }

    if (is_ready_1 & is_ready_2) {
        ui::serial.printf("  - Both motors at end-stop.\r\n");
        robot::Stop();
        // Set the calibration angles as current position
        robot::SetCalibrationAngles();
        robot::GoToNextState();
    }
}

/**
 * Bring robot to home position: STARTING FROM CALIBRATION POSITION!!
 */
void ref::HomingReference()
{
    // First move motor 1
    double dist_1 = kOriginTheta1 - theta_1;

    if (abs(dist_1)<=kCalibrationOmegaStep) {
        // Motor 1 ready
        theta_1 = kOriginTheta1;

        // Only move motor 2 after motor 1 at home
        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;
    }
}

/**
 * Cycle through demo coordinates to display straight lines
 */
void ref::DemoReference()
{
    // Compare current setpoint and goal coord and slowly move linearly in the
    // right direction (straight line 2D)

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

    // Compute next setpoint (x,y)
    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;
    }

    // Compute motor angles
    theta_1 = robot::InverseKinematicsTheta(x_new, y_new, 1);
    theta_2 = robot::InverseKinematicsTheta(x_new, y_new, 2);
}

/**
 * Set current robot position as reference position.
 */
void ref::SetPositionAsReference()
{
    ref::theta_1 = robot::get_angle(1);
    ref::theta_2 = robot::get_angle(2);
}