Diff: main.cpp

Revision:

51:b344a92b6a5f

Parent:

50:2c03357de7cc

Child:

52:0135deb3b07f

--- a/main.cpp	Thu Nov 03 09:55:11 2016 +0000
+++ b/main.cpp	Thu Nov 03 10:37:29 2016 +0000
@@ -68,7 +68,8 @@
 //int counts_encoder2;
 float rev_counts_motor1;
 float rev_counts_motor1_rad;
-
+const float gearboxratio=131.25;    // gearboxratio van encoder naar motor
+const float rev_rond=64.0;          // aantal revoluties per omgang van de encoder
 
 //reference
 volatile float d_ref = 0;
@@ -192,7 +193,7 @@
                 }
                 
 void reference(){
-    if (button_cw == 0){
+    if (onoffsignal_biceps==1){                     //right biceps contracted{
          d_ref = d_ref + w_ref * Ts;
     }
          if (d_ref > 21.36 ){
@@ -203,7 +204,7 @@
         d_ref = d_ref;
     }
     
-    if (button_ccw == 0){
+    if (onoffsignal_biceps==-1){                    //left biceps contracted{
         d_ref = d_ref - w_ref * Ts;
     }
         if (d_ref < -21.36){
@@ -218,7 +219,7 @@
 
 void m1_controller(){
     error1 = d_ref-rev_counts_motor1_rad;
-    ctrlOutput = PID_controller.step(error1);
+    controlOutput = PID_controller.step(error1);
 }
 
 //======================================================================   
@@ -240,6 +241,9 @@
 //PID controller
 PID_controller.PIDF(Kp,Ki,Kd,N,Ts);
 
+//Encoder
+QEI Encoder1(D13,D12, NC, rev_rond,QEI::X4_ENCODING);
+
 //Show the user what the starting motor will be and what will happen
 pc.printf("We will start the demonstration\r\n");
 pc.printf("\r\n\r\n\r\n");
@@ -263,14 +267,29 @@
 
     while (true) {                        // neverending loop
         
+counts_encoder1 = Encoder1.getPulses();
+rev_counts_motor1=(float)counts_encoder1/(gearboxratio*rev_rond); 
+rev_counts_motor1_rad=rev_counts_motor1*6.28318530718; 
+pc.printf("%f", pwm_motor1.read());
+pc.printf("   %f", d_ref);
+pc.printf("   %f \r\n", rev_counts_motor1_rad);
+
     if (onoffsignal_biceps==-1)  //left biceps contracted                        
     {
          if (switch_signal%2==0) //switch even                    
          { 
-           richting_motor1 = ccw;    //motor 1, left
-           pwm_motor1 = speedmotor1; //speed of motor 1
-                  
+            speedmotor1=controlOutput;
+            //richting_motor1 = ccw;    //motor 1, left
+            //pwm_motor1 = speedmotor1; //speed of motor 1
+            if (speedmotor1<0){
+                richting_motor1 = 0;
+                }
+            else   {
+                richting_motor1 = 1;
+                }
+                pwm_motor1 = fabs(speedmotor1);      //speed of motor 1
          } 
+            
          
          else                       //switch odd        
          {
@@ -284,8 +303,16 @@
     {
          if (switch_signal%2==0)                     //switch signal even
          {
-           richting_motor1 = cw;        //motor 1, right
-           pwm_motor1 = speedmotor1;    //speed motor 1
+           speedmotor1=controlOutput;
+            //richting_motor1 = ccw;    //motor 1, left
+            //pwm_motor1 = speedmotor1; //speed of motor 1
+            if (speedmotor1<0){
+                richting_motor1 = 0;
+                }
+            else   {
+                richting_motor1 = 1;
+                }
+                pwm_motor1 = fabs(speedmotor1);      //speed of motor 1
            
         } 
          else                         //switch signal odd