Diff: main.cpp

Revision:

11:cd7be08f46b0

Parent:

10:990287a722d2

Child:

12:55b76c319364

--- a/main.cpp	Tue Oct 29 20:05:39 2019 +0000
+++ b/main.cpp	Tue Oct 29 20:57:56 2019 +0000
@@ -23,15 +23,16 @@
 QEI encoder1(D9,D8,NC,64,QEI::X4_ENCODING);     //Encoding motor 1
 QEI encoder2(D13,D12,NC,64,QEI::X4_ENCODING);   //Encoding motor 2
 float Ts = 0.01;                                //Sample time
-float motor1angle=-135.0*0.0174532*5.5;                              //Measured angle motor 1
-float motor2angle;                              //Measured angle motor 2
+float deg2rad=0.0174532;                        //Conversion factor degree to rad
+float rad2deg=57.29578;                         //Conversion factor rad to degree
+float motor1angle=(-135.0-10.0)*deg2rad*5.5;    //Measured angle motor 1
+float motor2angle=(-10.0)*deg2rad*2.75;         //Measured angle motor 2
 float motor1offset;                             //Offset bij calibratie
 float motor2offset;
 float potmeter;
 float omega1;                                   //velocity rad/s motor 1
 float omega2;                                   //Velocity rad/s motor2
-float deg2rad=0.0174532;                        //Conversion factor degree to rad
-float rad2deg=57.29578;                         //Conversion factor rad to degree
+
 
 
 // Motor
@@ -43,8 +44,8 @@
 //Motorcontrol
 bool motordir1;
 bool motordir2;
-float motor1ref=-135.0*0.0174532*5.5;
-float motor2ref;
+float motor1ref=(-135.0-10.0)*deg2rad*5.5;
+float motor2ref=(-10.0)*deg2rad*2.75;
 double controlsignal1;
 double controlsignal2;
 double pi2= 6.283185;
@@ -72,7 +73,7 @@
 //RKI 
 float Vx=0.0;                                   //Desired linear velocity x direction
 float Vy=0.0;                                   //Desired linear velocity y direction
-float q1=0.0f*deg2rad;                          //Angle of first joint [rad]
+float q1=-10.0f*deg2rad;                          //Angle of first joint [rad]
 float q2=-135.0f*deg2rad;                       //Angle of second joint [rad]
 float q1dot;                                    //Velocity of first joint [rad/s]
 float q2dot;                                    //Velocity of second joint [rad/s]
@@ -212,9 +213,8 @@
     //Vy=potmeter2.read()*10*motortoggle;
     static float t=0;
     Vx=6.0f*sin(1.0f*t);
+    Vy=0.0f;
     t+=Ts;
-    //Vx=-1.0*;
-    Vy=0.0f;
     q1dot=(l2*cos(q1+q2)*Vx+l2*sin(q1+q2)*Vy)/((-l1*sin(q1)-l2*sin(q1+q2))*l2*cos(q1+q2)+l2*sin(q1+q2)*(l1*cos(q1)+l2*cos(q1+q2)));
     q2dot=((-l1*cos(q1)-l2*cos(q1+q2))*Vx+(-l1*sin(q1)-l2*sin(q1+q2))*Vy)/((-l1*sin(q1)-l2*sin(q1+q2))*l2*cos(q1+q2)+l2*sin(q1+q2)*(l1*cos(q1)+l2*cos(q1+q2)));
     q1=q1+q1dot*Ts;
@@ -247,25 +247,27 @@
 
 void motorControl(){
     counts1= counts1af-counts1offset;
-    motor1angle = (counts1 * factorin / gearratio)-(135.0*5.5*deg2rad); // Angle of motor shaft in rad
+    motor1angle = (counts1 * factorin / gearratio)-((135.0*5.5*deg2rad)+(10.0*5.5*deg2rad)); // Angle of motor shaft in rad + correctie voor q1 en q2
     omega1 = deltaCounts1 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s
     motor1error=motor1ref-motor1angle;         
     if (controlsignal1<0){
         motordir1= 0;}
     else {
          motordir1= 1;}
+
     motor1Power.write(abs(controlsignal1));
     motor1Direction= motordir1;
     
     counts2= counts2af - counts2offset;
-    motor2angle = (counts2 * factorin / gearratio); // Angle of motor shaft in rad
+    motor2angle = (counts2 * factorin / gearratio)-(10.0*2.75*deg2rad); // Angle of motor shaft in rad + correctie voor q1
     omega2 = deltaCounts2 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s
     motor2error=motor2ref-motor2angle;         
     if (controlsignal2<0){
         motordir2= 0;}
     else {
          motordir2= 1;}
-    motor2Power.write(abs(controlsignal2));
+    if (motor_calibration_done == true){
+    motor2Power.write(abs(controlsignal2));}
     motor2Direction= motordir2;    
 }
 
@@ -398,6 +400,8 @@
             pc.printf("counts1offset %i counts1af: %i counts1: %i\r\n",counts1offset,counts1af,counts1);
             pc.printf("counts2offset %i counts2af: %i counts2: %i\r\n",counts2offset,counts2af,counts2);
             pc.printf("state: %i motor1ref: %f motor1angle: %f\r\n",motor_curr_state,motor1ref,motor1angle);
+            pc.printf("q1: %f q2: %f motor1error: %f \r\n",q1*rad2deg,q2*rad2deg, motor1error);
+            
         wait(0.5);
     }
 }