Diff: main.cpp

Revision:

55:726fdab812a9

Parent:

54:4cda9af56bed

Child:

56:5ff9e5c1ed44

--- a/main.cpp	Tue Oct 27 10:11:29 2015 +0000
+++ b/main.cpp	Tue Oct 27 15:28:31 2015 +0000
@@ -14,7 +14,7 @@
 //Define important constants in memory
 #define     PI              3.14159265
 #define     SAMPLE_RATE     0.002   //500 Hz EMG sample rate
-#define     CONTROL_RATE    0.01    //100 Hz Control rate
+#define     CONTROL_RATE    0.005    //100 Hz Control rate
 #define     ENCODER_CPR     4200     //both motor encoders have 64 (X4), 32 (X2) counts per revolution of motor shaft
                                     //gearbox 1:131.25 ->  4200 counts per revolution of the output shaft (X2), 
 #define     PWM_PERIOD      0.0001  //10k Hz pwm motor frequency. Higher -> too hot, lower -> motor doesnt respond correctly
@@ -37,7 +37,7 @@
 
 Ticker      sample_timer;       //Ticker for EMG sampling
 Ticker      control_timer;      //Ticker for control loop
-HIDScope    scope(4);           //Scope 4 channels
+//HIDScope    scope(4);           //Scope 4 channels
 
 // AnalogIn potmeter(A4);       //potmeters
 // AnalogIn potmeter2(A5);      //
@@ -113,10 +113,10 @@
 //PID variables
 double u1; double u2;                                                         //Output of PID controller (PWM value for motor 1 and 2)
 double m1_error=0; double m1_e_int=0; double m1_e_prev=0;                     //Error, integrated error, previous error motor 1
-const double m1_kp=10; const double m1_ki=0.25; const double m1_kd=0.5;       //Proportional, integral and derivative gains.
+const double m1_kp=5; const double m1_ki=0.1; const double m1_kd=0.3;       //Proportional, integral and derivative gains.
 
 double m2_error=0; double m2_e_int=0; double m2_e_prev=0;                     //Error, integrated error, previous error motor 2
-const double m2_kp=5; const double m2_ki=0.125; const double m2_kd=0.25;       //Proportional, integral and derivative gains.
+const double m2_kp=5; const double m2_ki=0.1; const double m2_kd=0.3;       //Proportional, integral and derivative gains.
 
 //Calibration bools, checks if elbow/shoulder limits are hit
 volatile bool done1 = false;
@@ -282,7 +282,13 @@
 /*--------------------------------------------------------------------------------------------------------------------
 ---- MAIN LOOP -------------------------------------------------------------------------------------------------------
 --------------------------------------------------------------------------------------------------------------------*/
-
+void servotest(){
+    //Servo alignment
+    theta3 = 1.5*PI - (theta1) - theta2;
+    servopos = -(2100/PI)*theta3 + 2700;
+    servoPwm.SetPosition(servopos);
+}
+Ticker test;
 int main()
 {
     pc.baud(115200);            //serial baudrate
@@ -379,6 +385,8 @@
                 wait(1);
                 emg_control=false;
                 control_method=2;
+                
+                test.attach(&servotest,0.02);
                 start_control();
                 wait(1);   
                 controlButtons();
@@ -391,6 +399,7 @@
                 wait(1);
                 emg_control_time = 0;
                 emg_control=true;
+                test.attach(&servotest,0.02);
                 control_method=2;
                 
                 start_control();
@@ -444,19 +453,23 @@
     while(c != 'Q' && c != 'q') {
     //Debugging values:
     if(c!='q' && c!='Q'){
-    pc.printf("Reference position: %f and %f \r\n",x,y);
+    /*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("Pos Error: %f and %f \r\n",x_error,y_error);
     pc.printf("Current angles: %f and %f \r\n",theta1,theta2);
     pc.printf("DLS1: %f and DLS2 %f and DLS3 %f and DLS4: %f \r\n",dls1,dls2,dls3,dls4);
     pc.printf("Error in angles: %f and %f \r\n",q1_error,q2_error);
     pc.printf("PID output: %f and %f \r\n",u1,u2);
-    pc.printf("----------------------------------------\r\n\n");
+    pc.printf("----------------------------------------\r\n");
     pc.printf("Buffer 1: %f \r\n",biceps_avg);                
     pc.printf("Buffer 2: %f \r\n",triceps_avg);
     pc.printf("Buffer 3: %f \r\n",flexor_avg);
     pc.printf("Buffer 4: %f \r\n",extens_avg);
-    wait(1);
+    pc.printf("----------------------------------------\r\n");
+    pc.printf("Theta 3: %f \r\n",theta3);
+    pc.printf("Servoposition (us): %f \r\n",servopos);
+    pc.printf("----------------------------------------\r\n");
+    wait(1);  */
     }//end if
     
     if( pc.readable() ){
@@ -495,7 +508,24 @@
                                         
                       break;
     }//end switch
-    
+    /* if(c!='q' && c!='Q'){
+    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("Pos Error: %f and %f \r\n",x_error,y_error);
+    pc.printf("Current angles: %f and %f \r\n",theta1,theta2);
+    pc.printf("DLS1: %f and DLS2 %f and DLS3 %f and DLS4: %f \r\n",dls1,dls2,dls3,dls4);
+    pc.printf("Error in angles: %f and %f \r\n",q1_error,q2_error);
+    pc.printf("PID output: %f and %f \r\n",u1,u2);
+    pc.printf("----------------------------------------\r\n");
+    pc.printf("Buffer 1: %f \r\n",biceps_avg);                
+    pc.printf("Buffer 2: %f \r\n",triceps_avg);
+    pc.printf("Buffer 3: %f \r\n",flexor_avg);
+    pc.printf("Buffer 4: %f \r\n",extens_avg);
+    pc.printf("----------------------------------------\r\n");
+    pc.printf("Theta 3: %f \r\n",theta3);
+    pc.printf("Servoposition (us): %f \r\n",servopos);
+    pc.printf("----------------------------------------\r\n");
+    }*/
     }
     //end if pc readable
     
@@ -521,18 +551,18 @@
     biceps_power = lowpass.step(biceps_power); triceps_power = lowpass2.step(triceps_power); flexor_power = lowpass3.step(flexor_power); extens_power = lowpass4.step(extens_power);
    
     //send filtered emg to scope 
-    //scope.set(0,biceps_power);
-    //scope.set(1,triceps_power);
-    //scope.set(2,flexor_power);
-    //scope.set(3,extens_power);
+    /*scope.set(0,biceps_power);
+    scope.set(1,triceps_power);
+    scope.set(2,flexor_power);
+    scope.set(3,extens_power);
+    scope.send();
+    */
+    //send normalized emg to scope 
+    //scope.set(0,biceps);
+    //scope.set(1,triceps);
+    //scope.set(2,flexor);
+    //scope.set(3,extens);
     //scope.send();
-    
-    //send normalized emg to scope 
-    scope.set(0,biceps);
-    scope.set(1,triceps);
-    scope.set(2,flexor);
-    scope.set(3,extens);
-    scope.send();
        
 }
 
@@ -560,12 +590,12 @@
 //Run motors slowly clockwise to mechanical limit. Attached to 100Hz ticker
 void calibrate(void){
     if(done1==false){                   //if motor 1 limit has not been hit yet
-            pwm_motor1=0.3;             //move upper arm slowly cw
+            pwm_motor1=0.4;             //move upper arm slowly cw
             pc.printf("Motor 1 running %f \r\n");
         }
     if(done1==true && done2==false){    //if limit motor 1 has been hit
             pwm_motor1=0;               //stop motor1
-            pwm_motor2=0.3;             //start moving forearm slowly cw
+            pwm_motor2=0.4;             //start moving forearm slowly cw
             pc.printf("Motor 2 running %f \r\n");    
         }   
 }
@@ -924,8 +954,8 @@
         ypower = 0;
             
     //power: the longer a signal is active, the further the reference goes. So integrate to determine reference position
-    dx = xpower * CONTROL_RATE * 0.1;           //last value is a factor to control how fast the reference (so also end effector) changes 
-    dy = ypower * CONTROL_RATE * 0.1; 
+    dx = xpower * CONTROL_RATE * 0.15;           //last value is a factor to control how fast the reference (so also end effector) changes 
+    dy = ypower * CONTROL_RATE * 0.15; 
     
     //Direction! Sum dx and dy but make sure xdir and ydir are considered.
     if (xdir>0)     //if x direction of sample is positive, add it to reference position
@@ -1040,8 +1070,8 @@
     u2=pid2(m2_error,m2_kp,m2_ki,m2_kd,m2_e_int,m2_e_prev);    //motor 2
     
     //keep u between limits, sign = direction, PWM = 0-1
-    keep_in_range(&u1,-0.6,0.6);    
-    keep_in_range(&u2,-0.6,0.6);
+    keep_in_range(&u1,-0.7,0.7);    
+    keep_in_range(&u2,-0.7,0.7);
     
     //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. 
@@ -1052,11 +1082,7 @@
     pwm_motor2.write(abs(u2));
     
     
-    //Servo alignment
-    theta3 = 180 - theta1 - theta2;
-    servopos = (2100/180)*theta3 + 600;
-    servoPwm.SetPosition(servopos);
-    
+
     /*if(u1 > 0)
     {
         dir_motor1 = 0;