Revision 27:d1814e271a95, committed 2015-10-19

Comitter:

Vigilance88

Date:

Mon Oct 19 08:34:24 2015 +0000

Parent:

26:fe3a5469dd6b

Child:

28:743485bb51e4

Commit message:

added calibrate_arm & control testcode

Changed in this revision

--- a/main.cpp	Sun Oct 18 13:13:07 2015 +0000
+++ b/main.cpp	Mon Oct 19 08:34:24 2015 +0000
@@ -228,16 +228,61 @@
     //caliMenu();            // Menu function
     //calibrate_arm();        //Start Calibration
     
-    calibrate_emg();        
-    
-    //start_control();        //100 Hz control
+    //calibrate_emg();        
+    wait(3);
+    start_control();        //100 Hz control
     
     //maybe some stop commands when a button is pressed: detach from timers.
     //stop_control();
     //stop_sampling();
     
+    char c;
+    pc.printf("entering while loop \r\n");
+    
     while(1) {
-           
+    
+    if( pc.readable() ){
+        c = pc.getc();
+        switch (c) 
+            {
+            case '1' :  
+                     x = x + 0.01; 
+                     //controlMenu();
+                     //running=false;
+                     break;
+          
+            case '2' :
+                     x-=0.01;
+                     //controlMenu();
+                     //running=false;
+                     break;
+                    
+            case '3' :
+                     y+=0.01;
+                     //controlMenu();
+                     //running=false;
+                     break;
+                     
+          
+            case '4' :
+                     y-=0.01;
+                     //controlMenu();                    
+                     //running=false;
+                     break;
+                     
+            case 'q' :
+                      pc.printf("Quit");
+                      //running = false;                    
+                      break;
+    }//end switch
+    pc.printf("Reference position: %f and %f \r\n",x,y);
+    pc.printf("Current position: %f and %f \r\n",current_x,current_y);
+    pc.printf("Current angles: %f and %f \r\n",theta1,theta2);
+    pc.printf("Error in angles: %f and %f \r\n",dtheta1,dtheta2);
+    pc.printf("PID output: %f and %f \r\n",u1,u2);
+    pc.printf("----------------------------------------\r\n\n");
+    }
+    //end if
     }
     //end of while loop
 }
@@ -282,37 +327,65 @@
     
 }
 
+void controlMenu()
+{
+     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)
 {
-    red=0; blue=0;              //Debug light is purple during arm calibration
+     pc.printf("Calibrate_arm() entered\r\n");
     bool calibrating = true;
     bool done1 = false;
     bool done2 = false;
+    pc.printf("To start arm calibration, press any key =>");
+    pc.getc();
+    pc.printf("\r\n Calibrating... \r\n");
     dir_motor1=1;   //cw
-    dir_motor2=1;   //cw
-    pwm_motor1.write(0.2f);     //move upper arm slowly cw
-    pwm_motor2.write(0.2f);     //move forearm slowly cw
+    dir_motor2=0;   //cw
+    pwm_motor1.write(0.2);     //move upper arm slowly cw
+       
     
     while(calibrating){
-    
+        red=0; blue=0;              //Debug light is purple during arm calibration
+        
+        if(done1==true){
+            pwm_motor2.write(0.2);     //move forearm slowly cw
+        }
+        
         //when limit switches are hit, stop motor and reset encoder
         if(shoulder_limit.read() < 0.5){   //polling 
             pwm_motor1.write(0);
             Encoder1.reset();
             done1 = true;
+            pc.printf("Shoulder Limit hit - shutting down motor 1\r\n");
         }
         if(elbow_limit.read() < 0.5){     //polling
             pwm_motor2.write(0);
             Encoder2.reset();
             done2 = true;
+            pc.printf("Elbow Limit hit - shutting down motor 2. \r\n");
         }    
         if(done1 && done2){
             calibrating=false;      //Leave while loop when both limits are reached
             red=1; blue=1;          //Turn debug light off when calibration complete
         }
+   
+    }//end while
     
-    }//end while
+    //TO DO:
+    //mechanical angle limits -> pulses. If 40 degrees -> counts = floor( 40 / (2*pi/4200) )
+    //Encoder1.setPulses(100);       //edited QEI library: added setPulses()
+    //Encoder2.setPulses(100);       //edited QEI library: added setPulses()
+    //pc.printf("Elbow Limit hit - shutting down motor 2. Current pulsecount: %i \r\n",Encoder1.getPulses());
+    wait(1);
+    pc.printf("Arm Calibration Complete\r\n");
 
 }
 
@@ -395,7 +468,7 @@
    
    start_sampling();
    muscle=1;
-   timer.attach(&emg_mvc,0.002);
+   timer.attach(&emg_mvc,SAMPLE_RATE);
    wait(5);
    timer.detach();
   
@@ -457,24 +530,62 @@
     
     //calculate reference position based on the average emg (integrate)
     
+    //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
+    theta2 = Encoder2.getPulses() * rpc;
+    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)
     
-    //inverse kinematics (pos error to angle error)
+    costheta2 = (pow(x,2) + pow(y,2) - pow(l1,2) - pow(l2,2)) / (2*l1*l2) ;
+    sintheta2 = sqrt( 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);
+    
+    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
     
     u1=pid(m1_error,m1_kp,m1_ki,m1_kd,m1_e_int,m1_e_prev);    //motor 1
     u2=pid(m2_error,m2_kp,m2_ki,m2_kd,m2_e_int,m2_e_prev);    //motor 2
     
-    keep_in_range(&u1,-1,1);    //keep u between -1 and 1, sign = direction, PWM = 0-1
-    keep_in_range(&u2,-1,1);
+    keep_in_range(&u1,-0.6,0.6);    //keep u between -1 and 1, sign = direction, PWM = 0-1
+    keep_in_range(&u2,-0.6,0.6);
     
     //send PWM and DIR to motor 1
     dir_motor1 = u1>0 ? 1 : 0;          //conditional statement dir_motor1 = [condition] ? [if met 1] : [else 0]], same as if else below. 
     pwm_motor1.write(abs(u1));
     
     //send PWM and DIR to motor 2
-    dir_motor2 = u2>0 ? 1 : 0;          //conditional statement, same as if else below
+    dir_motor2 = u2>0 ? 0 : 1;          //conditional statement, same as if else below
     pwm_motor2.write(abs(u2));
     
     /*if(u1 > 0)
@@ -545,7 +656,7 @@
 //Start control
 void start_control(void)
 {
-    control_timer.attach(&control,SAMPLE_RATE);     //100 Hz control
+    control_timer.attach(&control,CONTROL_RATE);     //100 Hz control
     
     //Debug LED will be blue when control is on. If sampling and control are on -> blue + green = cyan.
     blue=0;