Revision 30:a9fdd3202ca2, committed 2015-10-21

Comitter:

Vigilance88

Date:

Wed Oct 21 10:30:03 2015 +0000

Parent:

29:948b0b14f6be

Child:

31:7b8b8459bddc

Commit message:

added emg reference in comments

Changed in this revision

--- a/main.cpp	Wed Oct 21 09:19:23 2015 +0000
+++ b/main.cpp	Wed Oct 21 10:30:03 2015 +0000
@@ -229,7 +229,7 @@
 int main()
 {
     pc.baud(115200);            //serial baudrate
-    red=1; green=1; blue=1;     //Make sure debug LEDS are off  
+    red=1; green=1; blue=1;     //Make sure debug LEDs are off  
     
     //Set PwmOut frequency to 10k Hz
     //pwm_motor1.period(PWM_PERIOD);    
@@ -245,8 +245,9 @@
     
     //calibrate_emg();        
     
+    //set initial reference position
+    x=0.5; y=0;
     
-    x=0.5; y=0;
     //start_control();        //100 Hz control
     
     //maybe some stop commands when a button is pressed: detach from timers.
@@ -414,7 +415,7 @@
     //end if
     }
     //end of while loop
-    }
+}
 
 //Sample and Filter  
 void sample_filter(void)
@@ -455,32 +456,6 @@
     
 }
 
-void controlMenu()
-{
-         //Title Box
-   pc.putc(201); 
-   for(int j=0;j<33;j++){
-   pc.putc(205);
-   }
-   pc.putc(187); 
-   pc.printf("\n\r");
-   pc.putc(186); pc.printf("          Control Menu           "); pc.putc(186);
-   pc.printf("\n\r");
-   pc.putc(200);
-   for(int k=0;k<33;k++){
-   pc.putc(205);
-   }
-   pc.putc(188); 
-   
-   pc.printf("\n\r");
-   //endbox
-     pc.printf("1) Move Arm Left\r\n");
-     pc.printf("2) Move Arm Right\r\n");
-     pc.printf("3) Move Arm Up\r\n");
-     pc.printf("4) Move Arm Down\r\n");
-     pc.printf("q) Exit \r\n");
-     pc.printf("Please make a choice => \r\n");
-}
 
 //Send arm to mechanical limits, and set encoder to 0. Then send arm to starting position.
 void calibrate_arm(void)
@@ -680,10 +655,74 @@
 //Analyze filtered EMG, calculate reference position from EMG, compare reference position with current position,convert to angles, send error through pid(), send PWM and DIR to motors 
 void control()
 {
-    //analyze emg (= velocity, averages?)
+   /* 
+   
+    //normalize emg to value between 0-1
+    biceps = (biceps - bicepsmin) / (bicepsmvc - bicepsmin)
+    biceps = (biceps - bicepsmin) / (bicepsmvc - bicepsmin)
+    biceps = (biceps - bicepsmin) / (bicepsmvc - bicepsmin)
+    biceps = (biceps - bicepsmin) / (bicepsmvc - bicepsmin)
+    
+    //analyze emg (= velocity)
+    if (biceps>triceps && biceps > 0.1)
+        xdir = 0;
+        xpower = biceps;
+    else if (triceps>biceps && triceps>0.1)
+        xdir = 1;
+        xpower = triceps;
+    else 
+        xpower=0;
+        
+    if (flexor>extensor && flexor > 0.1){
+        ydir = 0;
+        ypower = flexor;
+        }
+    else if (extensor>flexor && extensor > 0.1){
+        ydir = 1;
+        ypower = extensor;
+        }
+    else
+        ypower = 0;
+            
+    //We have power: the longer a signal is active, the further you go. So integrate to determine reference position
+    dx = xpower * CONTROL_RATE;
+    dy = ypower * CONTROL_RATE; 
     
-    //calculate reference position based on the average emg (integrate)
+    //But: direction! Sum dx and dy but make sure xdir and ydir are considered.
+    if (xdir>0)
+        x += dx;
+    else 
+        x += -dx;
+        
+    if (ydir>0)
+        y += dy;
+    else
+        y += -dy;
+        
+   //now we have x and y -> reference position.     
+   
+   //Set limits to the error! 
+   //lower limit:   Negative error not allowed to go further.
+   if (theta1 < limitangle)
+        if (error1 > 0)
+            error1 = error1;
+        else 
+            error1 = 0; 
+   if (theta2 < limitangle)
+    same as above
+   
+   //upper limit: Positive error not allowed to go further
+   if (theta1 > limitangle)
+        if (error1 < 0 )
+            error1 = error1;
+        else
+            error1 = 0;
+   if (theta2 > limitangle)
+    same as above
     
+             
+    */
+        
     //Current position - Encoder counts -> current angle -> Forward Kinematics 
     rpc=(2*PI)/ENCODER1_CPR;               //radians per count (resolution) - 2pi divided by 4200
     theta1 = Encoder1.getPulses() * rpc;   //multiply resolution with number of counts
@@ -691,40 +730,30 @@
     current_x = l1 * cos(theta1) + l2 * cos(theta1 + theta2);
     current_y = l1 * sin(theta1) + l2 * sin(theta1 + theta2);
     
-    //pc.printf("Calculated current position: x = %f and y = %f \r\n",current_x,current_y);
-    
-    
-    //pc.printf("X is %f and Y is %f \r\n",x,y);
         
     //calculate error (refpos-currentpos) currentpos = forward kinematics
     x_error = x - current_x;
     y_error = y - current_y;
     
-    //pc.printf("X error is %f and Y error is %f \r\n",x_error,y_error);
     
     //inverse kinematics (refpos to refangle)
     
     costheta2 = (pow(x,2) + pow(y,2) - pow(l1,2) - pow(l2,2)) / (2*l1*l2) ;
-    sintheta2 = sqrt( 1 - pow(costheta2,2) );
+    sintheta2 = sqrt( abs( 1 - pow(costheta2,2) ) );
      
-    //pc.printf("costheta2 = %f and sostheta2 = %f \r\n",costheta2,sostheta2);
     
     dtheta2 = atan2(sintheta2,costheta2);
     
     costheta1 = ( x * (l1 + l2 * costheta2) + y * l2 * sintheta2 ) / ( pow(x,2) + pow(y,2) );
-    sintheta1 = sqrt( 1 - pow(costheta1,2) );
-    
-    //pc.printf("costheta1 = %f and sostheta1 = %f \r\n",costheta1,sostheta1);
+    sintheta1 = sqrt( abs( 1 - pow(costheta1,2) ) );
     
     dtheta1 = atan2(sintheta1,costheta1);
     
     
     //Angle error
-    
     m1_error = dtheta1-theta1;
     m2_error = dtheta2-theta2;
     
-    //pc.printf("m1 error is %f and m2 error is %f \r\n",m1_error,m2_error);
     
     //PID controller
     
@@ -847,6 +876,34 @@
    pc.printf("[H"); // cursor to home 
 }
 
+
+void controlMenu()
+{
+         //Title Box
+   pc.putc(201); 
+   for(int j=0;j<33;j++){
+   pc.putc(205);
+   }
+   pc.putc(187); 
+   pc.printf("\n\r");
+   pc.putc(186); pc.printf("          Control Menu           "); pc.putc(186);
+   pc.printf("\n\r");
+   pc.putc(200);
+   for(int k=0;k<33;k++){
+   pc.putc(205);
+   }
+   pc.putc(188); 
+   
+   pc.printf("\n\r");
+   //endbox
+     pc.printf("1) Move Arm Left\r\n");
+     pc.printf("2) Move Arm Right\r\n");
+     pc.printf("3) Move Arm Up\r\n");
+     pc.printf("4) Move Arm Down\r\n");
+     pc.printf("q) Exit \r\n");
+     pc.printf("Please make a choice => \r\n");
+}
+
 void caliMenu(){
      
         //Title Box