Revision 41:55face19e06b, committed 2015-10-23

Comitter:

Vigilance88

Date:

Fri Oct 23 09:14:22 2015 +0000

Parent:

40:d62f96ed44c0

Child:

42:b9d26b1218b0

Commit message:

added mechanical limits

Changed in this revision

--- a/main.cpp	Fri Oct 23 07:35:52 2015 +0000
+++ b/main.cpp	Fri Oct 23 09:14:22 2015 +0000
@@ -182,6 +182,11 @@
 double x_error; double y_error;                     //Position errors
 double lambda=0.1;                                  //Damping constant
 
+//Mechanical Limits
+int theta1_cal, theta2_cal;                         //Pulse counts at mechanical limits.  
+double theta1_lower=0.698132, theta1_upper=2.35619; //motor1: lower limit 40 degrees, upper limit 135
+double theta2_lower=0.750492, theta2_upper=2.40855; //motor2: lower limit 43 degrees, upper limit 138 degrees.
+
 /*--------------------------------------------------------------------------------------------------------------------
 ---- Filters ---------------------------------------------------------------------------------------------------------
 --------------------------------------------------------------------------------------------------------------------*/
@@ -357,6 +362,7 @@
                 pc.printf("=> You chose EMG DLS control \r\n\n");
                 wait(1);
                 start_sampling();
+                x=0; y=0;
                 wait(1);
                 emg_control=true;
                 control_method=2;
@@ -509,23 +515,24 @@
 
 
 void shoulder(){
-    pwm_motor1.write(0);
-    Encoder1.reset();
+    pwm_motor1=0;
+    //Encoder1.reset();
     done1 = true;
     pc.printf("Shoulder Limit hit - shutting down motor 1\r\n");
     //mechanical angle limits -> pulses. If 40 degrees -> counts = floor( 40 * (4200/360) )
-    //Encoder1.setPulses(467);       //edited QEI library: added setPulses()
+    theta1_cal = floor(theta1_lower * (4200/(2*PI)));
+    Encoder1.setPulses(theta1_cal);       //edited QEI library: added setPulses()
     
 }
 
 void elbow(){
-    pwm_motor2.write(0);
-    Encoder2.reset();
+    pwm_motor2=0;
+    //Encoder2.reset();
     done2 = true;
     pc.printf("Elbow Limit hit - shutting down motor 2\r\n");
-    //Mechanical limit 30 degrees -> 30*(4200/360) = 350
-    
-    //Encoder2.setPulses(350);       //edited QEI library: added setPulses()
+    //Mechanical limit 43 degrees -> 43*(4200/360) = 350
+    theta2_cal = floor(theta2_lower * (4200/(2*PI)));
+    Encoder2.setPulses(theta2_cal);       //edited QEI library: added setPulses()
 }
     
 //Send arm to mechanical limits, and set encoder to 0. Then send arm to starting position.
@@ -805,6 +812,7 @@
     //TODO some idle time with static reference before EMG kicks in
     //emg_control_time += CONTROL_RATE; 
     // if emg_control_time < 5 seconds, reference is  x=0; y=0;    
+    
     //normalize emg to value between 0-1
     biceps = (biceps_power - bicepsmin) / (bicepsMVC - bicepsmin);
     triceps = (triceps_power - tricepsmin) / (tricepsMVC - tricepsmin);
@@ -918,29 +926,37 @@
     m2_error = q2_error;
     }//end control method 2
     
-     /*  
+    /*  
    //Set limits to the error! 
-   //lower limit:   Negative error not allowed to go further.
-   if (theta1 < limitangle)
-        if (error1 > 0)
-            error1 = error1;
+   motor1: lower limit 40 degrees, upper limit 135
+   motor2: lower 43 degrees, upper limit 138 degrees. */
+   
+   //lower limits:   Negative error not allowed to go further.
+   if (theta1 < theta1_lower){
+        if (m1_error > 0)
+            m1_error = m1_error;
         else 
-            error1 = 0; 
-   if (theta2 < limitangle)
-    same as above
-   
+            m1_error = 0; }
+   if (theta2 < theta2_lower){
+        if (m2_error > 0)
+            m2_error = m2_error;
+        else 
+            m2_error = 0;
+   }
    //upper limit: Positive error not allowed to go further
-   if (theta1 > limitangle)
-        if (error1 < 0 )
-            error1 = error1;
+   if (theta1 > theta1_upper){
+        if (m1_error < 0 )
+            m1_error = m1_error;
         else
-            error1 = 0;
-   if (theta2 > limitangle)
-    same as above
-    
+            m1_error = 0;
+   }
+   if (theta2 > theta2_upper){
+        if (m2_error < 0 )
+            m2_error = m2_error;
+        else
+            m2_error = 0;
+   }
              
-    */
-    
     //PID controller
     
     u1=pid(m1_error,m1_kp,m1_ki,m1_kd,m1_e_int,m1_e_prev);    //motor 1