File content as of revision 7:075ba23f3147:

#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 double Signal = 0.0;
const int SampleFrequency = 400;
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 Left(A0); //Motorspeed control Left
AnalogIn Right(A1); //MotorSpeed control Right
AnalogIn Change(A2);//EMG signal for other controls
double EMG_L;
double EMG_R;
double EMG_Change; 
//-------------------Hidscope----------------------------------


//--------------------------------------------------------------
//--------------------All Motors--------------------------------
//--------------------------------------------------------------
volatile int movement = 0,direction_L =0, direction_R =0; //determining the direction of the 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-----------------------------

InterruptIn OnOff(SW3); //System On/off
volatile bool Active = false;
void Start_Stop(){
    Active = !Active;
    }
    

//--------------------------------Sampling------------------------------------------   

Ticker Tick_Sample;// ticker for data 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;

               Encoder_L.reset();
               Encoder_R.reset();
               
    }
    
    bool Controller_Flag = false;
    
    Ticker Tick_Controller;
    
    void Set_controller_Flag(){
        Controller_Flag = true;
        }


    
//-------------------------------PI ---------------------------------
    
double PI(double e, const double Kp, const double Ki, double Sf,
 double &e_int){
    //This function is a PID controller that returns the errorsignal for the plant


    e_int = e_int + Sf * e;
    // PID
    return Kp*e + Ki*e_int ;
    }
 
 //---------------------------Controller---------------------------------------  
 
 

 // Controller gains (motor1−Kp,−Ki,...)
 const double Kpm= 0.1, Kim = 0*0.05;
 double er_int = 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 = PI( reference-signal*direction,Kpm, Kim, SampleFrequency, er_int );
    //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
        
        //PI controller
       double Speed_Set = PI( reference-signal*direction,Kpm, Kim,  SampleFrequency, er_int);
    //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){
                if(fabs(reference-signal*direction) < 0.5 and reference <0.5){ //Applying Setpoint tollerance
                    return  0;
                    } else {
            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 <= 10 and check > 1 ){
                    grip1 = 1;
                }
               if (check > 10){
                     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;
        }
    }
    //-----------------------------EMG Signal determinator----------------------
BiQuadChain bqc_L;
BiQuadChain bqc_R;
BiQuadChain bqc_Change;
BiQuad bq1_R( 9.35527e-01, -1.87105e+00, 9.35527e-01, -1.86689e+00, 8.75215e-01 );
BiQuad bq2_R( 9.90278e-01, -1.40046e+00, 9.90278e-01, -1.40046e+00, 9.80555e-01 );
BiQuad bq3_R( 2.20594e-05, 4.41189e-05, 2.20594e-05, -1.98667e+00, 9.86760e-01 );

BiQuad bq1_L( 9.35527e-01, -1.87105e+00, 9.35527e-01, -1.86689e+00, 8.75215e-01 );
BiQuad bq2_L( 9.90278e-01, -1.40046e+00, 9.90278e-01, -1.40046e+00, 9.80555e-01 );
BiQuad bq3_L( 2.20594e-05, 4.41189e-05, 2.20594e-05, -1.98667e+00, 9.86760e-01 );


BiQuad bq1_Change( 9.35527e-01, -1.87105e+00, 9.35527e-01, -1.86689e+00, 8.75215e-01 );
BiQuad bq2_Change( 9.90278e-01, -1.40046e+00, 9.90278e-01, -1.40046e+00, 9.80555e-01 );
BiQuad bq3_Change( 2.20594e-05, 4.41189e-05, 2.20594e-05, -1.98667e+00, 9.86760e-01 );
HIDScope scope(5); // Sending two sets of data
void getEMG(){
     
 
    EMG_Change = bqc_Change.step(Change.read());
    EMG_Change = fabs(EMG_Change);
    EMG_Change = bq3_Change.step(EMG_Change)*50;
         scope.set(0,EMG_Change);//=------------------------------------------------------------------------Scope     
                             scope.set(4,Change.read());     
     if (EMG_Change < 0.3){
        EMG_Change=0;
    } else {
        EMG_Change=1.0;
    }
           
            
    EMG_L =   bqc_L.step(Left.read());
    EMG_L= fabs(EMG_L);
    EMG_L= bq3_L.step( EMG_L)*10;
                    scope.set(1,EMG_L);//=------------------------------------------------------------------------Scope  
      
    if (EMG_L < 0.3){
        EMG_L=0;
    } else {
        EMG_L=1.0;
    }
    
               
  
    EMG_R =  bqc_R.step(Right.read());
    EMG_R= fabs(EMG_R);
    EMG_R= bq3_R.step( EMG_R)*10;        
          scope.set(2,EMG_R);//=------------------------------------------------------------------------Scope                  
    if (EMG_R < 0.3){
        EMG_R=0;
    } else {
        EMG_R= 1.0;
    }
    
}

/////////////-----------------------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;
                       rotation = 1;
                   translation = 0;
    Encoder_L.reset();
    Encoder_R.reset();
    
    Grap = 1;
    grip1 = 1;
    
        bqc_L.add( &bq1_L ).add( &bq2_L );
    bqc_R.add(&bq1_R).add(&bq2_R);
    bqc_Change.add(&bq1_Change).add(&bq2_Change);
    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);
    
    }

    
    
int main()
{
//---------------------Setting constants and system booting---------------------
   
    Boot();

    //\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/
    //\/\/\/\/\/\/\/\/\/\/\/\/\/\Main Loop for program\/\/\/\/\/\/\/\/\/\/\/\/\/
    //\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/  
    while (true) { 
       if (Sample_Flag == true){
            Sample();
            getEMG();
              //  EMG_Change = getEMG(  bqc.step(Change.read()));
            Signal = pi*(EMG_R-EMG_L);
                scope.set(3,Signal);//------------------------------------------------------------scope

                         scope.send();
            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;
                   rotation = 1;
                   translation = 0;
                   break;
                   
                   case 1:
                   //Radial movement outwards for a positive value
                   direction_L = 1;
                   direction_R = -1;
                   rotation = 0;
                   translation = 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;
            }
        }       
    }
}