File content as of revision 10:39a97906fa4b:

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

// ---- EMG parameters ----
HIDScope scope (2); 
EMG_filter emg1(A0);
float emgthreshold = 0.20;
double MVC = 0.0;
// ------------------------ 

// ---- Motor input and outputs ----
PwmOut speed1(D5);
PwmOut speed2(D6);
DigitalOut dir1(D4);
DigitalOut dir2(D7);
DigitalIn press(PTA4);
DigitalOut led1(D8);
DigitalOut led2(D11);
AnalogIn pot(A2);
AnalogIn pot2(A1);
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
double countmax = 0; //counter to find max peak of EMG signal every second
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;//I am setting it to move through 180 degrees
double setpoint2 = 6.28;//I am setting it to move through 180 degrees
double Kp = 250;// 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   
// -------------------------------------------

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

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

/*
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)
    {
        led2 = 1;
        emg1.calibration();                 // Using the calibration function of the EMG_filter class              
    }
}

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



//Function that reads out the raw EMG and filters the signal 
void processEMG ()
{
        led1 = 1;
        
        //go_EMG = false;                   //set the variable to false --> misschien nodig?
        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();
   
}

double peakdetection ()
{
    
    if (emg1.normalized>=MVC)
    {
        MVC = emg1.normalized; 
    }
    countmax++;
    if(countmax==100 && MVC>=emgthreshold)
    {
        setpoint = MVC*6.50;                         //setpoint in translational direction (cm). EMG output (0.2-1.0) scaled to maximal translation distance (52 cm). 
        countmax = 0;
        MVC = 0;
    }
    return setpoint; 
}

/*void setpointreadout()
{
    
    potvalue = pot.read();
    potvalue2 = pot2.read();
    setpoint2 = potvalue2*6.28f;
   
}*/
 
   

void PIDcalculation() // inputs: potvalue, motor#, setpoint
{
    setpoint = peakdetection();
    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()
{     
    if(go_EMG)
    {
        processEMG();
    }
    
    PIDcalculation();    
}

int main()
{   
    go_EMG = true;                      // Setting ticker variables    
    go_calibration = true;              // Setting the timeout variable
    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) {
        
        count++;
        if(count == 100){
            count = 0;
            pc.printf("Angle = %f, pidTerm = %f ,PID_scaled = %f, Error = %f, setpoint = %f, MVC = %f\r\n", angle, pidTerm ,pidTerm_scaled, error1,setpoint,MVC);
           // 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);
    }
       
            
            
        
        
    

}