File content as of revision 4:8b1df22779a7:

#include "mbed.h"
#include "MODSERIAL.h"
#include "QEI.h"

#include "math.h"
#include "HIDScope.h"
#include "Biquad.h"
 

///////////////////////////
////////Constantes/////////
///////////////////////////

double const pi = 3.14159265359;

///////////////////////////
// Initialise hardware/////
///////////////////////////

MODSERIAL pc(USBTX, USBRX);
//-----------------------------------------------------------
//--------------------All lights-----------------------------
//-----------------------------------------------------------
DigitalOut green(LED_GREEN);
DigitalOut red(LED_RED);
//------------------------------------------------------------
//--------------------All sensors-----------------------------
//------------------------------------------------------------

//--------------------Encoders--------------------------------

//Variables
   volatile int movement = 0,direction_L =0, direction_R =0;
    volatile double Signal = 0.0;


const int SampleFrequency = 100;
const double AnglePerPulse = 2*pi/4200; //Soms doet dit een beetje vaag, dus dit moet wel af en toe uitgetest worden
volatile double Position_L = 0.0; //The position of the left motor in radiants
volatile double Position_R = 0.0; //The position of the right motor in radiants
volatile double Previous_Position_L =0.0; //The previous position of the left motor in radiants
volatile double Previous_Position_R = 0.0; //The previous position of the right motor in radiants
volatile double Speed_L = 0.0; //The speed of the left motor
volatile double Speed_R = 0.0; //The speed of the right motor

//Defining Pins
QEI Encoder_L (D11, D10,NC, 64);  //D11 is poort A D10 is poort b
QEI Encoder_R (D13, D12,NC, 64); //D 13 is poort A 12 is poort b

//--------------------AnalogInput---------------------------------
AnalogIn EMG_L(A0); //Motorspeed control Left
AnalogIn EMG_R(A1); //MotorSpeed control Right
AnalogIn EMG_Change(A2);//EMG signal for other controls
//-------------------Hidscope----------------------------------
HIDScope scope(4); // Sending two sets of data

//-------------------Interrupts-------------------------------
InterruptIn Dir_L(D0); //Motor Dirrection left
InterruptIn Dir_R(D1); //Motor Dirrection Right
InterruptIn OnOff(SW3); //System On/off


//------------------Tickers------------------------------------

Ticker Tick_Sample;// ticker for data sampling

//--------------------------------------------------------------
//--------------------All Motors--------------------------------
//--------------------------------------------------------------

DigitalOut M_L_D(D4); //Richting motor links-> 
//while M_L_D is zero the motor's direction is counterclockwise and the pulses are positive
PwmOut     M_L_S(D5); //Speed motor links
DigitalOut M_R_D(D7); //Richting motor Rechts
PwmOut     M_R_S(D6); //Speed motor Rechts
const int Motor_Frequency = 1000;

DigitalOut Grap(D9); //Graple active or not;

//----------------------Lights ---------------------------------4
DigitalOut translation(D0);
DigitalOut grip1(D1);
DigitalOut rotation(D2);
DigitalOut grip2(D3);

//--------------------------------------------------------------
//-----------------------Functions------------------------------
//--------------------------------------------------------------

//-----------------------on of sitch of the sytem-----------------------------


volatile bool Active = false;
void Start_Stop(){
    Active = !Active;
    }
    
/////////////-----------------------Booting---------------------------------------=
void Boot(){
  
  //Fucnction that initializes variables that have to be initalized within the main of the script
  //And does the same thing for functions like encoder reset
    pc.baud(115200);
    pc.printf("\r\n**BOARD RESET**\r\n");
    
    M_L_S.period(1.0/Motor_Frequency);
    M_L_S = 0.0;
    M_R_S.period(1.0/Motor_Frequency);
    M_R_S= 0.0;
    
    Encoder_L.reset();
    Encoder_R.reset();
    
    Grap = 1;
    grip1 = 1;
    }

    
    
//--------------------------------Sampling------------------------------------------   
bool Sample_Flag = false;

void Set_Sample_Flag(){
    Sample_Flag = true;
    }
    
void Sample(){
    
//This function reads out the position and the speed of the motors in radiants 
//Aand stores them in the values Speed and Position

//Position in Radials:
    Previous_Position_L = Position_L;
    Previous_Position_R = Position_R;
    Position_L = Encoder_L.getPulses()*AnglePerPulse+Previous_Position_L;
    Position_R = Encoder_R.getPulses()*AnglePerPulse+Previous_Position_R;

//Speed in RadPers second
    Speed_L = (Position_L-Previous_Position_L)*SampleFrequency;
    Speed_R = (Position_R-Previous_Position_R)*SampleFrequency;
               scope.set(0,Speed_L);
               scope.set(1,Signal+Speed_L);
               scope.set(2,Speed_R);
               scope.set(3,Signal+Speed_R);

               scope.send();
               Encoder_L.reset();
               Encoder_R.reset();
               
    }
    
    bool Controller_Flag = false;
    
    Ticker Tick_Controller;
    
    void Set_controller_Flag(){
        Controller_Flag = true;
        }
    
   //---------------------------Defining the biquad filter function for controller----------------
double biquad( double u, double &v1, double &v2, const double a1, const double a2,
 const double b0, const double b1, const double b2 ){
 double v = u - (a1*v1) -(a2*v2);
 double y = b0*v + b1*v1 + b2*v2;
 v2 = v1; v1 = v;
 return y;
 }
   
    
//-------------------------------PID ---------------------------------
    
double PID(double e, const double Kp, const double Ki, const double Kd, double Sf,
 double &e_int, double &e_prev, double &f_v1, double &f_v2, const double f_a1,
 const double f_a2, const double f_b0, const double f_b1, const double f_b2){
    //This function is a PID controller that returns the errorsignal for the plant


     // Derivative Part with filtering
    double e_der = (e - e_prev)/Sf;
  //  e_der = biquad( e_der, f_v1, f_v2, f_a1, f_a2, f_b0, f_b1, f_b2 );
    e_prev = e;
    // Integral Part
    e_int = e_int + Sf * e;
    // PID
    return Kp*e + Ki*e_int + Kd * e_der;
    }
 
 //---------------------------Controller---------------------------------------  
 
 

 // Controller gains (motor1−Kp,−Ki,...)
 const double Kpm= 1, Kim = 0*0.02, Kdm = 0*0.041465;
 double er_int = 0, prev_er = 0;
 // Derivative filter coeicients (motor1−filter−b0,−b1,...)
 const double F_A1 = 1.0, F_A2 = 2.0, F_B0 = 1.0, F_B1 = 3.0, F_B2 = 4.0;
 // Filter variables (motor1−filter−v1,−v2)
 double f_v1 = 0, f_v2 = 0; 
    double Controller_L(int direction, double signal, double reference ){
        //This funcition returns a value that will be sent to drive the motor
        // - Motor_Direction_Input is the Digital in of the motor
        
        // 1 for dirrection means that a clockwise rotation wil be regarded as positive
        // and -1 for direction means the opposite
        
        //PID controller
       double Speed_Set = PID( reference-signal*direction,Kpm, Kim, Kdm, SampleFrequency, er_int,
 prev_er, f_v1, f_v2, F_A1, F_A2, F_B0, F_B1,F_B2 );
    //Determining rotational direction of the motor
            if ((reference-signal*direction >= 0)){
                 M_L_D = 1;
            } else {
                  M_L_D = 0;
             }
            return fabs(Speed_Set);
        }
        
double Controller_R(int direction, double signal, double reference ){
        //This funcition returns a value that will be sent to drive the motor
        // - Motor_Direction_Input is the Digital in of the motor
        
        // 1 for dirrection means that a clockwise rotation wil be regarded as positive
        // and -1 for direction means the opposite
        
        //PID controller
       double Speed_Set = PID( reference-signal*direction,Kpm, Kim, Kdm, SampleFrequency, er_int,
 prev_er, f_v1, f_v2, F_A1, F_A2, F_B0, F_B1,F_B2 );
    //Determining rotational direction of the motor
            if ((reference-signal*direction >= 0)){
                 M_R_D = 0;
            } else {
                 M_R_D = 1;
             }
            if (fabs(Speed_Set)< 0.4){
            return fabs(Speed_Set);
            } else {
                return 0.4;
            
            }
        }
//-----------------------------Change_Movement----------------------------------
// This function changes the movement type of the robot as a consequence of the input by EMG
// Or when the EMG signal is hold for 1.5S The grapler wil activate
Ticker Tick_Movement;
int check = 0;

bool Movement_Flag = false;
void Set_movement_Flag(){
    Movement_Flag = true;
}

void Change_Movement(){
   if (Movement_Flag == true){
       if (EMG_Change > 0.5f ){
            check += 1;
               if (check <= 6 and check > 1 ){
                    grip1 = 1;
                }
               if (check > 6){
                     grip2 =1;
                }
            } else {
                //Hold the signal shorter than 1.5 seconds
                if (check <= 6 and check > 1 ){
                    movement = !movement;
                    grip1 = 1;
                    }
                // Hold the signal longer than 1.5 seconds
                if (check > 6){
                     grip2 =1;
                     Grap = !Grap;
                     
                    }
                   
                grip2 = 0;
                grip1 =0;
                check = 0;
                    
        }
        Movement_Flag = false;
        }
    }
int main()
{
//---------------------Setting constants and system booting---------------------
   
    Boot();
    Tick_Sample.attach(Set_Sample_Flag, 1.0/SampleFrequency);    Tick_Controller.attach(Set_controller_Flag, 1.0/SampleFrequency);
    Tick_Movement.attach(Set_movement_Flag, 0.25);
    OnOff.fall(Start_Stop);
    //\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/
    //\/\/\/\/\/\/\/\/\/\/\/\/\/\Main Loop for program\/\/\/\/\/\/\/\/\/\/\/\/\/
    //\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/  
    while (true) { 
        if (Sample_Flag == true){
            Sample();
            Signal = pi*(EMG_R.read()-EMG_L.read());
            Sample_Flag = false;
            }
    //Main statement that states if the system is active or not  
        if (Active == true){
            green = 0;
            red = 1;
            Change_Movement();
            if (Controller_Flag == true){
               switch (movement) {
                   case 0:
                   //Clockwise rotation of the robot
                   direction_L = -1;
                   direction_R = -1;
                   break;
                   
                   case 1:
                   //Radial movement outwards for a positive value
                   direction_L = 1;
                   direction_R = -1;
                   break;
                   }
               M_L_S = Controller_L(direction_L,Signal,Speed_L);
               M_R_S = Controller_R(direction_R,Signal,Speed_R);
               Controller_Flag = false;
            }
        } else {
            if (Active == false){
                       green = 1;
            red = 0;
            M_R_S = 0;
            M_L_S = 0;
            }
        }       
    }
}