File content as of revision 11:bda77916d2ea:

#include "mbed.h"
#include "MODSERIAL.h"
#include "encoder.h"
#include "math.h"
#include "HIDScope.h"
#include "BiQuad.h"
#include "EMG_filter.h"

MODSERIAL pc(USBTX,USBRX);


enum r_states {R_HORIZONTAL, R_VERTICAL, R_BASE};
r_states state;

// ---- EMG parameters ----
//HIDScope scope (2); 
EMG_filter emg1(A0);
EMG_filter emg2(A1);
// ------------------------ 

// ---- Motor input and outputs ----
PwmOut speed1(D5);
PwmOut speed2(D6);
DigitalOut dir1(D4);
DigitalOut dir2(D7);
DigitalIn press(PTA4);
DigitalOut ledred(LED_RED);
DigitalOut ledgreen(LED_GREEN);
DigitalOut ledblue(LED_BLUE);
DigitalOut ledstateswitch(D8);
DigitalOut ledstatedef(D11);
AnalogIn pot(A2);
AnalogIn pot2(A3);
Encoder motor1(PTD0,PTC4);
Encoder motor2(D12,D13);
// ----------------------------------

// --- Define Ticker and Timeout ---
Ticker mainticker;          //Ticker that calls every function. Functions are called by means of a variable named go, e.g. go_EMG 
Timeout calibrationgo;      // Timeout that will determine the duration of the calibration.
// ---------------------------------


float PwmPeriod = 0.0001f;

double count = 0; //set the counts of the encoder
volatile double angle = 0;//set the angles

double count2 = 0; //set the counts of the encoder
volatile double angle2 = 0;//set the angles

double setpoint = 6.28;//I am setting it to move through 180 degrees
double setpoint2 = 6.28;//I am setting it to move through 180 degrees
double Kp = 500;// you can set these constants however you like depending on trial & error
double Ki = 100;
double Kd = 0;

float last_error = 0;
float error1 = 0;
float changeError = 0;
float totalError = 0;
float pidTerm = 0;
float pidTerm_scaled = 0;

float last_error2 = 0;
float error2 = 0;
float changeError2 = 0;
float totalError2 = 0;
float pidTerm2 = 0;
float pidTerm_scaled2 = 0;// if the total gain we get is not in the PWM range we scale it down so that it's not bigger than |255|

volatile double potvalue = 0.0;
volatile double potvalue2 = 0.0;
volatile double position = 0.0;
volatile double position2 = 0.0;

// --- Booleans for the maintickerfunction ---
//bool readoutsetpoint = true;
bool go_EMG;                    // Boolean to put on/off the EMG readout
bool go_calibration;            // Boolean to put on/off the calibration of the EMG 
bool go_switch;                  // Boolean to go to different state  
bool go_PID;                    // Boolean to calculate PID and motor aansturing
// -------------------------------------------

// --- calibrate EMG signal ----

void calibrationGO()                   // Function for go or no go of calibration
{
    go_calibration = false;
    
}

/*
Calibration of the robot works as follows:
EMG signal is read out for 5 seconds, 1000 times a second. This signal is filtered and the maximum value is compared to the current value of the MVC.
The value of the MVC is overwritten when the value of the filtered signal is bigger than the current value. 
During the calibration the user should exert maximum force.
*/
void calibrationEMG()                   // Function for calibrating the EMG signal
{
    if (go_calibration)
    {
        
        emg1.calibration();                 // Using the calibration function of the EMG_filter class  
        emg2.calibration();                             
    }
}

// ------------------------------

// --- motor movements ---
void r_movehorizontal()
{


}

void r_movevertical()
{

}

void r_movebase()
{

}
//--------------------------------

//--- State switch function -----
void r_processStateSwitch()
{
    if(go_switch) {                          //if go_switch is true state is switched
        go_switch = false;
        switch(state) {
            case R_HORIZONTAL:
                state = R_VERTICAL;
                ledblue = 1;
                ledred = 1;
                ledgreen = 0;
                pc.printf("state is vertical");
                break;
            case R_VERTICAL:
                state = R_BASE;
                ledgreen = 1;
                ledblue = 1;
                ledred = 0;
                pc.printf("state is base");
                break;
            case R_BASE:
                state = R_HORIZONTAL;
                ledgreen = 1;
                ledred = 1;
                ledblue = 0;
                pc.printf("state is horizontal");
                break;
                }
                wait(1.0f);                             // waits 1 second to continue with the main function. I think ticker does run, but main is on hold.
                ledstatedef = 1;
                ledstateswitch = 0;
        }   
}

//Function that reads out the raw EMG and filters the signal 
void processEMG ()
{
        

        emg1.filter();                      //filter the incoming emg signal
        emg2.filter();
        //emg3.filter();
        
      /*  scope.set(0, emg1.normalized);      //views emg1.normalized on port 0 of HIDScope  
        scope.set(1, emg1.emg0);        
        scope.send();*/
   
}
 
void setpointreadout()
{
    potvalue = pot.read();
    setpoint = emg1.normalized;
    
    potvalue2 = pot2.read();
    setpoint2 = emg2.normalized;
   
}
   

void PIDcalculation() // inputs: potvalue, motor#, setpoint
{
    setpointreadout();
    angle = motor1.getPosition()/4200.00;   
    angle2 = motor2.getPosition()/4200.00*6.28;
   
    //pc.printf("pos: %d, speed %f reference position = %.2f\r ",motor1.getPosition(), motor1.getSpeed(), setpoint);
    //motorpid = PID(potvalue - position, M1_KP, M1_KI, M1_KD, M1_TS, m1_err_int, m1_prev_err, m1_f_v1, m1_f_v2, M1_F_A1, M1_F_A2, M1_F_B0, M1_F_B1, M1_F_B2);

    error1 = setpoint - angle;
    error2 = setpoint2 - angle2;
    
    changeError = (error1 - last_error)/0.001f; // derivative term
    totalError += error1*0.001f; //accumalate errors to find integral term
    pidTerm = (Kp * error1) + (Ki * totalError) + (Kd * changeError);//total gain
    pidTerm = pidTerm;
    if (pidTerm >= 1000) {
        pidTerm = 1000;
    } else if (pidTerm <= -1000) {
        pidTerm = -1000;
    } else {
        pidTerm = pidTerm;
    }
    //constraining to appropriate value
        if (pidTerm >= 0) {
        dir1 = 1;// Forward motion
    } else {
        dir1 = 0;//Reverse motion
    }
    pidTerm_scaled = abs(pidTerm)/1000.0f; //make sure it's a positive value
    if(pidTerm_scaled >= 0.55f){
        pidTerm_scaled = 0.55f;
    }
    
    changeError2 = (error2 - last_error2)/0.001f; // derivative term
    totalError2 += error2*0.001f; //accumalate errors to find integral term
    pidTerm2 = (Kp * error2) + (Ki * totalError2) + (Kd * changeError2);//total gain
    pidTerm2 = pidTerm2;
    if (pidTerm2 >= 1000) {
        pidTerm2 = 1000;
    } else if (pidTerm2 <= -1000) {
        pidTerm2 = -1000;
    } else {
        pidTerm2 = pidTerm2;
    }
    //constraining to appropriate value
        if (pidTerm2 >= 0) {
        dir2 = 1;// Forward motion
    } else {
        dir2 = 0;//Reverse motion
    }
    pidTerm_scaled2 = abs(pidTerm2)/1000.0f; //make sure it's a positive value
    if(pidTerm_scaled2 >= 0.55f){
        pidTerm_scaled2 = 0.55f;
    }
        
    last_error = error1;
    speed1 = pidTerm_scaled+0.45f;
    last_error2 = error2;
    speed2 = pidTerm_scaled2+0.45f;
}

void maintickerfunction()
{    
    r_processStateSwitch();
     
    if(go_EMG)
    {
        processEMG();
    }
    /*if(emg1.normalized >=0.2 && emg2.normalized >= 0.2) // PIDcalculation should not happen. 
    { 
        go_PID = false;
    }
    else{go_PID = true;}*/
    
    if(go_PID){
    PIDcalculation(); 
    } 
}

int main()
{   
    pc.baud(9600);   
    go_EMG = true;                      // Setting ticker variables    
    go_calibration = true;              // Setting the timeout variable
    go_PID = true;
    go_switch = false;
    speed1.period(PwmPeriod);
    speed2.period(PwmPeriod);
    
    calibrationgo.attach(&calibrationGO, 5.0);     // Attach calibration timeout to calibration function
    mainticker.attach(&calibrationEMG, 0.001f);      // Attach ticker to the calibration function
    wait(5.0f);   
    mainticker.attach(&maintickerfunction,0.001f);        
    
    int count = 0;  
      
    while(true) {
       
        ledstatedef = 1;
        if(emg1.normalized >= 0.7 && emg2.normalized >= 0.7) {
            ledstateswitch = 1;
            ledstatedef = 0;
            go_switch = true;
                      
        }
        

        switch(state) {
            case R_HORIZONTAL:
                r_movehorizontal();
                break;
            case R_VERTICAL:
                r_movevertical();
                break;
            case R_BASE:
                r_movebase();
                break;
                       }

        count++;
        if(count == 100){
            count = 0;
            pc.printf("emg1 = %f, emg2 = %f\r\n", emg1.normalized, emg2.normalized);
            //pc.printf("Angle = %f, pidTerm = %f ,PID_scaled = %f, Error = %f, setpoint = %f\r\n", angle, pidTerm ,pidTerm_scaled, error1, setpoint);
            //pc.printf("Angle 2 = %f, pidTerm 2 = %f ,PID_scaled 2 = %f, Error 2 = %f, setpoint 2 = %f\r\n", angle2, pidTerm2 ,pidTerm_scaled2, error2,setpoint2);
        }
         
         
    wait(0.001f);
    }
       
            
            
        
        
    

}