Diff: main.cpp

Revision:

5:3562c205d001

Parent:

4:afa85283eb18

Child:

6:643c44fb044a

diff -r afa85283eb18 -r 3562c205d001 main.cpp
--- a/main.cpp	Wed Nov 01 11:50:05 2017 +0000
+++ b/main.cpp	Fri Nov 03 06:40:08 2017 +0000
@@ -1,229 +1,528 @@
- /**
- * Demo program for BiQuad and BiQuadChain classes
- * author: T.J.W. Lankhorst <t.j.w.lankhorst@student.utwente.nl> and Matthijs and Roy and Dion
- */
+//initialisation 
 #include "mbed.h"
+#include "encoder.h"
 #include "HIDScope.h"
+#include "MODSERIAL.h"
+#include "math.h"
+#include "BiQuad.h"
+#include "Matrix.h"
+#include "MatrixMath.h"
 #include <stdlib.h>
 #include <iostream>
 #include <iomanip>
 #include <complex>
 #include <vector>
-#include "BiQuad.h"
-#include "Matrix.h"
-#include "MatrixMath.h"
-#include "MODSERIAL.h"
-#include "encoder.h"
+
+Ticker                  MainTicker;
 
-AnalogIn    emg0( A0 );
-AnalogIn    emg1( A1 );
-DigitalIn   button1(D11);
-Encoder     encoder1(D13,D12);
-Encoder     encoder2(D13,D12); //moet nog even aangepast worden
-
-
-DigitalOut motorDirection(D4);
-PwmOut motorSpeed(D5);
-DigitalOut motorDirection1(D4); //nog goede channel toevoegen
-PwmOut motorSpeed1(D5);         //nog goede channel toevoegen
-
-MODSERIAL   pc(USBTX, USBRX);
+//Define objects EMG
+AnalogIn                emg0( A0 );
+AnalogIn                emg1( A1 );
+DigitalIn               buttonCal(D11);
+DigitalIn               changeX(D2);            
+DigitalIn               changeY(D3);
+InterruptIn             turning(D10);
 
-Ticker      sample_timer;
-Ticker      cal_timer;
-HIDScope    scope( 4 );
-DigitalOut  led(LED1);
-DigitalOut  led2(LED_GREEN);                  
+Ticker                  sample_timer;
+Ticker                  calX_timer;
+Ticker                  calY_timer;
+//HIDScope                scope( 4 );
+DigitalOut              led(LED1);
+DigitalOut              led2(LED_GREEN);  
+
+//Define variables EMG            
 const int size = 100;
-vector<double> S(size,0);
-vector<double> S1(size,0);
+vector<double> SX(size,0);
+vector<double> SY(size,0);
 double meanSum = 0;
-
 double maxsignal = 0;
-double emgX = 0;
-double emgY = 0;
 
-double L0 = 0.123;
-double L1 = 0.37;
-double L2 = 0.41;
-double q1 = encoder1.getPosition()/131.25; //Calibreren nog toevoegen
-double q2 = encoder2.getPosition()/131.25; //Calibreren mist nog
-double Periode = 1/500;   //1000 is het aantal Hz
-
-//Filter toevoegen, bestaande uit notch, high- en lowpass filter
+//Filters
 BiQuadChain Notch;
 BiQuadChain Notch50;
 BiQuadChain bqcLP;
 BiQuadChain bqc;
-//50 Hz Notch filter, for radiation from surroundings
 
 //Notch filter chain for 100 Hz
 BiQuad bqNotch1( 9.75180e-01, -1.85510e+00, 9.75218e-01, -1.87865e+00, 9.75178e-01 );
 BiQuad bqNotch2( 1.00000e+00, -1.90228e+00, 1.00004e+00, -1.88308e+00, 9.87097e-01 );
 BiQuad bqNotch3( 1.00000e+00, -1.90219e+00, 9.99925e-01, -1.89724e+00, 9.87929e-01 );
 
-//BiQuad Notch(0.9179504645111498,-1.4852750515677946,  0.9179504645111498, -1.4852750515677946, 0.8359009290222995);
-
 //Notch filter chain for 50 Hz
 BiQuad bq1( 9.50972e-01, -1.53921e+00, 9.51003e-01, -1.57886e+00, 9.50957e-01 );
 BiQuad bq2( 1.00000e+00, -1.61851e+00, 9.99979e-01, -1.57185e+00, 9.74451e-01 );
 BiQuad bq3( 1.00000e+00, -1.61855e+00, 9.99988e-01, -1.62358e+00, 9.75921e-01 );
 
-//15 Hz Highpass filter, as recommended by multiple studies regarding EMG signals
-//BiQuad Highpass15(9.35527e-01, -1.87105e+00, 9.35527e-01, -1.86689e+00, 8.75215e-01);
-
-//2Hz Highpass
+//2 Hz High Pass filter
 BiQuad Highpass1(9.82385e-01, -1.96477e+00, 9.82385e-01, -1.96446e+00, 9.65081e-01);
 
-
-//20 Hz Lowpass
-//BiQuad Lowpass100( 3.62168e-03, 7.24336e-03, 3.62168e-03, -1.82269e+00, 8.37182e-01 );
-
-//250 Hz Lowpass
-//BiQuad Lowpass100 (2.92893e-01, 5.85786e-01, 2.92893e-01, -3.60822e-16, 1.71573e-01 );
-
-//80 Hz Lowpass
+//80 Hz Low Pass filter
 BiQuad bqLP1( 5.78904e-02, 5.78898e-02, 0.00000e+00, -2.90527e-01, 0.00000e+00 );
 BiQuad bqLP2( 1.00000e+00, 2.00001e+00, 1.00001e+00, -7.53537e-01, 4.06308e-01 );
 
 
+//Define objects MotorControl
+DigitalOut              gpo(D0);
+DigitalOut              UpperMotorDir(D4);
+PwmOut                  UpperMotorVel(D5);
+DigitalOut              LowerMotorDir(D6);
+PwmOut                  LowerMotorVel(D7);      
+AnalogIn                potMeterIn(A1);
+InterruptIn             ButMotorDir(D3);
+Encoder                 encoder1(D13,D12);
+Encoder                 encoder2(D9,D8);
+Ticker                  m1_Ticker;
+Ticker                  setpoint_Ticker;
 
-//450 Hz Lowpass filter, to remove any noise (sample frequency is only 500 Hz, but still...)
-//BiQuad Lowpass450(8.00592e-01, 1.60118e+00, 8.00592e-01, 1.56102e+00, 6.41352e-01);
+//Define variables MotorControl
+float           M_PI = 3.14159265359;
+
+float           M1_KP = 1.0f;
+float           M1_KI = 0.1f;
+const float     M1_TS = 0.002;
+const float     q_RAD_PER_PULSE_m2 = 0.36f*M_PI/1300.0f;                        //2*M_PI/4320 voor als het nu geen rechte lijn meer is8 
+const float     q_RAD_PER_PULSE_m1 = 0.5f*M_PI/3300.0f;
+
+float           m1_err_int = 0;
+float           m2_err_int = 0;
+int             motorD1 = 0;
+float           motor1 = 0;
+int             motorD2 = 0;
+float           motor2 = 0;
+float           position1 = 0;
+float           position2 = 0;
+
+float           emgX = 0;
+float           emgY = 0;
 
-//Making matrices globaly     
-Matrix      JAPPAPP(2,2);
-Matrix      qdot(2,1);
-Matrix      Vdes(2,1);
-Matrix      qsetpoint(2,1);     
-     
-     
+float           L0 = 0.123;
+float           L1 = 0.37;
+float           L2 = 0.41;
+float           q1 = 0.0f*M_PI;
+float           q2 = 0.0f*M_PI;
+float           Periode = 0.001;   //1000 is het aantal Hz
+
+float           ref_pos1;
+float           ref_pos2;
+
+float           Qset1 = 0;
+float           Qset2 = 0;
+
+int UpperMotorPos;
+int UpperMotorPos_prev;
+int LowerMotorPos;
+int LowerMotorPos_prev;
+
+int             NPC = 0;
+float           NoTurnies = 1; 
+
+//Define matrices MotorControl
+Matrix          JAPPAPP(2,2);
+Matrix          qdot(2,1);
+Matrix          Vdes(2,1);
+
+//Define object Servo Motors
+DigitalOut              Ledr(LED_RED);
+DigitalOut              Ledg(LED_GREEN);
+DigitalOut              Ledb(LED_BLUE);
+PwmOut                  LeftServoVel(D11);
+PwmOut                  RightServoVel(D10); 
+DigitalIn               ServoButton(D2);
+Ticker                  ServoTick;
 
 
-     
-double findRMS(vector<double> array)
-{
+//Other variables
+float Cal_Strength = 0.6f;
+
+
+//Connection with TeraTerm
+MODSERIAL pc(USBTX, USBRX);
+
+//Creating of the states
+enum states{calib1, calib2, calibEMG, UpDown, LeftRight, Vertical, MotionEndEffector, GrabbingPlacing, Off, Idle, No_State};
+
+//Root Mean Square value calculator for useful envelope, using vectors
+double findRMS(vector<double> array) {
      int i;
-     double sumsquared = 0.000;
+     double sumsquared = 0.000;     //define variable that resets to 0 for every call of the function
      double RMS;
-     //sumsquared = 0;
-     for (i = 0; i < size; i++)
+     
+     for (i = 0; i < size; i++)     //for loop to calculate the total sum of the squared values
      {
         sumsquared = sumsquared + array.at(i)*array.at(i);
      }
-     RMS = sqrt((double(1)/size)*(sumsquared)); 
+     RMS = sqrt((double(1)/size)*(sumsquared));     //root mean square calculation
      return RMS;
 }
-     
-     
-     
-void sample()
-{
+
+
+//Function to show the EMG signals in HIDScope
+void sample() {
     /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */
-    //scope.set(0, emg0.read() );
-    //scope.set(1, fabs(Notch.step(emg0.read())) );
-    scope.set(0, emg0.read()-0.4542);
-    //scope.set(1, bqcLP.step(Highpass1.step(emg0.read()-0.4542)));
-    //scope.set(2, fabs(Notch50.step(bqcLP.step(Highpass1.step(emg0.read()-0.4542)))));
-    //scope.set(3, fabs(bqc.step(emg0.read()-0.4542)));
-    //scope.set(3, Notch.step(Notch50.step(bqcLP.step(Highpass1.step(fabs(emg0.read()-0.4542))))));
+    //Standard emg value without filters, offset removed with the - value
+    //scope.set(0, emg0.read()-0.4542f);
+    
+    //Use S vector to save values coming from EMG signal
+    SX.erase(SX.begin());                         //remove first value
+    double emgXvalue = bqc.step(emg0.read()-0.4542f);
+    SX.push_back(emgXvalue);                             //add extra value to begin of vector
+    
+    SY.erase(SY.begin());                         //remove first value
+    double emgYvalue = bqc.step(emg0.read()-0.4542f);
+    SY.push_back(emgYvalue);                             //add extra value to begin of vector
     
     
-    //scope.set(0, fabs(Highpass1.step(Lowpass100.step(Notch.step(emg0.read())))) ); 
-    /*
-    for (int i = size-1; i > 1; i--) {
-            S[i] = S[i-1];    
-        }
-    */
-    S.erase(S.begin());
-    S1.erase(S1.begin());
-    double b = bqc.step(emg0.read()-0.4542);
-    double c = bqc.step(emg1.read()-0.4542);
-    S.push_back(b);
-    S1.push_back(c);
-    //S[0] = bqc.step(emg0.read()-0.4542)));
-    //Notch50.step(bqcLP.step(Highpass1.step(emg0.read()-0.4542)));
-    emgX = findRMS(S);
-    emgY = findRMS(S1);
-    scope.set(1, emgX);
-    //scope.set(2, S[0]);
-    //meanSum = 0; */
-    /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels) 
-    *  Ensure that enough channels are available (HIDScope scope( 2 ))
-    *  Finally, send all channels to the PC at once */
-    scope.send();
+    //Calculate Root Mean Square value using findRMS function as defined above
+    double emgX = findRMS(SX);
+    double emgY = findRMS(SY);
+    
+    //Display results
+    //scope.set(1, emgX);
+    //scope.set(2, emgY);
+    
+    //Send all scopes to HIDScope
+    //scope.send();
+    
     /* To indicate that the function is working, the LED is toggled */
     led = !led;
-    
-    motorSpeed.write(0.38080*(emgX/maxsignal));
-    motorDirection.write(1);
 }
 
-void calibration()
-{
+//Calibration function for the EMG signals
+void calibrationX() {
     //function to determine maximal EMG input in order to allow motorcontrol to be activated
     
-    if (button1.read() == false){
+    if (buttonCal.read() == false){       //activated when button is pressed
         led2 = !led2;
         for (int n = 1; n < 5000; n++){ //calibrate for 5000 samples or 10 seconds
-            
-                S.erase(S.begin());
-                double b = bqc.step(emg0.read()-0.4542);
-                S.push_back(b);
-                //S[0] = bqc.step(emg0.read()-0.4542)));
-                //Notch50.step(bqcLP.step(Highpass1.step(emg0.read()-0.4542)));
-                double signalfinal = findRMS(S);
-            if (signalfinal >= maxsignal){
-                maxsignal = signalfinal; //keep changing the maximal signal
-                pc.printf("%d \n",maxsignal);
+                SX.erase(SX.begin());
+                double emgXCalvalue = bqc.step(emg0.read()-0.4542f);
+                SX.push_back(emgXCalvalue);
+                double signalfinal = findRMS(SX);    //calculate RMS values from EMG signals
+            if (signalfinal >= maxsignal){          //Check if current signal is higher than current maximal signal
+                maxsignal = signalfinal;            //keep changing the maximal signal
             }
         }
-        led2 = 1;       
+        led2 = 1;                                   //Turn LED off when calibration is finished
+    }
+}
+
+void calibrationY() {
+    //function to determine maximal EMG input in order to allow motorcontrol to be activated
+    
+    if (buttonCal.read() == false){       //activated when button is pressed
+        led2 = !led2;
+        for (int n = 1; n < 5000; n++){ //calibrate for 5000 samples or 10 seconds
+                SY.erase(SY.begin());
+                double emgYCalvalue = bqc.step(emg1.read()-0.4542f);
+                SY.push_back(emgYCalvalue);
+                double signalfinal = findRMS(SY);    //calculate RMS values from EMG signals
+            if (signalfinal >= maxsignal){          //Check if current signal is higher than current maximal signal
+                maxsignal = signalfinal;            //keep changing the maximal signal
+            }
+        }
+        led2 = 1;                                   //Turn LED off when calibration is finished
+    }
+}
+
+
+void NegPosChanger(){
+    if(NPC == 0){
+        NoTurnies = 0;
+        NPC++;
+    } else {
+        NoTurnies = 1;
+        NPC= 0;
     }
 }
 
 
-void qSetpointSet(){
-        // Fill Matrix with data.
-    JAPPAPP(1,1) =  -(L0 - L0*sin(q1) + L1*sin(q1) - L2*sin(q2))/(L1*L1*cos(q1)*sin(q1) + L0*L1*cos(q1) + L0*L2*cos(q2) - L0*L1*cos(q1)*sin(q1) - L0*L2*cos(q2)*sin(q1) - L1*L2*cos(q1)*sin(q2));  
-    JAPPAPP(1,2) = -(L2*cos(q2))/(L1*L1*cos(q1)*sin(q1) + L0*L1*cos(q1) + L0*L2*cos(q2) - L0*L1*cos(q1)*sin(q1) - L0*L2*cos(q2)*sin(q1) - L1*L2*cos(q1)*sin(q2));
-    JAPPAPP(2,1) = (L1*sin(q1) - L2*sin(q2))/(L1*L1*cos(q1)*sin(q1) + L0*L1*cos(q1) + L0*L2*cos(q2) - L0*L1*cos(q1)*sin(q1) - L0*L2*cos(q2)*sin(q1) - L1*L2*cos(q1)*sin(q2));                     
-    JAPPAPP(2,2) = (L1*cos(q1) + L2*cos(q2))/(L1*L1*cos(q1)*sin(q1) + L0*L1*cos(q1) + L0*L2*cos(q2) - L0*L1*cos(q1)*sin(q1) - L0*L2*cos(q2)*sin(q1) - L1*L2*cos(q1)*sin(q2));
-             
+void qSetpointSet() {
     // Fill Matrix with data.
-    Vdes(1,1) = emgX;        //Goede code nog toevoegen, Vx
-    Vdes(2,1) = emgY;        //goede code nog toevoegen, Vy
+    q1 = encoder1.getPosition()*q_RAD_PER_PULSE_m1; //Calibreren nog toevoegen
+    q2 = 0.5f*M_PI - q1 + encoder2.getPosition()*q_RAD_PER_PULSE_m2; //Calibreren mist nog
+    
+    JAPPAPP(1,1) = (cos(q1)*cos(q2) - sin(q1)*sin(q2))/(L1*sin(q2)*cos(q1)*cos(q1) + L1*sin(q2)*sin(q1)*sin(q1));
+    JAPPAPP(1,2) = (cos(q1)*sin(q2) + cos(q2)*sin(q1))/(L1*sin(q2)*cos(q1)*cos(q1) + L1*sin(q2)*sin(q1)*sin(q1));
+    JAPPAPP(2,1) = -(L1*cos(q1) + L2*cos(q1)*cos(q2) - L2*sin(q1)*sin(q2))/(L1*L2*sin(q2)*cos(q1)*cos(q1) + L1*L2*sin(q2)*sin(q1)*sin(q1));
+    JAPPAPP(2,2) =  -(L1*sin(q1) + L2*cos(q1)*sin(q2) + L2*cos(q2)*sin(q1))/(L1*L2*sin(q2)*cos(q1)*cos(q1) + L1*L2*sin(q2)*sin(q1)*sin(q1));
+       
+    // Fill Matrix with data.
+    int Xchanger = (changeX == 1) ? -1 : 1;
+    int Ychanger = (changeY == 1) ? -1 : 1;
+    Vdes(1,1) = emgX*Xchanger*NoTurnies;
+    Vdes(2,1) = emgY*Ychanger*NoTurnies;
+    int turnChanger = (turning == 1) ? -1 : 1;
     
     qdot = JAPPAPP*Vdes;
-        
-    qsetpoint(1,1) = q1 + qdot(1,1)*Periode;
-    qsetpoint(2,1) = q2 + qdot(2,1)*Periode;    
+    
+    Qset1 = Qset1 + qdot(1,1)*Periode;       //Vdes(1,1)
+    Qset2 = Qset2 + qdot(2,1)*Periode;      //Vdes(2,1)
+
+    if (Qset1 <= 0.43f*M_PI && Qset1 >= 0.01f){
+       Qset1 = Qset1;
+    }
+    else if (Qset1 < 0.01f){
+        Qset1 = 0.01f;
+    }
+    else if (Qset1 > 0.43f*M_PI){
+        Qset1 = 0.43f*M_PI;
+    }
+    if (Qset1 + Qset2 < 0.5f*M_PI){
+        Qset2 = 0.5f*M_PI - Qset1;
+    }
+    if (Qset2 <= 0.5f*M_PI && Qset2 >= 0.1f){
+       Qset2 = Qset2;
+    }
+    else if (Qset2 < .1f){
+        Qset2 = 0.1f;
+    }
+    else if (Qset1+Qset2 > M_PI){       
+        Qset2 = M_PI - Qset1;
+    }
+    
+}
+
+float PI( float e, const float Kp, const float Ki, float Ts, float &e_int ){                 // Reusable PI controller
+    return fabs(Kp * e);
+}
+
+void PositionControl() {
+    //position1 = RAD_PER_PULSE * encoder1.getPosition();
+    //position2 = RAD_PER_PULSE * encoder2.getPosition();
+    ref_pos1 = Qset1 - q1;
+        //if (ref_pos1 < 0.1) { ref_pos1 = 0; }
+    ref_pos2 = Qset2 - q2;                                          // Don’t use magic numbers!
+        //if (ref_pos2 < 0.1) { ref_pos2 = 0; }
+    motor1 = PI(ref_pos1, M1_KP, M1_KI, M1_TS, m1_err_int );
+    motor2 = PI(ref_pos2, M1_KP, M1_KI, M1_TS, m2_err_int );
+    if (ref_pos1 <= 0) {
+            motorD1=1;
+        }
+        else{
+            motorD1 = 0;
+        }
+    if (ref_pos2 <= 0) {
+            motorD2=0;
+        }
+        else{
+            motorD2=1;
+        }
+        UpperMotorDir.write(motorD1);
+        UpperMotorVel.write(motor1);
+        LowerMotorDir.write(motorD2);
+        LowerMotorVel.write(motor2);
+}
+
+/** Servo motor control function **/
+
+//Servo initialisation
+enum servostates{Close, Open};
+
+//Set default state to open, as mentioned in paragraph ... of the report
+servostates CurrentState = Open;
+
+int i = 0;
+int j = 0;
+
+//Simply switch the current state when button is pressed
+void Change() {
+    if(CurrentState == Close){
+    CurrentState = Open;
+    } 
+    else{
+    CurrentState = Close;
+    }
 }
 
-int main()
-{   
-pc.baud(115200);
+//State machine for the servo motors, to determine the state
+void ProcessStateMachine(void) {
+/**
+State machine for the servo motors, motor response dependent on the current state and the amount of milliseconds of activation.
+The entire function must be sampled at 2000 Hz, since it counts half milliseconds. This is required since the servos respond to half millisecond actuation.
+For further information on the functioning of the servo motors; see paragraph ... in report 'Design of a Jenga playing robot' by group 2.
+**/
+    switch (CurrentState)
+        {        
+        case Close:
+            if(j <= 35){
+                RightServoVel.write(0);
+                LeftServoVel.write(0);
+                j++;
+            }
+            else if (j == 36){
+                RightServoVel.write(1);
+                LeftServoVel.write(0);
+                j++;
+            }
+            else if (j == 37){
+                RightServoVel.write(1);
+                LeftServoVel.write(0);
+                j++;
+            }
+            else if (j == 38){
+                RightServoVel.write(1);
+                LeftServoVel.write(0);
+                j++;
+            }
+            else if (j == 39){
+                RightServoVel.write(1);
+                LeftServoVel.write(1);
+                j = 0;
+            }
+        break;
+        
+        case Open:
+            if(j <= 35){
+                RightServoVel.write(0);
+                LeftServoVel.write(0);
+                j++;
+            }
+            else if (j == 36){
+                RightServoVel.write(0);
+                LeftServoVel.write(0);
+                j++;
+            }
+            else if (j == 37){
+                RightServoVel.write(0);
+                LeftServoVel.write(1);
+                j++;
+            }
+            else if (j == 38){
+                RightServoVel.write(1);
+                LeftServoVel.write(1);
+                j++;
+            }
+            else if (j == 39){
+                RightServoVel.write(1);
+                LeftServoVel.write(1);
+                j = 0;
+            }
+        break;
+    }
+}
+
+//StateMachine for the moving of the end effector
+
+void ProcessStateMachineMain(void) {
+    states CurrentState1 = calibEMG;
+    switch (CurrentState1)
+        {        
+        case MotionEndEffector:
+            m1_Ticker.attach( &PositionControl, M1_TS );    
+            setpoint_Ticker.attach(&qSetpointSet, Periode);  
+        break;
+        
+        case calibEMG:
+            //Function to calibrate the filtered EMG signals to the user, in order to control the motors
+                double maxsignal = 0;   //empty variable to check the maximal EMG output
+                for (int n = 1; n < 5000; n++){ //calibrate for 5000 samples or 5 seconds
+                    double signalfinal = fabs(bqc.step(emg0.read()));   //get values from the filtered EMG signal
+                    if (signalfinal >= maxsignal){  //check whether the received signal is higher or lower than the current highest signal value
+                        maxsignal = signalfinal; //keep changing the maximal signal
+                    }
+                }
+        break;
+
+        case calib1:
+            UpperMotorVel = Cal_Strength; UpperMotorDir = true;          //depends on definitions
+            LowerMotorVel = 0.0f;
+            UpperMotorPos = encoder1.getPosition();
+            if (abs(UpperMotorPos - UpperMotorPos_prev) < 0.01f){
+                if (Cal_Strength < 1.0f) { 
+                    Cal_Strength += 0.05f;
+                }
+                else{
+                    encoder1.setPosition(0);            //Afhankelijk van RKI uitzoeken of deze echt naar nul moet of naar 0.5pi
+                    CurrentState1 = calib2;
+                    Cal_Strength = 0.6f;
+                }
+            int UpperMotorPos_prev = UpperMotorPos;
+            } 
+        break;
+
+        case calib2:
+            LowerMotorVel = Cal_Strength; LowerMotorDir = true;          // Check definitions
+            UpperMotorVel = 1.0f;
+            LowerMotorPos = encoder2.getPosition();
+            if (abs(LowerMotorPos - UpperMotorPos_prev) < 0.01f){
+                if (Cal_Strength < 1.0f) { 
+                    Cal_Strength += 0.05f;
+                }
+                else{
+                    encoder2.setPosition(0);
+                    CurrentState1 = Idle;
+                    //M1_controller = true; //turn on the position controller here
+                    UpperMotorVel = 0.0f; LowerMotorVel = 0.0f;
+                }
+                int UpperMotorPos_prev = UpperMotorPos;
+            }
+        break;
+       
+        case Idle:
+            emgX = 0;
+            emgY = 0;
+            m1_Ticker.attach( &PositionControl, M1_TS );    
+            setpoint_Ticker.attach(&qSetpointSet, Periode);  
+        break;
+        
+        case Off:
+            //Robot is off, also return to default state so you have a reference position when it starts up again
+        break;
+        
+        default:
+            //Robot is in idle mode, not doing anything (motors still powered to hold current position) (Posible when waiting for your turn)
+
+    }
+}
+        
+//Main function for the servo motors, to change open or closed state
+int main() {
+    //allow for the 2000 Hz sampling of the Servo state machine function
+    MainTicker.attach(ProcessStateMachineMain, 0.0005);
+    
+    ServoTick.attach(ProcessStateMachine, 0.0005);
+    
+      
+            
+
+    
+    /** EMG signals **/
     //Constructing the notch filter chain
-    Notch.add( &bqNotch1 ).add( &bqNotch2 ).add( &bqNotch3 );
-    Notch50.add( &bq1 ).add( &bq2 ).add( &bq3 );
-    bqcLP.add( &bqLP1 ).add( &bqLP2 );
     bqc.add( &bqNotch1 ).add( &bqNotch2 ).add( &bqNotch3 ).add( &bq1 ).add( &bq2 ).add( &bq3 ).add( &bqLP1 ).add( &bqLP2 );
-    /**Attach the 'sample' function to the timer 'sample_timer'.
-    * this ensures that 'sample' is executed every... 0.001 seconds = 1000 Hz
-    */
-    sample_timer.attach(&sample, Periode);
-    cal_timer.attach(&calibration, 0.002);//ticker to check if motor is being calibrated
-    led2 = 1;
-    /*empty loop, sample() is executed periodically*/
+    //Sample every 0.002 seconds, or 500 Hz
+    sample_timer.attach(&sample, 0.002);
+    //Ticker to check if motor is being calibrated, also at 500 Hz
+    calX_timer.attach(&calibrationX, 0.002);
+    calY_timer.attach(&calibrationY, 0.002);
+    //Turn LED off, turned on when calibrating
+    led2 = 1;      
     
+    turning.rise(&NegPosChanger);                         
+    
+    //MODSERIAL connection
+    pc.baud(115200);
+    
+    
+    while (true)
+    {
 
-    DigitalOut myled(LED1);
- 
-//--- 
-    
-    while(true) {
-    qdot.print();                    //Alleen visualisatie
-    wait(0.5);  
+        if (buttonCal.read() == false){   //when calibrate button is pressed
+            states CurrentState1 = calibEMG;
+        }  
+        
+        if (!ServoButton) {
+            Change();
+            wait(0.3);
+        }
+        
+        
+        /*Control end-effector
+        wait(0.1);
+        float v_ref = GetReferenceVelocity();
+        setMotor(v_ref);
+        pc.printf("%f \r\n", FeedForwardControl(v_ref));
+        UpperMotorDir.write(motorDirection);
+        UpperMotorVel.write(RightServoVel);   //PWM Speed Control
+        */
     }
-
-}
\ No newline at end of file
+}  
\ No newline at end of file