File content as of revision 7:b9a209f889f5:

#include "robot.h"
#include "ui.h"
#include "ref.h"
#include "tools.h"


/**
 * Robot implementation
 */
namespace robot
{
Motor m1(M1_PWM, M1_DIR, M1_ENC_A, M1_ENC_B, GEAR_RATIO*COUNTS_PER_REV);
Motor m2(M2_PWM, M2_DIR, M2_ENC_A, M2_ENC_B, GEAR_RATIO*COUNTS_PER_REV, true);

Controller c1(.500,.200,0.070,kSampleTime);
Controller c2(.400,.200,0.050,kSampleTime);

State state = OFF;
bool is_calibrated = false;

/**
 * Switching ROBOT state to a new state
 */
void 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 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);
            is_calibrated = false;
            Stop();
    }
}

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

/**
 * Get motor angle in [deg] for motor i=1,2
 */
double 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 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 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 get_x()
{
    return ForwardKinematicsX(get_angle(1),get_angle(2));
}

/**
 * Compute end-effector y-coordinate [m] for current motor angles.
 */
double 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 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 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 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 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 SetCalibrationAngles()
{
    m1.set_angle(kCalibAngleMotor1);
    m2.set_angle(kCalibAngleMotor2);
    is_calibrated = true;
}

} // end namespace robot