Diff: main.cpp

Revision:

9:5023f5a21eab

Parent:

8:d608c797ea2c

Child:

10:e328acbdf885

--- a/main.cpp	Thu Nov 02 18:22:20 2017 +0000
+++ b/main.cpp	Thu Nov 02 19:48:09 2017 +0000
@@ -41,12 +41,10 @@
 float set_speed;
 float reference_pos;
 
-
-
 double M1_home; 
 double M1_error_pos = 0;
-float M1_Kp = 2;
-float M1_Ki = 4;
+float M1_Kp = 2.2;
+float M1_Ki = 3.8;
 float M1_Kd = 0.19;
 double M1_e_int=0;
 double M1_e_prior=0;
@@ -55,7 +53,7 @@
 double M2_error_pos = 0;
 float M2_Kp = 2;
 float M2_Ki = 2.5;
-float M2_Kd = 0;
+float M2_Kd = 0.1;
 double M2_e_int=0;
 double M2_e_prior=0;
 
@@ -63,10 +61,20 @@
 
 double M1_rel_pos;
 double M2_rel_pos;
+double M1_reference_pos;
 double M2_reference_pos;
 
 double q1_step;
 double q2_step;
+double Arm1_home;
+double Arm2_home;
+double M1_actual_pos;
+double M2_actual_pos;
+double q1;
+double q2;
+double vdx;
+double vdy;
+
 
 float Ts = 0.01; //500hz sample freq
 
@@ -80,8 +88,9 @@
 void kinematica(double q1, double q2, double vdx, double vdy, double &q1_new, double &q2_new){
   double q1_dot = (2.5*cos(q1+q2))/(sin(q2)) *vdx + (2.5*sin(q1+q2))/(sin(q2))*vdy;
   double q2_dot = -(0.3*cos(q1+q2)+0.4*cos(q1))/(0.12*sin(q2))*vdx -(0.3*sin(q1+q2)+0.4*sin(q1))/(0.12*sin(q2))*vdy;
-  q1_new = q1 +q1_dot*Ts;
-  q2_new = q2 +(q2_dot-q1_dot)*Ts;
+  q1_new += q1_dot*Ts;
+  q2_new += (q2_dot-q1_dot)*Ts;
+  //pc.printf("q1 dot = %f, q2 dot = %f, ",q1_dot,q2_dot);
  return;
 }
 
@@ -193,30 +202,31 @@
     }
  
 void geterror(){ 
-    
+    M1_rel_pos = enc1.getPulses()/32.0/131.25*2.0*3.1416; //relative position in radians
+    M2_rel_pos = enc2.getPulses()/32.0/131.25*2.0*3.1416; //relative position in radians   
 
-    float Arm1_home = 130.0/180.0*3.1416;//home position of large link attached to base in radians - 112 degrees
-    float Arm2_home = 10.0/180.0*3.1416;//home position of small link attached to base
+    Arm1_home = 132.0/180.0*3.1416;//home position of large link attached to base in radians - 112 degrees
+    Arm2_home = 5.0/180.0*3.1416;//home position of small link attached to base
     
-    double M1_actual_pos = Arm1_home + (M1_rel_pos - M1_home)/2; //!!!!!!!Gearing ratio of timing belt = 2!!!!!!!!!! - Also known as angle theta
-    double M2_actual_pos = Arm2_home + (M2_rel_pos - M2_home)/2; //!!!!!!!Gearing ratio of timing belt = 2!!!!!!!!!! - Also known as angle Alpha
+    M1_actual_pos = Arm1_home + (M1_rel_pos - M1_home)*2; //!!!!!!!Gearing ratio of timing belt = 2!!!!!!!!!! - Also known as angle theta
+    M2_actual_pos = Arm2_home + (M2_rel_pos - M2_home)*2; //!!!!!!!Gearing ratio of timing belt = 2!!!!!!!!!! - Also known as angle Alpha
     
-    double q1 = M1_actual_pos;
-    double q2 = -q1 - M2_actual_pos; //see drawing
+    q1 = M1_actual_pos;
+    q2 = -q1 - M2_actual_pos; //see drawing
     
-    double vdx = 2*potmeter.read();
-    double vdy = 2*potmeter2.read();
+    vdx = 0.1*potmeter.read();
+    vdy = -0.1*potmeter2.read();
     
     kinematica(q1,q2,vdx,vdy,q1_step,q2_step);
     
-    double M1_reference_pos = q1_step; //should cover the right range - radians
-    double M2_reference_pos = -(q1_step+q2_step);
+    M1_reference_pos = q1_step; //should cover the right range - radians
+    M2_reference_pos = -(q1_step+q2_step);
     
     
     
         
     
-    pc.printf("VDX = %f, q1 = %f, q2 =  %f,q1_step = %f, q2_step =  %f,M1_rel = %f, M2_rel = %f, M2_reference = %f\n\r",vdx, q1, q2,q1_step,q2_step,M1_rel_pos,M2_rel_pos, M2_reference_pos);
+    pc.printf("VDX = %f, q1 = %f, q2 =  %f,q1_step = %f, q2_step =  %f,M1_rel = %f, M2_rel = %f, M2_reference = %f, M1_error_pos = %f, M2_error_pos = %f\n\r",vdx, q1, q2,q1_step,q2_step,M1_rel_pos,M2_rel_pos, M2_reference_pos,M1_error_pos,M2_error_pos);
 
     if(M1_reference_pos > Arm1_home){
         M1_reference_pos = Arm1_home;
@@ -230,6 +240,30 @@
     else{
         M2_error_pos = M2_reference_pos - M2_actual_pos;
     }
+    
+    
+    //---------PID motor control-------------
+    
+      set_speed = PID(M1_error_pos,M1_Kp,M1_Ki,M1_Kd,Ts,M1_e_int,M1_e_prior);
+    if(set_speed > 0){
+     M1_speed.write(abs(set_speed));
+     M1_direction.write(0);
+     }
+    else if (set_speed < 0){
+     M1_speed.write(abs(set_speed));
+     M1_direction.write(1);
+     }
+    else{M1_speed.write(0);}
+     set_speed = PID(M2_error_pos,M2_Kp,M2_Ki,M2_Kd,Ts,M2_e_int,M2_e_prior);
+    if(set_speed > 0){
+     M2_speed.write(abs(set_speed));
+     M2_direction.write(0);
+     }
+    else if (set_speed < 0){
+     M2_speed.write(abs(set_speed));
+     M2_direction.write(1);
+     }
+    else{M2_speed.write(0);}   
 }
  
 int main() {
@@ -244,16 +278,29 @@
 pc.printf("Setting home position complete\n\r");
 //attach all interrupt
 pc.printf("attaching interrupt tickers now \n\r");
+    M1_rel_pos = enc1.getPulses()/32.0/131.25*2.0*3.1416; //relative position in radians
+    M2_rel_pos = enc2.getPulses()/32.0/131.25*2.0*3.1416; //relative position in radians   
+
+    Arm1_home = 132.0/180.0*3.1416;//home position of large link attached to base in radians - 112 degrees
+    Arm2_home = 5.0/180.0*3.1416;//home position of small link attached to base
+    
+    M1_actual_pos = Arm1_home + (M1_rel_pos - M1_home)*2; //!!!!!!!Gearing ratio of timing belt = 2!!!!!!!!!! - Also known as angle theta
+    M2_actual_pos = Arm2_home + (M2_rel_pos - M2_home)*2; //!!!!!!!Gearing ratio of timing belt = 2!!!!!!!!!! - Also known as angle Alpha
+    
+    q1 = M1_actual_pos;
+    q2 = -q1 - M2_actual_pos; //see drawing
+    
+    q1_step = q1;
+    q2_step = q2;
    //stop motor interrupt
-motor_update1.attach(&M1_control,0.01);
-motor_update2.attach(&M2_control,0.01);
-error_update.attach(&geterror,0.005);
+//motor_update1.attach(&M1_control,0.01);
+//motor_update2.attach(&M2_control,0.01);
+error_update.attach(&geterror,0.02);
 
 pc.printf("initialization complete - position control of motors now active\n\r");
     
     while(1){   
-        double M1_rel_pos = enc1.getPulses()/32.0/131.25*2.0*3.1416; //relative position in radians
-        double M2_rel_pos = enc2.getPulses()/32.0/131.25*2.0*3.1416; //relative position in radians    
+         
      
     }