Diff: main.cpp

Revision:

4:8b1df22779a7

Parent:

2:0954eb04bb4c

Child:

5:4b2ff2a4664a

--- a/main.cpp	Thu Oct 13 11:07:26 2016 +0000
+++ b/main.cpp	Tue Oct 25 09:29:12 2016 +0000
@@ -1,90 +1,134 @@
 #include "mbed.h"
 #include "MODSERIAL.h"
-#include "QEI.h"
+#include "QEI.h"
+
 #include "math.h"
 #include "HIDScope.h"
 #include "Biquad.h"
  
-DigitalOut green(LED_GREEN);
-MODSERIAL pc(USBTX, USBRX);
+
+///////////////////////////
+////////Constantes/////////
+///////////////////////////
+
+double const pi = 3.14159265359;
+
 ///////////////////////////
-// Hardware initialiseren//
+// Initialise hardware/////
 ///////////////////////////
+
+MODSERIAL pc(USBTX, USBRX);
+//-----------------------------------------------------------
+//--------------------All lights-----------------------------
+//-----------------------------------------------------------
+DigitalOut green(LED_GREEN);
+DigitalOut red(LED_RED);
 //------------------------------------------------------------
 //--------------------All sensors-----------------------------
 //------------------------------------------------------------
+
 //--------------------Encoders--------------------------------
 
-QEI Encoder_L (D11, D10,NC, 32);  //D11 is poort A D10 is poort b
-QEI Encoder_R (D13, D12,NC, 32); //D 13 is poort A 12 is poort b
+//Variables
+   volatile int movement = 0,direction_L =0, direction_R =0;
+    volatile double Signal = 0.0;
+
 
-double AnglePerPulse = 11.25;
-double Position_L = 0.0;
-double Position_R = 0.0;
-double Previous_Position_L = 0.0;
-double Previous_Position_R = 0.0;
-//--------------------Analog---------------------------------
-AnalogIn P_M_L(A0); //Motorspeed Left control
-AnalogIn P_M_R(A1); //MotorSpeed Right control
+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(2); // Sending two sets of data
+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); //Motor On/off
+InterruptIn OnOff(SW3); //System On/off
 
 
 //------------------Tickers------------------------------------
 
-Ticker Measure; // ticker for data mesuaring
-//------------------------------------------------------------
-//--------------------All Motors----------------------------
-//------------------------------------------------------------
+Ticker Tick_Sample;// ticker for data sampling
 
-//-------------------- Motor Links----------------------------
-DigitalOut M_L_D(D4); //Richting motor links
+//--------------------------------------------------------------
+//--------------------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------------------------------
 //--------------------------------------------------------------
 
-//-----------------------Interrupts-----------------------------
-volatile int Motor_Frequency = 1000;
+//-----------------------on of sitch of the sytem-----------------------------
+
+
 volatile bool Active = false;
-volatile float Speed_L= 0.0;
-volatile float Speed_R= 0.0;
-
+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_D = 1;
     M_L_S = 0.0;
     M_R_S.period(1.0/Motor_Frequency);
-    M_R_D = 0;
     M_R_S= 0.0;
     
     Encoder_L.reset();
     Encoder_R.reset();
+    
+    Grap = 1;
+    grip1 = 1;
     }
-void Switch_Dirr_L(){ // Switching dirrection left motor (Every motor starts in clockwise dirrection)
-     M_L_D = !M_L_D;
+
+    
+    
+//--------------------------------Sampling------------------------------------------   
+bool Sample_Flag = false;
+
+void Set_Sample_Flag(){
+    Sample_Flag = true;
     }
     
-void Switch_Dirr_R(){ // Switching dirrection Right motor
-    M_R_D = !M_R_D;
-    }
-void De_Activate(){
-    Active = !Active;
-    }
+void Sample(){
     
-void Information(){
-    
-//---------------------Motor Data-----------------------------------------------
+//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;
@@ -92,43 +136,198 @@
     Position_R = Encoder_R.getPulses()*AnglePerPulse+Previous_Position_R;
 
 //Speed in RadPers second
-    Motor_Speed_L = Previous_Position_L - Position_R
-//---------------------Sending data---------------------------------------------
-    scope.set(0,M_L_S); //Sending motor left speed
-    scope.set(1,M_R_S); //sending random value
-    scope.send();
+    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;
     }
-//-----------------------Encoders-----------------------------------------------
-int Get_Phase_L(int Phase){
-    Phase = Encoder_L.getCurrentState();    
-    return Phase;
+ 
+ //---------------------------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--------------------
-    pc.baud(115200);
-    pc.printf("\r\n**BOARD RESET**\r\n");
+//---------------------Setting constants and system booting---------------------
+   
     Boot();
-
-    
-    Dir_L.fall(Switch_Dirr_L);
-    Dir_R.fall(Switch_Dirr_R); 
-    OnOff.fall(De_Activate);
-    Measure.attach(Information, 0.01);     
-    while (true) {  
-       // control= pc.getc();  
-       if (Active == true){
+    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;
-            Speed_L =  P_M_L.read();//5.0+0.1;
-            M_L_S = Speed_L/5+0.1f;
-            Speed_R = P_M_R.read();//5.0+0.1;
-            M_R_S = Speed_R/5+0.1f ;
+            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;
+            if (Active == false){
+                       green = 1;
+            red = 0;
+            M_R_S = 0;
             M_L_S = 0;
-            M_R_S = 0;
-                }
+            }
         }       
     }
-}
\ No newline at end of file
+}
+