Diff: main.cpp

Revision:

6:b0b15eb27de1

Parent:

5:9e8847a0492c

Child:

7:c78b25ef3c7b

--- a/main.cpp	Thu Nov 02 14:24:47 2017 +0000
+++ b/main.cpp	Thu Nov 02 15:30:30 2017 +0000
@@ -54,7 +54,7 @@
 double M2_home; 
 double M2_error_pos = 0;
 float M2_Kp = 1.5;
-float M2_Ki = 1.5;
+float M2_Ki = 2.5;
 float M2_Kd = 0;
 double M2_e_int=0;
 double M2_e_prior=0;
@@ -65,7 +65,10 @@
 double M2_rel_pos;
 double M2_reference_pos;
 
-float Ts = 0.002; //500hz sample freq
+double q1_step;
+double q2_step;
+
+float Ts = 0.01; //500hz sample freq
 
 bool M1homflag;
 bool M2homflag;
@@ -74,11 +77,11 @@
 
 
  
-void kinemtaica(double q1, double q2, double vdx, double vdy, double &q1_new, double &q2_new){
+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(q2))/(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;
 }
 
@@ -130,7 +133,7 @@
     
        while(1){
             if (HomStart.read() == 0){
-                  setpoint += 0.003; //move setpoint 0.2 radian per second (at 100hz)
+                  setpoint += 0.005; //move setpoint 0.2 radian per second (at 100hz)
                   pc.printf("Homing... \n\r");
             }     
             
@@ -196,16 +199,22 @@
     double 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
+    double q2 = -(q1 + M2_actual_pos); //see drawing
+    
+    double vdx = potmeter.read();
+    double vdy = -potmeter2.read();
     
-    //double M1_reference_pos = 1+potmeter.read()*0.5*3.1416; //should cover the right range - radians
-    double M1_reference_pos = 0.5*3.1416-potmeter.read(); //should cover the right range - radians
-    //double M2_reference_pos = 0.25*3.1416+potmeter2.read();
-    double M2_reference_pos = 0.4+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);
+    
+    
+    
         
     
-    pc.printf("M2 home = %f, M2 reference = %f \n\r",Arm2_home,M2_reference_pos);
-    //M2_error_pos = 0;
+    pc.printf("VDX = %f, q2 =  %f, q2_step =  %f, M2_actual = %f, M2_reference \n\r",vdx, q2,q2_step,M2_actual_pos, M2_reference_pos);
+
     if(M1_reference_pos > Arm1_home){
         M1_reference_pos = Arm1_home;
     }