Diff: main.cpp

Revision:

8:d608c797ea2c

Parent:

7:c78b25ef3c7b

Child:

9:5023f5a21eab

--- a/main.cpp	Thu Nov 02 17:49:09 2017 +0000
+++ b/main.cpp	Thu Nov 02 18:22:20 2017 +0000
@@ -53,7 +53,7 @@
 
 double M2_home; 
 double M2_error_pos = 0;
-float M2_Kp = 1.5;
+float M2_Kp = 2;
 float M2_Ki = 2.5;
 float M2_Kd = 0;
 double M2_e_int=0;
@@ -81,7 +81,7 @@
   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*Ts;
+  q2_new = q2 +(q2_dot-q1_dot)*Ts;
  return;
 }
 
@@ -174,7 +174,7 @@
                 break;
                 //while(1); //stop after homing.
         }
-        //pc.printf("M2 error = %f M2 reference = %f\n\r", M2_error_pos,M2_reference_pos);
+        pc.printf("M2 error = %f M2 reference = %f,M2_rel = %f\n\r", M2_error_pos,M2_reference_pos,M2_rel_pos);
         wait(0.01);
        }
 
@@ -193,8 +193,7 @@
     }
  
 void geterror(){ 
-    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
+    
 
     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
@@ -211,13 +210,13 @@
     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;
+    double M2_reference_pos = -(q1_step+q2_step);
     
     
     
         
     
-    pc.printf("VDY = %f, q2 =  %f,q1_step = %f, q2_step =  %f, M2_actual = %f, M2_reference = %f\n\r",vdy, q2,q1_step,q2_step,M2_actual_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\n\r",vdx, q1, q2,q1_step,q2_step,M1_rel_pos,M2_rel_pos, M2_reference_pos);
 
     if(M1_reference_pos > Arm1_home){
         M1_reference_pos = Arm1_home;
@@ -248,12 +247,13 @@
    //stop motor interrupt
 motor_update1.attach(&M1_control,0.01);
 motor_update2.attach(&M2_control,0.01);
-error_update.attach(&geterror,0.01);
+error_update.attach(&geterror,0.005);
 
 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    
      
     }