Diff: main.cpp

Revision:

55:d742332ced11

Parent:

54:fb72c58a7150

Child:

56:a38412383477

--- a/main.cpp	Thu Nov 03 12:18:53 2016 +0000
+++ b/main.cpp	Thu Nov 03 15:27:59 2016 +0000
@@ -20,6 +20,10 @@
 Ticker      ticker_switch;              //ticker for switch, every second it is possible to switch
 Ticker      ticker_referenceangle;      //ticker for the reference angle
 Ticker      ticker_controllerm1;        //ticker for the controller (PID) of motor 1
+Ticker      ticker_encoder;
+
+//Timer
+Timer       timer;
 
 //Monitoring
 HIDScope    scope(5);                   //open 5 channels in hidscope
@@ -70,10 +74,13 @@
 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
+QEI Encoder1(D13,D12,NC,rev_rond,QEI::X4_ENCODING);
 
 //reference
 volatile float d_ref = 0;
-const float w_ref = 3;
+const float w_ref = 1.5;
+volatile double t_start;
+volatile double w_var;
 const double Ts = 0.001;
 
 //controller
@@ -83,6 +90,8 @@
 const double N = 100;
 volatile double error1;
 volatile double controlOutput; 
+bool start_motor = true;
+volatile double starttime;
 //=======================================
 //filter coefficients
 
@@ -193,24 +202,50 @@
                 }
                 
 void reference(){
-    if (onoffsignal_biceps==-1 && switch_signal%2==0){ //switch even       //right biceps contracted{
-         d_ref = d_ref + w_ref * Ts;
+    if (start_motor == true){
+        timer.start();
     }
-         if (d_ref > 8){
-            d_ref = 8;
+    if (onoffsignal_biceps==-1 && switch_signal%2==0){ //switch even      
+         t_start = timer.read_ms();
+         start_motor = false;
+         
+         if (t_start < 1.0){
+             w_var = t_start*1.5;
+            }
+            
+        else {
+         w_var = 1.5;
+         }
+         
+         d_ref = d_ref + w_var * Ts;
+         
+    }
+         if (d_ref > 12){
+            d_ref = 12;
+            start_motor = true;
             //d_ref_const_cw = 1;
-        }
+         }
     else{ 
         d_ref = d_ref;
     }
     
     if (onoffsignal_biceps==1 && switch_signal%2==0){ //switch even    //left biceps contracted{
-        d_ref = d_ref - w_ref * Ts;
-    }
-        if (d_ref < -8){
-        d_ref = -8;
+        t_start = timer.read_ms();
+        start_motor = false;
+        
+        if (t_start < 1.0){
+            w_var = t_start*1.5;
+            }
+        else {
+            w_var = 1.5;
+            }
+        d_ref = d_ref - w_var * Ts;
         
     }
+        if (d_ref < -12){
+        d_ref = -12;
+        start_motor = true;
+        }
     else{
         d_ref = d_ref;
     }
@@ -222,6 +257,13 @@
     controlOutput = PID_controller.step(error1);
 }
 
+void encoders(){
+    counts_encoder1 = Encoder1.getPulses();
+    rev_counts_motor1 = (float)counts_encoder1/(gearboxratio*rev_rond);
+    rev_counts_motor1_rad = rev_counts_motor1*6.28318530718;
+    
+}
+
 //======================================================================   
 //program
 //======================================================================   
@@ -237,12 +279,13 @@
 ticker_switch.attach(&switch_function,1.0);
 ticker_referenceangle.attach(&reference, Ts);
 ticker_controllerm1.attach(&m1_controller, Ts);
+ticker_encoder.attach(&encoders, Ts);
 
 //PID controller
 PID_controller.PIDF(Kp,Ki,Kd,N,Ts);
 
 //Encoder
-QEI Encoder1(D13,D12, NC, rev_rond,QEI::X4_ENCODING);
+//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");
@@ -272,7 +315,10 @@
 rev_counts_motor1_rad=rev_counts_motor1*6.28318530718; 
 
 pc.printf("%f \r\n", d_ref);
-pc.printf("%f ", rev_counts_motor1_rad);
+//pc.printf("%f ", rev_counts_motor1_rad);
+//pc.printf("%f",w_var);
+pc.printf("%d\n",start_motor);
+
 
     if (onoffsignal_biceps==-1)  //left biceps contracted                        
     {
@@ -327,6 +373,7 @@
     //no contraction of biceps   
     pwm_motor2=0;
     pwm_motor1=0;
+    start_motor = true;
        }              
                
 }//while true closed