Diff: main.cpp

Revision:

7:c78b25ef3c7b

Parent:

6:b0b15eb27de1

Child:

8:d608c797ea2c

--- a/main.cpp	Thu Nov 02 15:30:30 2017 +0000
+++ b/main.cpp	Thu Nov 02 17:49:09 2017 +0000
@@ -79,9 +79,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(q2))/(0.12*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;
+  q2_new = q2 +q2_dot*Ts;
  return;
 }
 
@@ -120,7 +120,7 @@
      M2_direction.write(1);
      }
     else{M2_speed.write(0);}       
-    //pc.printf("M2 speed = %f, M2 direction = %i, M2 pos error =  %f, M2 setpoint = %f\n\r",set_speed,M2_direction.read(),M2_error_pos,M2_reference_pos);
+    //pc.printf("M2 speed = %f, M2 direction = %i, M2 pos error =  %f, M2 int error = %f\n\r",set_speed,M2_direction.read(),M2_error_pos,M2_e_int);
     //pc.printf("M2 integral = %f position error = %f\n\r", M2_e_int,M2_error_pos);
   }
  
@@ -134,19 +134,21 @@
        while(1){
             if (HomStart.read() == 0){
                   setpoint += 0.005; //move setpoint 0.2 radian per second (at 100hz)
-                  pc.printf("Homing... \n\r");
+                  //pc.printf("Homing... \n\r");
             }     
             
             double M1_rel_pos = enc1.getPulses()/32.0/131.25*2.0*3.1416; 
             double M2_rel_pos = enc2.getPulses()/32.0/131.25*2.0*3.1416;  
             if(LimSW1.read() == 0){
-                  M1_error_pos = 1.2*setpoint - M1_rel_pos;
+                  M1_error_pos = setpoint - M1_rel_pos;
+                  M1_control();
             }
             if(LimSW2.read() == 0){
                 M2_error_pos = - setpoint - M2_rel_pos;
+                M2_control();
             }
-            M1_control();
-            M2_control();
+            
+            
             
         
         if(LimSW1.read() ==  1){
@@ -164,13 +166,15 @@
                 pc.printf("Homing finished \n\r");
                 M1_speed.write(0);
                 M2_speed.write(0);
-                wait(0.5);
+                wait(0.1);
                 M1_home = enc1.getPulses()/32.0/131.25*2.0*3.1416; //in radians
                 M2_home = enc2.getPulses()/32.0/131.25*2.0*3.1416; //in radians
+                M1_e_int = 0;
+                M2_e_int = 0;
                 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\n\r", M2_error_pos,M2_reference_pos);
         wait(0.01);
        }
 
@@ -192,28 +196,28 @@
     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 = 122.0/180.0*3.1416;//home position of large link attached to base in radians - 112 degrees
-    float Arm2_home = 19.0/180.0*3.1416;//home position of small link attached to base
+    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
     
     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
     
     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 vdx = 2*potmeter.read();
+    double vdy = 2*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);
+    double M2_reference_pos = q1_step-q2_step;
     
     
     
         
     
-    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);
+    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);
 
     if(M1_reference_pos > Arm1_home){
         M1_reference_pos = Arm1_home;