Diff: main.cpp

Revision:

28:0460786f9573

Parent:

27:0cefe83f83d3

Child:

30:ec66691d226d

--- a/main.cpp	Sun Oct 18 23:24:30 2015 +0000
+++ b/main.cpp	Mon Oct 19 00:15:39 2015 +0000
@@ -65,10 +65,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
-const double m1_kp=0.001; const double m1_ki=1; const double m1_kd=0;   //Proportional, integral and derivative gains.
+const double m1_kp=1; const double m1_ki=0.0125; const double m1_kd=0.1;   //Proportional, integral and derivative gains.
 
 double m2_error=0; double m2_e_int=0; double m2_e_prev=0;           //Error, integrated error, previous error
-const double m2_kp=0.001; const double m2_ki=2; const double m2_kd=0;   //Proportional, integral and derivative gains.
+const double m2_kp=1; const double m2_ki=0.0125; const double m2_kd=0.1;   //Proportional, integral and derivative gains.
 
 
 //lowpass filter 7 Hz  - envelope
@@ -151,7 +151,7 @@
     //calibrate_arm();        //Start Calibration
     
     //calibrate_emg();        
-    wait(1);
+    wait(3);
     start_control();        //100 Hz control
     
     //maybe some stop commands when a button is pressed: detach from timers.
@@ -272,14 +272,13 @@
 {
     // Derivative
     double e_der = (error-e_prev)/ CONTROL_RATE;
-    //e_der = derfilter.step(e_der);
+    e_der = derfilter.step(e_der);
     e_prev = error;
     // Integral
     e_int = e_int + CONTROL_RATE * error;
     // PID
-    return kp*error + ki*e_int;
+    return kp*error + ki*e_int + kd * e_der;
     
-    //+ kd * e_der;
 }
 
 void controlMenu()
@@ -325,7 +324,7 @@
     
     dtheta2 = atan2(sintheta2,costheta2);
     
-    costheta1 = ( x * (l1 + l2 * costheta2) + y * l2 * sostheta2 ) / ( pow(x,2) + pow(y,2) );
+    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);
@@ -345,15 +344,15 @@
     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 ? 0 : 1;          //conditional statement dir_motor1 = [condition] ? [if met 1] : [else 0]], same as if else below. 
+    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));