File content as of revision 7:809f122886ae:

#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);
// -------------------------------
HIDScope scope(2);      // define EMG input and HIDScope output
EMG_filter emg1(A0);
//---------------------------------
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);
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.
Encoder motor1(PTD0,PTC4);
Encoder motor2(D12,D13);

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 = 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;

// ---- variables for the ticker ----
//Define ticker variables
bool go_EMG;                    // Boolean to put on/off the EMG readout
bool go_calibration;            // Boolean to put on/off the calibration of the EMG     


void setpointreadout()
{  
    potvalue = pot.read();
    setpoint = potvalue*6.28f;
    
    potvalue2 = pot2.read();
    setpoint2 = potvalue2*6.28f;   
}
/*
//Function that reads out the raw EMG and filters the signal 
void processEMG ()
{
    if (go_EMG)
    {
        //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();
    }
   
} 

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              
    }
} 
*/


void PIDcalculation() // inputs: potvalue, motor#, setpoint
{
    
    setpointreadout();
    angle = motor1.getPosition()/4200.00*6.28;   
    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()
{  
    PIDcalculation();
}

int main()
{
    mainticker.attach(&maintickerfunction,0.01f);
    speed1.period(PwmPeriod);
    speed2.period(PwmPeriod);
    
    int count = 0;    
    while(true) {
        
        count++;
        if(count == 100){
            count = 0;
            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);
    }
       
            
            
        
        
    

}