File content as of revision 14:d0e5894aa352:

#include "mbed.h"
#include "math.h"
#include "stdio.h"
#include "MODSERIAL.h"
#include "QEI.h"
#include "BiQuad.h"
#include "motor.h"
#include "EMG_input.h"
#include "HIDScope.h"
// Angle limits 215, 345 lower arm, 0, 145 upper arm
// Robot arm x,y limits (limit to table top)


#define M_PI 3.141592653589793238462643383279502884L // Pi

/*
** State variable
*/
enum r_states{R_INIT, R_HORIZONTAL, R_VERTICAL, R_GRIPPER};
r_states state;

/*
** Table surface and robot body limits
*/
const double R_XTABLE_LEFT = -30.0;     // Robot in the middle, table width=60
const double R_XTABLE_RIGHT = 30.0;
const double R_YTABLE_BOTTOM = -18.0;
const double R_YTABLE_TOP = 82.0;
const double R_ROBOT_LEFT = -8.0;     // Robot limited from moving onto itself (20x20 base)
const double R_ROBOT_RIGHT = 8.0;
const double R_ROBOT_BOTTOM = -18.0;
const double R_ROBOT_TOP = 2.0;

/*
** Robot arm angle limits
*/
const double ARM1_ALIMLO = (2*M_PI/360)*125;
const double ARM1_ALIMHI = -(2*M_PI/360)*125;
const double ARM2_ALIMLO = 0;
const double ARM2_ALIMHI = (2*M_PI/360)*145;

/*
** Ticker variables
*/
bool flag_switch;
bool flag_PID;
bool flag_output;
bool flag_EMG;

HIDScope scope(5);

Ticker mainticker;  // Main ticker

/*
** Motor variables
*/
motor motor1(D6, D7, D13, D12, 0.8, 0.2, 0.1, true);
motor motor2(D5, D4, D10, D11, 0.8, 0.2, 0.1, false);

/*
** Gripper variables
*/
PwmOut gripperpwm(D3);      // Gripper pwm port
bool gripperclosed=true;    // Gripper open/close flag

/*
** Setpoint variables
*/
float vx=0;     // Setpoint x speed
float vy=0;     // Setpoint y speed
float ax=0.01;  // Setpoint accelleration x direction
float ay=0.01;  // Setpoint accelleration y diraction
const double maxspeed=3.0f; // Setpoint maximum speed
bool moveleft;  // Flag set to true if setpoint moving left
bool movedown;  // Flag set to true if setpoint moving down

/*
** EMG variables
*/

EMG_input emg1(A0);
EMG_input emg2(A1);

// Struct r_link:
// Defines a robot link (arm or end effector).
struct r_link {
    float length;   // link length
    float x;        // link x position
    float y;        // link y position
    float theta;    // link angle
};
    
MODSERIAL pc(USBTX, USBRX);
DigitalIn switch1(SW3);
DigitalIn switch2(SW2);
DigitalIn switch3(D9);
bool switch3down=false;
DigitalOut ledr(LED_RED);
DigitalOut ledg(LED_GREEN);


r_link arm1;    // Robot upper arm
r_link arm2;    // Robot lower arm
r_link end;     // Robot end effector

/*
** =============================================================================
** robot_applySetpointLimits
** =============================================================================
** Function:
** Limits the cartesian coordinates to table, length of robot and prevents
** the robot arm crossing it's own base. 
** Input:
** float x, float y: position in cartesian coordinates
** Output:
** float x, float y: limited position in cartesian coordinates
** =============================================================================
*/
void robot_applySetpointLimits (float& x, float& y){
    float z;
    float angle;
    
    // Limit x,y to table top size
    if (x<R_XTABLE_LEFT) { x=R_XTABLE_LEFT; vx=0;}
    else if (x>R_XTABLE_RIGHT) { x=R_XTABLE_RIGHT; vx=0;}
    if (y<R_YTABLE_BOTTOM) { y=R_YTABLE_BOTTOM; vy=0;}
    else if (y>R_YTABLE_TOP) { y=R_YTABLE_TOP; vy=0; }
    
    // Limit x,y to maximum reach of robot arm
    z = sqrt(pow(x,2)+pow(y,2));
    angle = atan2(y,x);
    if (z>(arm1.length+arm2.length)){
        z = arm1.length+arm2.length;
        x = z*cos(angle);
        y = z*sin(angle);
    }
    
    // Prevent x,y from entering robot body
    if (end.x > R_ROBOT_LEFT && end.x < R_ROBOT_RIGHT && end.y >= R_ROBOT_TOP && y < R_ROBOT_TOP) {
        y = R_ROBOT_TOP;
    }
    if (end.x <= R_ROBOT_LEFT && x > R_ROBOT_LEFT && y < R_ROBOT_TOP) {
        x = R_ROBOT_LEFT;
    }
    if (end.x >= R_ROBOT_RIGHT && x < R_ROBOT_RIGHT && y < R_ROBOT_TOP) {
        x = R_ROBOT_RIGHT;
    }
}

/*
** =============================================================================
** robot_applyAngleLimits
** =============================================================================
** Function:
** Limits the angles of the robot's arms to prevent them from moving into 
** themselves
** Input: -
** Output: -
** =============================================================================
*/
void robot_applyAngleLimits(){
    if (arm1.theta>ARM1_ALIMLO){    // Limit angle arm1
        arm1.theta = ARM1_ALIMLO;
    }
    else if (arm1.theta<ARM1_ALIMHI){
        arm1.theta = ARM1_ALIMHI;
    }

    if (arm2.theta>ARM2_ALIMHI){    // Limit angle arm 2
        arm2.theta = ARM2_ALIMHI;
    } else if (arm2.theta < ARM2_ALIMLO) {
        arm2.theta = ARM2_ALIMLO;
    }
}

/*
** =============================================================================
** robot_setSetpoint
** =============================================================================
** Function:
** Transforms cartesian coordinates into rotadions of the joints of the robot's 
** arms.
** Input:
** float x, float y: setpoint position in cartesian coordinates
** =============================================================================
**       (x,y)
**        |\
**        | \
**        |b2\
**     z2 |   \arm2.length
**        |    \
**  z     |_ _ _\
**        |     /
**        |    /
**     z1 |   /arm1.length
**        |b1/
**        | /
**        |/
** =============================================================================
*/
void robot_setSetpoint (float x,float y){
    float z;        // length z of vector (x,y)
    float z1;       // part z1
    float z2;       // part z2

    float angle;    // angle of vector (x,y), !pointing up (x==0) the angle is defined as 0!
    float b1,b2;    // angle b1 and b2 (see drawing)
    
    robot_applySetpointLimits(x,y); // Apply limits
    
    z = sqrt(pow(x,2)+pow(y,2));    // Calculate vector (x,y) length
    
    // Calculate vector (x,y) angle
    angle = atan2(y,x);                 // Calculate (x,y) angle using atan 2
    if (angle<0.5*M_PI && angle>0){     // Rotate result of atan2 90 degrees counter clockwise, so up pointing vector (x==0, y>0) has a zero degree rotation instead of 90 degrees.

        angle = angle - 0.5*M_PI;
    }
    else if (angle<M_PI && angle>0.5*M_PI){
        angle = angle - 0.5*M_PI;
    }
    else if (angle<0 && angle>-0.5*M_PI){
        angle = angle - 0.5 * M_PI;
    }
    else{
        angle = angle + 1.5*M_PI;
    }  
    if (y==0 && x<0) { angle = 0.5*M_PI; }  // Special case correction: y=0, x < 0 Normally this returns PI, now it  must return PI/2
    if (y==0 && x>0) { angle = -0.5*M_PI; }  // Special case correction: y=0, x > 0 Normally this returns -PI, now it  must return -PI/2

    z1 = (pow(arm1.length,2)-pow(arm2.length,2)+pow(z,2))/(2*z);    // Calculate z1
    z2 = z-z1;                                                      // Calculate z2
    // Roundoff errors sometimes make z1 and z2 slightly higher than the arm lengths, this causes erratic behaviour in the acos function,
    // so it is countered here by an extra check and fix.
    if (z1>arm1.length) {z1 = arm1.length;}
    else if (z1<-arm1.length) { z1 = -arm1.length;}
    if (z2>arm2.length) {z2 = arm2.length;}
    else if (z2<-arm2.length) { z2 = -arm2.length;}
    
    b1 = acos(z1/arm1.length);      // Calculate angle b1
    b2 = acos(z2/arm2.length);      // Calculate angle b2
    
    arm1.theta = angle - b1;        // Calculate new angle arm 1
    arm2.theta = b1 + b2;           // Calculate new angle arm 2
    robot_applyAngleLimits();
        
    arm2.x = arm1.length*-sin(arm1.theta);  // Calculate new x position arm 2
    arm2.y = arm1.length*cos(arm1.theta);   // Calculate new y position arm 2
    
    end.x = arm2.length*(-sin(arm1.theta+arm2.theta)) + arm2.x; // Calculate new x position end effector
    end.y = arm2.length*(cos(arm1.theta+arm2.theta)) + arm2.y;  // Calculate new y position end effector
}

/*
** =============================================================================
** robot_init
** =============================================================================
** Function:
** Initialise robot parameters
** =============================================================================
*/
void robot_init() {
    // Set pwm motor periods
 //   gripperpwm.period_ms(20);
    
    pc.baud(115200);    // Set serial communication speed

    // Initialise robot segments
    state = R_HORIZONTAL;
    // Init arm1 (upper arm) first link
    arm1.length = 28.0f;
    arm1.x = 0;
    arm1.y = 0;
    arm1.theta = 0;

    // Init arm2 (lower arm) second link
    arm2.length = 35.0f;
    arm2.x = 0; 
    arm2.y = arm1.length;
    arm2.theta = 0;

    // Init end (end effector) third/last link
    end.length = 5.0f;
    end.x = 0; 
    end.y = arm1.length+arm2.length;
    end.theta = 0;    
}

/*
** =============================================================================
** r_moveHorizontal
** =============================================================================
** Function:
** Moves setpoint left or right depending on which switch is active.
** =============================================================================
*/
void r_moveHorizontal(){
    if (flag_switch){
        if (!switch1.read() || emg1.read()){
            moveleft = true;
            vx = (vx<-maxspeed)?-maxspeed:(vx-ax);
        }
        else {
            vx = moveleft?0:vx;
        }
        if (!switch2.read() || emg2.read()){
            moveleft = false;
            vx = (vx>maxspeed)?maxspeed:(vx+ax);
        }
        else {
            vx = !moveleft?0:vx;
        }
        robot_setSetpoint(end.x+vx, end.y);
    }
}

/*
** =============================================================================
** r_moveVertical
** =============================================================================
** Function: 
** Moves setpoint up or down depending on which switch is active.
** =============================================================================
*/
void r_moveVertical(){
    if (flag_switch){
        if (!switch1.read() || emg1.read()){
            movedown = true;
            vy = (vy<-maxspeed)?-maxspeed:(vy-ay);
        }
        else {
            vy = movedown?0:vy;
        }
        if (!switch2.read() || emg2.read()){
            movedown = false;
            vy = (vy>maxspeed)?maxspeed:(vy+ay);
        }
        else {
            vy = !movedown?0:vy;
        }
        robot_setSetpoint(end.x, end.y+vy);
    }
}

/*
** =============================================================================
** r_moveGripper
** =============================================================================
** Function: 
** Opens and closes the gripper depending on which switch is active.
** =============================================================================
*/
void r_moveGripper(){
    if(flag_switch){
        if((!switch1.read() || emg1.read()) && !gripperclosed){
            gripperpwm.pulsewidth_us(1035);     // Close gripper
            gripperclosed = true;
        } else if((!switch2.read() || emg2.read()) && gripperclosed){
            gripperpwm.pulsewidth_us(1500);     // Open gripper
            gripperclosed = false;
        }
    }
}

/*
** =============================================================================
** r_processStateSwitch
** =============================================================================
** Function: 
** Processes presses of the state switch.
** This switch switches between control states of the robot.
** States are horizontal movement, vertical movement and gripper open/close.
** =============================================================================
*/
void r_processStateSwitch(){
    if(flag_switch){
        if(switch3.read()){
            switch3down = false;
        }
        else {
            if (switch3down==false){
                switch3down = true;
                switch(state){
                    case R_HORIZONTAL:
                        state = R_VERTICAL;
                        break;
                    case R_VERTICAL:
                        state = R_GRIPPER;
                        break;
                    case R_GRIPPER:
                        state = R_HORIZONTAL;
                        break;
                    
                }
            }
        }
    }    
}

/*
** =============================================================================
** r_switchFlagReset
** =============================================================================
** Function:
** Resets the switch ticker flag.
** Call near the end of the main loop, after all switches have been read.
** =============================================================================
*/
void r_switchFlagReset(){
    if (flag_switch){
        flag_switch = false;
    }
}


/*
** =============================================================================
** r_doPID
** =============================================================================
** Function: 
** Runs the PID controller using the setpoint and sets the motorspeeds.
** =============================================================================
*/
void r_doPID(){
    double dt = 1/50.0; // PID time step

    if (flag_PID){
        flag_PID = false;
        motor1.PID(arm1.theta, dt);
        motor2.PID(arm2.theta, dt);
    }     
}

/*
** =============================================================================
** r_outputToMatlab
** =============================================================================
** Function: 
** Outputs data to the serial interface to be read in matlab (or putty).
** =============================================================================
*/
void r_outputToMatlab(){
    if (flag_output){
        flag_output = false;    
        printf("Angle1\n\r");
        printf("%f\n\r", arm1.theta);
        printf("Angle2\n\r");
        printf("%f\n\r", arm2.theta);
        printf("Pos1\n\r");
        printf("%f\n\r", motor1.getPosition());
        printf("Pos2\n\r");
        printf("%f\n\r", motor2.getPosition());
        
        printf("x: %f | y: %f\n\r", end.x, end.y);
    }
}

/*
** =============================================================================
** r_processEMG
** =============================================================================
** Function: 
** Processes the EMG signals at ticker frequency
** =============================================================================
*/
void r_processEMG(){
    if(flag_EMG){
        flag_EMG = false;
        emg1.tick();
        emg2.tick();
        scope.set(0, emg1.discrete);
        scope.set(1, emg1.read()?0.5:0);
        scope.set(2, emg2.discrete);
        scope.set(3, emg2.read()?0.5:0);

    }
}
/*
** =============================================================================
** maintickerfunction
** =============================================================================
** Function: 
** Ticker call back. Sets flags at appropriate timer intervals for
** reading switches, calculating PID, outputting information via serial and
** reading EMG signals.
** =============================================================================
*/
void maintickerfunction (){
    static int cnt=0;
    if (cnt%20 == 0){   // Read and react to switches 50 times per second
        flag_switch = true;
    }
    if (cnt%20 == 0){   // Run PID controller 50 times per second
        flag_PID = true;
    }
    if (cnt%100 == 0){  // Output data to serial bus 10 times per second
        flag_output = true;
    }
    flag_EMG = true;
    cnt++;
}

/*
** =============================================================================
** main
** =============================================================================
** Function:
** Contains the main loop with state machine.
** =============================================================================
*/
int main(){
    // Initialise main ticker
    mainticker.attach(&maintickerfunction,0.001f);
    robot_init();
    while(true){
        unsigned int t1 = us_ticker_read();
        switch(state){
            case R_INIT:
                break;
            case R_HORIZONTAL:
                r_moveHorizontal();
                break;
            case R_VERTICAL:
                r_moveVertical();
                break;
            case R_GRIPPER:
                r_moveGripper();
                break;
        }
        r_processEMG();

        if (flag_switch){
            scope.send();

        }
        r_processStateSwitch();
        r_switchFlagReset();
        r_doPID();
        t1 = us_ticker_read()-t1;
        scope.set(4, (float)t1);
        r_outputToMatlab();
    }
}