File content as of revision 9:e764cb50d343:

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


//--------------------------------------------------------------
//--------------------All Motors--------------------------------
//--------------------------------------------------------------
volatile int movement = 1,direction_L =1, direction_R =-1; //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-----------------------------



AnalogIn Active(A5);

    

//--------------------------------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.005;
 double e_L_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, e_L_int );
    //Determining rotational direction of the motor
           if ((reference-signal*direction >= 0)){
                 M_L_D = 1;
            } else {
                 M_L_D = 0;
             }
            if (fabs(Speed_Set)< 1){
                if(fabs(reference-signal*direction) < 0.5 and reference <0.5){ //Applying Setpoint tollerance
                    return  0;
                    } else {
            return fabs(Speed_Set);
            }
            } else {
                return 1;            
            }
            }
      double e_R_int = 0;   
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, e_R_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)< 1){
                if(fabs(reference-signal*direction) < 0.5 and reference <0.5){ //Applying Setpoint tollerance
                    return  0;
                    } else {
            return fabs(Speed_Set);
            }
            } else {
                return 1;
            
            }
        }
//-----------------------------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;
        }
    }
    //-----------------------------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
HIDScope scope(3);
void getEMG(){
     
 
    EMG_Change = bqc_Change.step(Change.read());
    EMG_Change = fabs(EMG_Change);
    EMG_Change = bq3_Change.step(EMG_Change)*30;
         scope.set(0,EMG_Change);//=------------------------------------------------------------------------Scope     
        //scope.send();           //        scope.set(4,Change.read());     
     if (EMG_Change < 0.4){
        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)*30;
                    scope.set(1,EMG_L);//=------------------------------------------------------------------------Scope  
      
    if (EMG_L < 0.5){
        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)*30;        
         scope.set(2,EMG_R);//=------------------------------------------------------------------------Scope                  
    if (EMG_R < 0.5){
        EMG_R=0;
    } else {
        //EMG_R= 1.0;
    }
    scope.send();
}
//------------------------------------------------------Postition reset----------------------------------
//After the robot has been set in it's equlibrium state, this data is used to prevent the robot of destroying itself.

void Position_reset(){
    Position_R =0;
    Position_L =0;
    }

//-------------------------------------------------------Limit-------------------------------------
//calculates the angele of the robot with for its limit, and checks of its limit has been reached
    int config_count = 0;
    double max_angle = 0;
    double min_angle = 0.5;
int Limit(){
    double angle=Position_R-Position_L; //R moves the left arm and right moves the right arm
    int limit = 0;
    if (angle < min_angle){ //limit angle of -13 degrees translated to the motor -13*2*pi*136/(33*360)
            limit = 1;
        } 
        if (angle > max_angle){ //limit angle of 15 degrees tranlated to the motor 2*15*2*pi*136/(33*360) 
            limit = 2;
        }
      return limit;

        
    }



bool Configure_Flag = true;

    
/////////////-----------------------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 = 0;
                   translation = 1;
    Encoder_L.reset();
    Encoder_R.reset();
    
    Grap = 0;
    grip1 = 0;
    
    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);
    }

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

            
            //Signal = pi*(Right.read()-Left.read());
            //EMG_Change = Change.read();
               // scope.set(3,Signal);//------------------------------------------------------------scope

                  //       scope.send();
            Sample_Flag = false;
            }   
            //-------------------------------------------- Configureren-------------------------------------         
            if (Configure_Flag == true){
                
                 if (config_count < 20){   
                Signal = pi;
                } else {
                    Signal = -pi;
                    }
                 if (config_count == 19){ 
                  Position_reset();
                  
                 }
                 if (config_count > 40){
                     Configure_Flag = false;
                     max_angle = Position_R-Position_L-0.5;
                     }
                 
                
                M_L_S = 0.02*Controller_L(direction_L,Signal,Speed_L);
                M_R_S = 0.02*Controller_R(direction_R,Signal,Speed_R);

               
                //Signal = -pi;
               // M_L_S = 0.03*Controller_L(direction_L,Signal,Speed_L);
               // M_R_S = 0.03*Controller_R(direction_R,Signal,Speed_R);


                Grap = 0;
                if (Movement_Flag == true){
                rotation = !rotation;
                translation =!translation;

                grip1 = !grip1;
                grip2 = !grip2;
                config_count ++;
                Movement_Flag= false;
                }
            } else {
    //Main statement that states if the system is active or not  
        if (Active.read() > 0.5){
            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;
                   Signal = Signal*2;
                    M_L_S = Controller_L(direction_L,Signal,Speed_L);
                    M_R_S = Controller_R(direction_R,Signal,Speed_R);
                   break;
                   
                   case 1:
                   //Radial movement outwards for a positive value
                   direction_L = 1;
                   direction_R = -1;
                   rotation = 0;
                   translation = 1;
                   
                   switch (Limit()){ //making sure the limits are not passed
                       case 0: //case the robot is in its working space
                             M_L_S = Controller_L(direction_L,Signal,Speed_L);
                             M_R_S = Controller_R(direction_R,Signal,Speed_R);
                       break;
                       case 1: //case the robot has reached it's lowest limit
                            if (Signal < 0){
                                M_L_S = Controller_L(direction_L,Signal,Speed_L);
                                M_R_S = Controller_R(direction_R,Signal,Speed_R);
                            } else {
                                M_L_S = 0;
                                M_R_S = 0;
                                }
                       break;
                       case 2: //case the robot has reachted its highest limit
                            if (Signal > 0){
                                M_L_S = Controller_L(direction_L,Signal,Speed_L);
                                M_R_S = Controller_R(direction_R,Signal,Speed_R);
                            } else {
                                M_L_S = 0;
                                M_R_S = 0;
                                }
                       break; 
                       }
                   break;
                   }

               Controller_Flag = false;
            }
        } else {
            if (Active.read() < 0.5){
            green = 1;
            red = 0;
            M_R_S = 0;
            M_L_S = 0;
            Grap=0;
            }
        }       
    }
}
}