Diff: main.cpp

Revision:

20:14edaecd7413

Parent:

19:591572f4e4b5

Child:

21:9307dc9be4f7

Child:

24:847321a23e60

--- a/main.cpp	Fri Nov 03 02:10:11 2017 +0000
+++ b/main.cpp	Fri Nov 03 02:54:09 2017 +0000
@@ -38,7 +38,7 @@
 MODSERIAL pc(USBTX,USBRX);
 
 double PwmPeriod = 1.0/5000.0;           //set up of PWM periode (5000 Hz, want 5000 periodes in 1 seconde)
-const double Ts = 0.1;                   // tickettijd/ sample time
+const double Ts = 0.002f;                   // tickettijd/ sample time
 double e_prev = 0; 
 double e_int = 0;
 double e_prev2 = 0;
@@ -229,13 +229,15 @@
 
 void CalibrationEMG()
 {
+    pc.printf("Timescalibration = %i \r\n",Timescalibration);
     Timescalibration++;
     
     if(Timescalibration<2000)
     {
+        pc.printf("calibratie rust EMG \r\n");
         led = 1;
-        wait(0.2);
-        led = 0;
+       // wait(0.2);
+        //led = 0;
         
     emgNotchLB = NFLB.step(emgLB.read() );
     emgHPLB = HPFLB.step(emgNotchLB);   
@@ -263,12 +265,13 @@
     }
     if(Timescalibration==1999)
     {
-        led = 1;
+        /*led = 1;
         wait(0.2);
         led = 0; 
         wait(0.2);
         led = 1;
         wait(0.2);
+        */
         led = 0;
         RESTMEANLB = emgSUMLB/Timescalibration; //determine the mean rest value
         RESTMEANRB = emgSUMRB/Timescalibration; //determine the mean rest value
@@ -277,8 +280,9 @@
     }
     if(Timescalibration>2000 && Timescalibration<6000)
     {
+        pc.printf("maximum linker biceps \r\n");
         led = 1;
-        wait(0.2);
+        /*wait(0.2);
         led = 0;
         wait(0.2);
         led = 1;
@@ -288,6 +292,7 @@
         led = 1;
         wait(0.2);
         led = 0;
+        */
     emgNotchLB = NFLB.step(emgLB.read() ); 
     emgHPLB = HPFLB.step(emgNotchLB);        
     emgAbsHPLB = abs(emgHPLB);          
@@ -301,7 +306,8 @@
     
     if(Timescalibration>6000 && Timescalibration<10000)
     {
-        led = 1;
+        pc.printf(" maximum rechter biceps \r\n");
+        /*led = 1;
         wait(0.2);
         led = 0;
         wait(0.2);
@@ -315,6 +321,7 @@
         wait(0.2);
         led = 1;
         wait(0.2);
+        */
         led = 0;
     emgNotchRB = NFRB.step(emgRB.read()); 
     emgHPRB = HPFRB.step(emgNotchRB); 
@@ -329,8 +336,9 @@
     
     if(Timescalibration>10000 && Timescalibration<14000)
     {
+        pc.printf("maximum linker triceps \r\n");
         led = 1;
-        wait(0.2);
+        /*wait(0.2);
         led = 0;
         wait(0.2);
         led = 1;
@@ -348,6 +356,7 @@
         led = 1;
         wait(0.2);
         led = 0;
+        */
     emgNotchLT = NFLT.step(emgLT.read() );
     emgHPLT = HPFLT.step(emgNotchLT);
     emgAbsHPLT = abs(emgHPLT);
@@ -361,6 +370,7 @@
     
     if(Timescalibration>14000 && Timescalibration<18000)
     {
+        pc.printf("maximum rechter triceps");
     emgNotchRT = NFRT.step(emgRT.read() );  
     emgHPRT = HPFRT.step(emgNotchRT);       
     emgAbsHPRT = abs(emgHPRT);           
@@ -374,7 +384,8 @@
     
     if(Timescalibration>18000)
     {
-         caldone=true; 
+         pc.printf("calibratie afgelopen");
+         State = SelectDevice; 
     }
    // pc.printf("maxi waarde = %f emgfinal = %f \r\n",maxi,emgfinal);
    //}
@@ -532,6 +543,7 @@
     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 changePosition ()    // DIT MOET NOG HEEL ERG GETUNED WORDEN !!!
@@ -553,8 +565,10 @@
 
 void Loop_funtion()
 {   
+    pc.printf("state machine begint \r\n");
     switch(State){
         case Cal1: //Calibration motor 1
+        pc.printf("cal1 \r\n");
         State=Cal2;
         // naar achteren bewegen( als voorbeeld Arvid), daarna deze waarde opslaan als offset. Dan bewegen naar home middels PID en verschil encodervalue uiterste stand en home1.
            /* motorValue1 = 0.1f; motorValue2=0;
@@ -575,6 +589,7 @@
             break;
             
         case Cal2: //Calibration motor 2
+        State = CalEMG;
                /*  if (Huidigepositie2== 0)
                  {
                        if (Huidigepositie2<0.01){
@@ -587,7 +602,6 @@
             break;
         case CalEMG: // Calibration EMG
             CalibrationEMG();   //calculates average EMGFiltered at rest and measures max signal EMGFiltered.
-            State=SelectDevice;
             break;    
         case SelectDevice: //Looks at the difference between current position and home. Select aansturen EMG or buttons
             if (button==1) {
@@ -598,6 +612,7 @@
             }
             break;
         case EMG: //Aansturen met EMG
+        pc.printf("EMG begint met aansturen \r\n");
             Filteren();
             changePosition();
             //RKI --> output refP van motor
@@ -644,9 +659,10 @@
     
     //voor serial
     pc.baud(115200);
+    pc.printf("begint met programma \r\n");
     
     //motor
-   // M1E.period(PwmPeriod); //set PWMposition at 5000hz
+   M1E.period(PwmPeriod); //set PWMposition at 5000hz
     //Ticker
     //Treecko.attach(MeasureAndControl, tijdstap);   //Elke 1 seconde zorgt de ticker voor het runnen en uitlezen van de verschillende 
                                             //functies en analoge signalen. Veranderingen worden elke 1 seconde doorgevoerd.
@@ -655,7 +671,7 @@
     //State Machine
     State = Cal1;
     Position_controller_on = false;
-    Treecko.attach(&Loop_funtion, looptime);
+    Treecko.attach(&Loop_funtion, Ts);
     while(true)
     {   }