Diff: main.cpp

Revision:

48:d6894723d9bf

Parent:

47:9b1f09b4d15c

Child:

49:e28c752d826f

--- a/main.cpp	Tue Nov 01 16:43:18 2016 +0000
+++ b/main.cpp	Wed Nov 02 09:42:14 2016 +0000
@@ -20,12 +20,14 @@
     //callibration buttons
     DigitalIn   button_calibration_biceps  (SW3);                //button to start calibration biceps
     DigitalIn   button_calibration_triceps (SW2);               // button to start calibration tricps
-
+    DigitalIn   buttontest                  (D9);
+    DigitalIn   buttontest2                 (PTC12);    
     //tickers
     Ticker      sample_timer;               //ticker
     Ticker      switch_function;            //ticker
     Ticker      ticker_calibration_biceps;
     Ticker      ticker_calibration_triceps;
+    Ticker      encoder_ticker;
 
 
 //everything for monitoring
@@ -36,9 +38,9 @@
     DigitalOut  blue(LED_BLUE);
  
 //motors
-    DigitalOut richting_motor1(D4);
+    DigitalOut direction_motor1(D4);
     PwmOut pwm_motor1(D5);
-    DigitalOut richting_motor2(D7);
+    DigitalOut direction_motor2(D7);
     PwmOut pwm_motor2(D6);
 
 //define variables
@@ -46,8 +48,8 @@
      //for motorcontrol
         const int cw = 0;                   // motor should turn clockwise
         const int ccw =1;                   // motor should turn counterclockwise
-        const float gearboxratio=131.25;    // gearboxratio from encoder to motor
-        const float rev_rond=64.0;          // revolutions per round of encoder
+        const double gearboxratio=131.25;    // gearboxratio from encoder to motor
+        const double rev_rond=64.0;          // revolutions per round of encoder
         int    onoffsignal_biceps=0;        // on/off signal: 1; biceps activation, 0: nothing, -1, triceps activation
         int    switch_signal_triceps=0;     // switching between motors. 
         
@@ -59,10 +61,12 @@
         int motorswitch=0;
         
         //encoders
-         double rev_counts_motor1=0;
-         double rev_counts_motor2=0;
-         double counts_encoder1;
-         double counts_encoder2;
+         volatile double rev_counts_motor1=0;
+         volatile double rev_counts_motor2=0;
+         volatile double counts_encoder1;
+         volatile double counts_encoder2;
+         QEI Encoder1(D12,D13, NC, rev_rond,QEI::X4_ENCODING);  // maakt een encoder aan! D12/D13 ingangen, rev_rond zijn aantal pulsen per revolutie! Bovenaan in te stellen. 
+         QEI Encoder2(D10,D11, NC, rev_rond,QEI::X4_ENCODING);
         
         //variables and constants for calibration
         const float percentage_max_triceps=0.3;
@@ -112,6 +116,12 @@
     BiQuad filterlow_b2 (6.2942e-06, 1.2588e-05,6.2942e-06,-1.9929e+00,9.9292e-01);
 
 //function teller
+void encoder_working(){
+       counts_encoder1 = Encoder1.getPulses(); 
+       rev_counts_motor1=counts_encoder1/(gearboxratio*rev_rond);
+       counts_encoder2 = Encoder2.getPulses(); 
+       rev_counts_motor2=counts_encoder2/(gearboxratio*rev_rond);  }
+
 void SwitchN() {                        // maakt simpele functie die 1 bij n optelt
     if(switch_signal_triceps==1)
     {
@@ -281,13 +291,12 @@
 int main()
 {  
 pc.baud(115200); //connect with pc with baudrate 115200
-QEI Encoder1(D12,D13, NC, rev_rond,QEI::X4_ENCODING);  // maakt een encoder aan! D12/D13 ingangen, rev_rond zijn aantal pulsen per revolutie! Bovenaan in te stellen. 
-QEI Encoder2(D10,D11, NC, rev_rond,QEI::X4_ENCODING);
 
 sample_timer.attach(&filter, 0.001);                    //continously execute the EMG reader and filter, it ensures that filter and sampling is executed every 1/frequency seconds
 switch_function.attach(&SwitchN,1.0);                   //switch is every second available
 ticker_calibration_biceps.attach (&calibration_biceps,2.0);   //to call calibration biceps, stop everything else
 ticker_calibration_triceps.attach(&calibration_triceps,2.0);  //to call calibration triceps, stop everything else
+encoder_ticker.attach(&encoder_working,0.0001);
 
   if (motorswitch%2==0) {
         pc.printf("If you contract the right arm, the robot will go right \r\n");
@@ -312,45 +321,40 @@
 
     while (true) {                        // zorgt er voor dat de code oneindig doorgelopen wordt  
     
-        //encoder aan
-            counts_encoder1 = Encoder1.getPulses(); 
-            rev_counts_motor1=counts_encoder1/(gearboxratio*rev_rond);
-            counts_encoder2 = Encoder2.getPulses(); 
-            rev_counts_motor2=counts_encoder2/(gearboxratio*rev_rond);  
                   
     //motor control with muscles.
-    if (onoffsignal_biceps==-1)                           // als s ingedrukt wordt gebeurd het volgende
+    if (buttontest==0)                           // als s ingedrukt wordt gebeurd het volgende
     {
-         if (motorswitch%2==0)                     // als s ingedrukt wordt en het getal is even gebeurd het onderstaande
+         if (motorswitch%2==0 && rev_counts_motor1>-3.4)                     // als s ingedrukt wordt en het getal is even gebeurd het onderstaande
          { 
-           richting_motor1 = ccw;
-           pwm_motor1 = 0.5; 
+           direction_motor1 = ccw; //right
+           pwm_motor1 = 0.18; 
            pc.printf("ccw m1\r\n");
            
          } 
          
-         else                           // als s is ingedrukt maar het getal is niet even (dus oneven) gebeurdt het onderstaande
+         else if (motorswitch%2!=0 && rev_counts_motor2>-10.0)                        // als s is ingedrukt maar het getal is niet even (dus oneven) gebeurdt het onderstaande
          {
-           richting_motor2 = ccw;
+           direction_motor2 = ccw;  //down
            pwm_motor2 = 1;
           
          }      
               
     }
-    else if (onoffsignal_biceps==1)                     // als d ingedrukt wordt gebeurd het volgende
+    else if (buttontest2==0)                     
     {
-         if (motorswitch%2==0)                     // als d is ingedrukt en n is even dan gebeurd het volgende
+         if (motorswitch%2==0 && rev_counts_motor1<3.4)    //restriction
          {
-           richting_motor1 = cw;
+           direction_motor1 = cw; //left
            pwm_motor1 = 0.5;
            pc.printf("cw 1 aan\r\n");
            
     
            
         } 
-         else                           // als d is ingedrukt maar het getal is niet even (dus oneven) gebeurt het onderstaande
+         else if(motorswitch%2!=0 && rev_counts_motor2<19.0)                          // als d is ingedrukt maar het getal is niet even (dus oneven) gebeurt het onderstaande
          {
-           richting_motor2 = cw;
+           direction_motor2 = cw;
            pwm_motor2 = 1;
               
          }  
@@ -361,7 +365,6 @@
     pwm_motor1=0;
              
        }              
-    pc.printf("rev_counts_motor1= %f \r\n",rev_counts_motor1);
-    pc.printf("counts_encoder 1= %f \r\n",counts_encoder1);          
+    pc.printf("rev_counts_motor1= %f \r\n",rev_counts_motor1);          
 }
  }       
\ No newline at end of file