Diff: main.cpp

Revision:

31:a636a8f590a6

Parent:

30:b76c27e4730c

Child:

32:30a36362f23d

--- a/main.cpp	Tue Nov 07 08:54:37 2017 +0000
+++ b/main.cpp	Tue Nov 07 21:28:10 2017 +0000
@@ -9,7 +9,6 @@
 //State Machine and calibration
 enum States {CalEMG, SelectDevice, EMG, Rest, Demonstration};
 States State;
-bool Position_controller_on;
 DigitalIn button (D1);
 
 //Buttons  en leds voor calibration
@@ -309,9 +308,8 @@
     q1 = q1 + w1*Tijd;
     q2 = q2 + w2*Tijd;
     
-    int maxwaarde = 4096;                   // = 64x64
-    refP = (((0.5*pi) - q1)/(2*pi))*maxwaarde;
-    refP2 = (( q1 + q2)/(2*pi))*maxwaarde;    //Get reference positions
+    refP = (((0.5*pi) - q1)/(2*pi));
+    refP2 = (( q1 + q2)/(2*pi));    //Get reference positions
 }
 
 void SetpointRobot()
@@ -360,33 +358,33 @@
 
 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 kp = 0.0015;                               // kind of scaled.
-    double Proportional= kp*error;
+    double kp = 0.0008*4200;                               // kind of scaled.
+    double Proportional= kp*;
     
-    double kd = 0.000008;                           // kind of scaled. 
+    double kd = 0.0008*4200;                           // kind of scaled. 
     double VelocityError = (error - e_prev)/Ts; 
     double Derivative = kd*VelocityError;
     e_prev = error;
     
-    double ki = 0.0001;                           // kind of scaled.
+    double ki = 0.001*4200;                           // kind of scaled.
     e_int = e_int+Ts*error;
     double Integrator = ki*e_int;
     
-    double motorValue = Proportional + Integrator + Derivative;
+    double motorValue = (Proportional + Integrator + Derivative)/4200;
     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 kp2 = 0.002;                             // kind of scaled.
+    double kp2 = 0.0008*4200;                             // kind of scaled.
     double Proportional2= kp2*error2;
     
-    double kd2 = 0.000008;                           // kind of scaled. 
+    double kd2 = 0.0008*4200;                           // kind of scaled. 
     double VelocityError2 = (error2 - e_prev2)/Ts; 
     double Derivative2 = kd2*VelocityError2;
     e_prev2 = error2;
     
-    double ki2 = 0.00005;                           // kind of scaled.
+    double ki2 = 0.00005*4200;                           // kind of scaled.
     e_int2 = e_int2+Ts*error2;
     double Integrator2 = ki2*e_int2;
     
@@ -430,16 +428,15 @@
 {
     RKI();
     // control of 1st motor
-    double Huidigepositie = motor1.getPosition(); 
+    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(); 
+    double Huidigepositie2 = motor2.getPosition()/4200; 
     double error2 = (refP2 - Huidigepositie2);// make an error
     double motorValue2 = FeedBackControl2(error2, e_prev2, e_int2);
     SetMotor2(motorValue2);
-    pc.printf("refP = %f, huidigepos = %f, motorvalue = %f, refP2 = %f, huidigepos2 = %f, motorvalue2 = %f \r\n", refP, Huidigepositie, motorValue, refP2, Huidigepositie2, Huidigepositie2);
 }
 
 void Loop_funtion()
@@ -455,7 +452,7 @@
             if (button==1) {
                 State=EMG;
             }
-            else {           // if (button==0) {
+            else {           // if (button==0)
                 State=Demonstration;
             }
             break;
@@ -463,8 +460,19 @@
             Filteren();
             changePosition();
             MeasureAndControl();
+            if (button==0) {
+                State=Rest;
+            }
+            else {}
             break;
         case Rest: // When it is not your turn, the robot shouldn't react on muscle contractions.
+            error=0;
+            error2=0;
+            if ( button==1) {
+                State=EMG;
+            }
+            else {}
+            break;
         case Demonstration: // Control with potmeters
             SetpointRobot();
             MeasureAndControl();
@@ -472,46 +480,39 @@
         }
 } 
 
-int main()//deze moet ooit nog weg --> pas op voor errors
+int main()
 {
     //voor EMG filteren
-    //Left Bicep
+    //Left Biceps
     NFLB.add( &N1LB );
     HPFLB.add( &HP1LB ).add( &HP2LB );
     LPFLB.add( &LP1LB ).add( &LP2LB );
     
-    //Right Bicep
+    //Right Biceps
     NFRB.add( &N1RB );
     HPFRB.add( &HP1RB ).add( &HP2RB );
     LPFRB.add( &LP1RB ).add( &LP2RB );  
     
-    //Left Tricep
+    //Left Triceps
     NFLT.add( &N1LT );
     HPFLT.add( &HP1LT ).add( &HP2LT );
     LPFLT.add( &LP1LT ).add( &LP2LT ); 
     
-    //Right Tricep
+    //Right Triceps
     NFRT.add( &N1RT );
     HPFRT.add( &HP1RT ).add( &HP2RT );
     LPFRT.add( &LP1RT ).add( &LP2RT ); 
     
-    //voor serial
+    // serial
     pc.baud(115200);
-    pc.printf("begint met programma \r\n");
     
     //motor
-   M1E.period(PwmPeriod); //set PWMposition at 5000hz
-    //Ticker
-    //Treecko.attach(MeasureAndControl, Ts);   //Elke 1 seconde zorgt de ticker voor het runnen en uitlezen van de verschillende 
-                                            //functies en analoge signalen. Veranderingen worden elke 1 seconde doorgevoerd.
-  //  printer.attach(Tickerfunctie,0.4);
-    
+    M1E.period(PwmPeriod); //set PWMposition at 5000hz
+   
     //State Machine
     State = CalEMG;
-    Position_controller_on = false;
     Treecko.attach(&Loop_funtion, Ts);
     while(true)
-    {   }
-    
- //is deze wel nodig?  
+    {}
+
  }