File content as of revision 17:e5d9a543157b:

#include "mbed.h"
#include "MODSERIAL.h"
#include "QEI.h"
#include "FastPWM.h"
#include "math.h"
//#include "HIDScope.h"
#include "BiQuad.h"
#include "BiQuad4.h"
#include "FilterDesign.h"
#include "FilterDesign2.h"

const double  pi = 3.14159265359;
// LED's
DigitalOut led_red(LED_RED);
DigitalOut led_green(LED_GREEN);
DigitalOut led_blue(LED_BLUE);
// Buttons
DigitalIn button_clbrt_home(SW2);
DigitalIn button_Demo(D5);
DigitalIn button_Emg(D6);
DigitalIn Fail_button(SW3);
// Modserial
MODSERIAL pc(USBTX, USBRX);
// Encoders
QEI Encoder1(D11, D10, NC, 4200) ;  // Encoder motor 1, (pin 1A, pin 1B, index pin(not used), counts/rev)
QEI Encoder2(D9, D8, NC, 4200) ;    // Encoder motor 2, (pin 2A, pin 2B, index pin (not used), counts/rev)
// Motors (direction and PWM)
DigitalOut directionM1(D4);
DigitalOut directionM2(D7);
FastPWM motor1_pwm(D5);
FastPWM motor2_pwm(D6);
// Inverse Kinematics
volatile double q1_diff;
volatile double q2_diff;
double sq = 2.0; //to square numbers
const double L1 = 250.0; //length of the first link
const double L3 = 350.0; //length of the second link
int track;
const double x0 = 80.0; //zero x position after homing
const double y0 = 141.0; //zero y position after homing
volatile double  setpointx = x0;
volatile double  setpointy = y0;
volatile double  U1;
volatile double  U2;
// Reference angles of the starting position
double q2_0 = pi + acos((pow(x0,sq)+pow(y0,sq)-pow(L1,sq)-pow(L3,sq))/(2.0*L1*L3));
double q1_0 = atan(y0/x0)+acos((-pow(L3,sq)+pow(L1,sq)+pow(x0,sq)+pow(y0,sq))/(2.0*L1*sqrt(pow(x0,sq)+pow(y0,sq))));
double q2_0_enc = q2_0 + q1_0;
// EMG input en start value of filtered EMG
AnalogIn    emg1_raw( A0 );
AnalogIn    emg2_raw( A1 );
double emg1_filtered = 0.00;
double emg2_filtered = 0.00;
float threshold_EMG = 0.25;         // Threshold on 25 percent of the maximum EMG

// Declare timers and Tickers
Timer       timer;                        // Timer for counting time in this state
Ticker      WriteValues;            // Ticker to show values of velocity to screen
Ticker      StateMachine;
//Ticker      sample_EMGtoHIDscope;   // Ticker to send the EMG signals to screen
//HIDScope    scope(4);               //Number of channels which needs to be send to the HIDScope

// Set up ProcessStateMachine
enum states {WAITING, MOTOR_ANGLE_CLBRT, EMG_CLBRT, HOMING, WAITING4SIGNAL, MOVE_W_EMG, MOVE_W_DEMO, FAILURE_MODE};
states currentState = WAITING;
bool stateChanged = true;
volatile bool writeVelocityFlag = false;

// Global variables
char c;
int counts1; 
int counts2;
float theta1;
float theta2;
float vel_1;
float vel_2;
float theta1_prev = 0.0;
float theta2_prev = 0.0;
float tijd = 0.005;
float time_in_state;

int need_to_move_1;                     // Does the robot needs to move in the first direction?
int need_to_move_2;                     // Does the robot needs to move in the second direction?
double EMG_calibrated_max_1 = 2.00000;  // Maximum value of the first EMG signal found in the calibration state.
double EMG_calibrated_max_2 = 2.00000;  // Maximum value of the second EMG signal found in the calibration state.

// ----------------------------------------------
// ------- FUNCTIONS ----------------------------
// ----------------------------------------------

float ReadEncoder1()            // Read Encoder of motor 1.
{
    counts1 = Encoder1.getPulses();              // Counts of outputshaft of motor 1
    theta1 = (float(counts1)/4200) * 2*pi;       // Angle of outputshaft of motor 1
    vel_1 = (theta1 - theta1_prev) / tijd;       // Velocity, current angle - previous angle, devided by avarage time between encoder read-outs
    theta1_prev = theta1;                        // Define theta_prev
    return vel_1;
}
float ReadEncoder2()                // Read encoder of motor 2.
{
    counts2 = Encoder2.getPulses();              // Counts of outputshaft of motor 2
    theta2 = (float(counts2)/4200) * 2*pi;       // Angle of outputshaft of motor 2
    vel_2 = (theta2 - theta2_prev) / tijd;       // Velocity, current angle - previous angle, devided by avarage time between encoder read-outs
    theta2_prev = theta2;                        // Define theta_prev
    return vel_2;
}
void MotorAngleCalibrate()                  // Function that drives motor 1 and 2.
{
        float U1 = -0.2;            // Negative, so arm goes backwards.
        float U2 = -0.2;             // Motor 2 is not taken into account yet.
        
        motor1_pwm.write(fabs(U1));         // Send PWM values to motor
        motor2_pwm.write(fabs(U2));
        
        directionM1 = U1 > 0.0f;            // Either true or false, determines direction.
        directionM2 = U2 > 0.0f;
} 
void sample()
{
    emg1_filtered = FilterDesign(emg1_raw.read());
    emg2_filtered = FilterDesign2(emg2_raw.read());
    
    /**
    scope.set(0, emg1_raw.read());      // Raw EMG 1 send to scope 0
    scope.set(1, emg1_filtered);        // Filtered EMG 1 send to scope 1
    scope.set(2, emg2_raw.read());      // Raw EMG 2 send to scope 2
    scope.set(3, emg2_filtered);        // Filtered EMG 2 send to scope 3
    scope.send();                       // Send the data to the computer
    */
}

// ---------------------------------------------------
// --------INVERSE-KINEMATICS-------------------------
// --------------------------------------------------- 
double makeAngleq1(double x, double y){
    double q1 = atan(y/x)+acos((-pow(L3,sq)+pow(L1,sq)+pow(x,sq)+pow(y,sq))/(2.0*L1*sqrt(pow(x,sq)+pow(y,sq)))); //angle of the first joint in the setpoint configuration
    q1_diff = -2.0*(q1-q1_0);                                                                                    //the actual amount of radians that the motor has to turn in total to reach the setpoint
    return q1_diff;
}

double makeAngleq2(double x, double y){
    double q2 = -acos((pow(x,sq)+pow(y,sq)-pow(L1,sq)-pow(L3,sq))/(2.0*L1*L3));                                  //angle of the second joint in setpoint configuration
    double q1 = atan(y/x)+acos((-pow(L3,sq)+pow(L1,sq)+pow(x,sq)+pow(y,sq))/(2.0*L1*sqrt(pow(x,sq)+pow(y,sq)))); //angle of the first joint in the setpoint configuration
    double q2_motor = (pi - q2)+q1;                                                                              //because q2 represents the angle at joint two and not at the motor a calculation has to be done
    q2_diff = (2.0*(q2_motor - q2_0_enc))/(2.0*pi);                                                              //the actual amount of radians that the motor has to turn in total to reach the setpoint
    return -q2_diff;
}

// --------------------------------------------------------------------
// ---------------READ-OUT ENCODERS------------------------------------
// --------------------------------------------------------------------    
double  counts2angle1()
{
    counts1 = Encoder1.getPulses();         // Counts of outputshaft of motor 1
    theta1 = -(double(counts1)/4200) * 2*pi;       // Angle of outputshaft of motor 1
    return theta1;
}

double counts2angle2()
{
    counts2 = Encoder2.getPulses();         // Counts of outputshaft of motor 2
    theta2 = (double(counts2)/4200) * 2*pi;       // Angle of outputshaft of motor 2
    return theta2;
}

// -----------------------------------------------------------------    
// --------------------------- PI controllers ----------------------
// -----------------------------------------------------------------
double PI_controller1(double error1)
{
    static double error_integral1 = 0;
    
    // Proportional part
    double Kp1 = 3.95;              // Kp (proportionele controller, nu nog een random waarde)
    double u_p1 = Kp1*error1;        // Voltage dat naar de motor gestuurd wordt (volgt uit error en Kp)
    
    // Integral part
    double Ki1 = 6.0;             // Ki (Integrale deel vd controller, nu nog een random waarde)
    double Ts1 = 0.005;           // Sample tijd, net zo vaak als de controller wordt aangeroepen (200 Hz, statemachine)   
    error_integral1 = error_integral1 + error1 * Ts1;  
    double u_i1 = Ki1 * error_integral1;
    
    // Sum 
    U1 = u_p1 + u_i1;
    
    // Return
    return U1;      
}
double PI_controller2(double error2)
{
    static double error_integral2 = 0;
    
    // Proportional part
    double Kp2 = 3.95;              // Kp (proportionele controller, nu nog een random waarde)
    double u_p2 = Kp2*error2;        // Voltage dat naar de motor gestuurd wordt (volgt uit error en Kp)
    
    // Integral part
    double Ki2 = 6.0;             // Ki (Integrale deel vd controller, nu nog een random waarde)
    double Ts2 = 0.005;           // Sample tijd, net zo vaak als de controller wordt aangeroepen (200 Hz, statemachine)   
    error_integral2 = error_integral2 + error2 * Ts2;  
    double u_i2 = Ki2 * error_integral2;
    
    // Sum +
    U2 = u_p2 + u_i2;
    
    // Return
    return U2;      
} 

// -----------------------------------------------
// ------------ RUN MOTORS -----------------------
// -----------------------------------------------
void motoraansturing()
{
    determinedemoset();
    q1_diff = makeAngleq1(setpointx, setpointy);
    q2_diff = makeAngleq2(setpointx, setpointy);

    theta2 = counts2angle2();           
    error2 = q2_diff - theta2;
    theta1 = counts2angle1();  
    error1 = q1_diff - theta1;          // Setpoint error, te behalen setpoint minus de huidige positie van de as.

    U1 = PI_controller1(error1);       // Voltage dat naar de motor gestuurd wordt. 
    U2 = PI_controller2(error2);   
    
    motor1_pwm.write(fabs(U1));         // Motor aansturen
    directionM1 = U1 > 0.0f;            // Richting van de motor bepalen
    motor2_pwm.write(fabs(U2));
    directionM2 = U2 > 0.0f; 
}

// ---------------------------------------------------
// --------STATEMACHINE------------------------------- 
// --------------------------------------------------- 
void ProcessStateMachine(void)
{
    switch (currentState)
    {
        case WAITING:
            // Description:
            // In this state we do nothing, and wait for a command
            
            // Actions        
            led_red = 0; led_green = 0; led_blue = 0;   // Colouring the led WHITE
            
            // State transition logic
            if (button_clbrt_home == 0) 
            {
                currentState = MOTOR_ANGLE_CLBRT;
                stateChanged = true;
                pc.printf("Starting Calibration\n\r");
            }
            else if (Fail_button == 0)
            {
                currentState = FAILURE_MODE;
                stateChanged = true;
            }
            break;
                
        case MOTOR_ANGLE_CLBRT:
            // Description:
            // In this state the robot moves with low motor PWM to some 
            // mechanical limit of motion, in order to calibrate the motors.
             
            // Actions       
            led_red = 1; led_green = 0; led_blue = 0;   // Colouring the led TURQUOISE
            timer.start();                              //Start timer to get time in the state "MOTOR_ANGLE_CLRBRT" 
            if (stateChanged)
                {
                    MotorAngleCalibrate();                      // Actuate motor 1 and 2.
                    vel_1 = ReadEncoder1();                     // Get velocity of motor 1    
                    vel_2 = ReadEncoder2();                     // Get velocity of motor 2
                    stateChanged = true;                        // Keep this loop going, until the transition conditions are satisfied.    
                }
        
            // State transition logic
            time_in_state = timer.read();                       // Determine if this state has run for long enough.
        
            if(time_in_state > 2.0f && vel_1 < 1.1f && vel_2 < 1.1f)
                {
                    //pc.printf( "Tijd in deze staat = %f \n\r", time_in_state);           
                    //pc.printf( "Tijd tijdens actions loop (Waarde voor bepalen van snelheid)") = %f \n\r", tijd);
                    pc.printf("Motor calibration has ended. \n\r");
                    timer.stop();                              // Stop timer for this state
                    timer.reset();                             // Reset timer for this state
                    motor1_pwm.write(fabs(0.0));               // Send PWM values to motor
                    motor2_pwm.write(fabs(0.0));
                    Encoder1.reset();                          // Reset Encoders when arrived at zero-position
                    Encoder2.reset();
            
                    currentState = EMG_CLBRT;                   // Switch to next state (EMG_CLRBRT).
                    pc.printf("EMG calibration \r\n");
                    stateChanged = true;                       
               }
            if (Fail_button == 0)
            {
                currentState = FAILURE_MODE;
                stateChanged = true;
            }
            break; 
                        
        case EMG_CLBRT:
            // In this state the person whom is connected to the robot needs 
            // to flex his/her muscles as hard as possible, in order to 
            // measure the maximum EMG-signal, which can be used to scale 
            // the EMG-filter.
                    
             led_red = 1; led_green = 1; led_blue = 0;   // Colouring the led BLUE
                    
             // Requirements to move to the next state:
             // If enough time has passed (5 sec), and the EMG-signal drops below 10%
             // of the maximum measured value, we move to the Homing state.
                        
             wait(5.0f); // time_in_this_state > 5.0f
             // INSERT CALIBRATING
             currentState = HOMING;
            if (Fail_button == 0)
            {
                currentState = FAILURE_MODE;
                stateChanged = true;
            }
             break;       
                            
        case HOMING:
            // Description:
            // Robot moves to the home starting configuration
            pc.printf("HOMING \r\n");
                        
            led_red = 0; led_green = 1; led_red = 0;                  // Colouring the led PURPLE
                    
            // Requirements to move to the next state:
            // If we are in the right location, within some margin, we move to the Waiting for
            // signal state.
                    
            wait(5.0f); // time_in_this_state > 5.0f
            // INSERT MOVEMENT
            currentState = WAITING4SIGNAL;
            if (Fail_button == 0)
            {
                currentState = FAILURE_MODE;
                stateChanged = true;
            }
            break;
                
        case WAITING4SIGNAL:
            // Description:
            // In this state the robot waits for an action to occur.
            
            led_red = 0; led_green = 0; led_blue = 0;                // Colouring the led WHITE
                    
            // Requirements to move to the next state:
            // If a certain button is pressed we move to the corresponding 
            // state (MOVE_W_DEMO, MOVE_W_EMG or SHUTDOWN)
                            
            if (button_clbrt_home == 0) 
            {
                currentState = MOTOR_ANGLE_CLBRT;
                stateChanged = true;
                pc.printf("Starting Calibration \n\r");
            }
            else if (button_Demo == 1) 
            {
                currentState = MOVE_W_DEMO;
                pc.printf("DEMO \r\n");
                wait(1.0f);
            }
            else if (button_Emg == 1) 
            {
                currentState = MOVE_W_EMG;
                pc.printf("EMG \r\n");
                wait(1.0f);
            }
            else if (Fail_button == 0)
            {
                currentState = FAILURE_MODE;
                stateChanged = true;
            }
            
            break;
                
        case MOVE_W_DEMO:
        // Description:
        // In this state the robot follows a preprogrammed shape, e.g. 
        // a square.
                    
        led_red = 1; led_green = 1; led_blue = 0;   // Colouring the led GREEN
                
        // Requirements to move to the next state:
        // When the home button or the failure button is pressed, we 
        // will the move to the corresponding state.
                
        // BUILD DEMO MODE
        
        if (button_clbrt_home == 0) 
        {
            currentState = HOMING;
            stateChanged = true;
            pc.printf("Moving home\n\r");
        }
        else if (Fail_button == 0)
        {
            currentState = FAILURE_MODE;
            stateChanged = true;
        }
        break;
                
        case MOVE_W_EMG:
            // Description:
            // In this state the robot will be controlled by use of 
            // EMG-signals. 
                        
            led_red = 1; led_green = 0; led_blue = 1;   // Colouring the led GREEN
                 
            if (emg1_filtered >= (threshold_EMG*EMG_calibrated_max_1)){
                need_to_move_1 = 1; // The robot does have to move
                }
            else {
                need_to_move_1 = 0; // If the robot does not have to move
                }
    
            if(emg2_filtered >= threshold_EMG*EMG_calibrated_max_2){
                need_to_move_2 = 1;
                }
            else {
                need_to_move_2 = 0;
                }   
            
            
            // Requirements to move to the next state:
            // When the home button or the failure button is pressed, we 
            // will the move to the corresponding state.
            
            if (button_clbrt_home == 0) 
            {
                currentState = MOTOR_ANGLE_CLBRT;
                stateChanged = true;
                pc.printf("Starting Calibration \n\r");
            }        
            else if (Fail_button == 0)
            {
                currentState = FAILURE_MODE;
                stateChanged = true;
            }
            break;      
                 
        case FAILURE_MODE:
            // Description:
            // This state is reached when the failure button is reached. 
            // In this state everything is turned off.
                
            led_red = 0; led_green = 1; led_blue = 1;   // Colouring the led RED
            // Actions
            if (stateChanged)
            {
                motor1_pwm.write(fabs(0.0));               // Stop all motors!
                motor2_pwm.write(fabs(0.0));
                pc.printf("FAILURE MODE \r\n PLEASE RESTART THE WHOLE ROBOT \r\n (and make sure this does not happen again) \r\n");
                stateChanged = false;
            }    
            break;
        
        // State transition logic
        // No state transition, you need to restart the robot.
                        
        default: 
        // This state is a default state, this state is reached when 
        // the program somehow defies all of the other states. 
                
        pc.printf("Unknown or unimplemented state reached!!! \n\r");
        led_red = 1; led_green = 1; led_blue = 1;   // Colouring the led BLACK
        for (int n = 0; n < 50; n++)                // Making an SOS signal with the RED led
        {              
            for (int i = 0; i < 6; i++) 
            { 
                led_red = !led_red;
                wait(0.6f);
            }
            wait(0.4f);
            for (int i = 0 ; i < 6; i++) 
            {
            led_red = !led_red;
            wait(0.2f);
            }
            wait(0.4f);
        }
    }
}
            
 // --------------------------------  
 // ----- MAIN LOOP ----------------
 // --------------------------------  
    
int main()
{
    // Switch all LEDs off
    led_red = 1;
    led_green = 1;
    led_blue = 1;
    
    pc.baud(115200);
    
    pc.printf("\r\n _______________ INSERT ROBOT NAME HERE! _______________ \r\n");
    wait(0.5f);
    pc.printf("WAITING... \r\n");
    
    StateMachine.attach(&ProcessStateMachine, 0.005f);   // Run statemachine 200 times per second
    sample_EMGtoHIDscope.attach(&sample, 0.02f);         // Display EMG values 50 times per second
    
    while (true) {
        
    }
}