Diff: main.cpp

Revision:

32:30a36362f23d

Parent:

31:a636a8f590a6

Child:

33:4e5aca9f73e6

--- a/main.cpp	Tue Nov 07 21:28:10 2017 +0000
+++ b/main.cpp	Tue Nov 07 21:37:13 2017 +0000
@@ -49,7 +49,7 @@
 MODSERIAL pc(USBTX,USBRX);
 
 //PID
-const double Ts = 0.002f;                   // tickettijd/ sample time
+const double Ts = 0.002f;                   // tickertime/ sample time
 double e_prev = 0; 
 double e_int = 0;
 double e_prev2 = 0;
@@ -312,7 +312,7 @@
     refP2 = (( q1 + q2)/(2*pi));    //Get reference positions
 }
 
-void SetpointRobot()
+void SetpointRobot()                    // Get position from Potmeters
 {    
     double Potmeterwaarde2 = potMeter2.read();
     double Potmeterwaarde1 = potMeter1.read();
@@ -336,10 +336,10 @@
     }
 }
 
-void changePosition ()    // DIT MOET NOG HEEL ERG GETUNED WORDEN !!!
+void changePosition ()  // Get position from EMG signals
 {
     if (RBF>0.5) {
-        Rsx +=0.001;    // hoe veel verder gaat hij? 1 cm? 10 cm?
+        Rsx +=0.001;    // increases 1 mm
     }
     else {}
     if (RTF>0.5) {
@@ -356,10 +356,10 @@
     else {}
 }
 
-double FeedBackControl(double error, double &e_prev, double &e_int)   // schaalt de snelheid naar de snelheid zodat onze chip het begrijpt (is nog niet in werking)
+double FeedBackControl(double error, double &e_prev, double &e_int)   // PID controller motor 1
 {
     double kp = 0.0008*4200;                               // kind of scaled.
-    double Proportional= kp*;
+    double Proportional= kp*error;
     
     double kd = 0.0008*4200;                           // kind of scaled. 
     double VelocityError = (error - e_prev)/Ts; 
@@ -374,7 +374,7 @@
     return motorValue;
 }
 
-double FeedBackControl2(double error2, double &e_prev2, double &e_int2)   // schaalt de snelheid naar de snelheid zodat onze chip het begrijpt (is nog niet in werking)
+double FeedBackControl2(double error2, double &e_prev2, double &e_int2)   // PID controller motor 2
 {
     double kp2 = 0.0008*4200;                             // kind of scaled.
     double Proportional2= kp2*error2;
@@ -447,12 +447,12 @@
         case CalEMG: // Calibration EMG
             CalibrationEMG();   //calculates average EMGFiltered at rest and measures max signal EMGFiltered.
             break;    
-        case SelectDevice: //Looks at the difference between current position and home. Select aansturen EMG or buttons
+        case SelectDevice:      //Looks at the difference between current position and home. Select aansturen EMG or buttons
             State = EMG;
             if (button==1) {
                 State=EMG;
             }
-            else {           // if (button==0)
+            else {           // button==0
                 State=Demonstration;
             }
             break;
@@ -466,14 +466,24 @@
             else {}
             break;
         case Rest: // When it is not your turn, the robot shouldn't react on muscle contractions.
-            error=0;
-            error2=0;
+            refP=refP;
+            refP2=refP2;
+            double Huidigepositie = motor1.getPosition()/4200; 
+            double error = (refP - Huidigepositie);// make an error
+            double motorValue = FeedBackControl(error, e_prev, e_int);
+            SetMotor1(motorValue);
+            // control of 2nd motor
+            double Huidigepositie2 = motor2.getPosition()/4200; 
+            double error2 = (refP2 - Huidigepositie2);// make an error
+            double motorValue2 = FeedBackControl2(error2, e_prev2, e_int2);
+            SetMotor2(motorValue2);
+            
             if ( button==1) {
                 State=EMG;
             }
             else {}
             break;
-        case Demonstration: // Control with potmeters
+        case Demonstration: // Control with Potmeters
             SetpointRobot();
             MeasureAndControl();
             break;
@@ -482,7 +492,7 @@
 
 int main()
 {
-    //voor EMG filteren
+    //for filtering EMG
     //Left Biceps
     NFLB.add( &N1LB );
     HPFLB.add( &HP1LB ).add( &HP2LB );