Diff: main.cpp

Revision:

17:8a0c720733b8

Parent:

16:e51ddfaf2e7a

Child:

18:e0086bd1d87e

--- a/main.cpp	Mon Oct 21 12:43:43 2019 +0000
+++ b/main.cpp	Mon Oct 21 14:27:25 2019 +0000
@@ -34,13 +34,13 @@
 DigitalOut motor1Direction(D7);
 FastPWM motor1Power(D6);
 
-volatile int motor1Toggle = 1;
+volatile int motortoggle = 1;
 
 //Motorcontrol
 bool motordir1;
 bool motordir2;
-float motor1ref= 0;
-float motor2ref=0;
+float motor1ref=0.1745;
+float motor2ref=0.0873;
 double controlsignal1;
 double controlsignal2;
 double pi2= 6.283185;
@@ -61,9 +61,17 @@
 float up2;
 float ek1;
 float ek2;
-float ei1= 0;
-float ei2=0;
-float Ka= 1;
+float ei1= 0.0;
+float ei2=0.0;
+float Ka= 1.0;
+
+//RKI 
+float Vx;
+float Vy;
+float q1dot;
+float q2dot;
+float l1=26.0;  //Afstand base-link [cm]
+float l2=62.0;  //Afstand link-endpoint [cm]
 
 //Hidscope
 HIDScope scope(3); //Going to send 3 channels of data. To access data go to 'http:/localhost:18082/' after starting HIDScope application.
@@ -174,22 +182,27 @@
     motor1ref= 0.5*pi2+motor1ref;
     motor2ref= 0.5*pi2+motor2ref;
     // static float t = 0;
-    // refangle= pi2/3.0f*sin(5.0f*t)*motor1Toggle;
+    // refangle= pi2/3.0f*sin(5.0f*t)*motortoggle;
     //t+=0.01;
     }
 
-//void RKI(){
-    
-//    RKI magic
-    
-//    motor1ref=??;
-//    motor2ref=??;
-//    }    
+void RKI(){
+  
+    Vy=potmeter1.read()*-10.0*motortoggle;
+    //Vy=potmeter2.read()*10*motortoggle;
+    //Vx=-1.0*;
+    Vx=0.0;
+    q1dot=(l2*cos(motor1ref+motor2ref)*Vx+l2*sin(motor1ref+motor2ref)*Vy)/((-l1*sin(motor1ref)-l2*sin(motor1ref+motor2ref))*l2*cos(motor1ref+motor2ref)+l2*sin(motor1ref+motor2ref)*(l1*cos(motor1ref)+l2*cos(motor1ref+motor2ref)));
+    q2dot=((-l1*cos(motor1ref)-l2*cos(motor1ref+motor2ref))*Vx+(-l1*sin(motor1ref)-l2*sin(motor1ref+motor2ref))*Vy)/((-l1*sin(motor1ref)-l2*sin(motor1ref+motor2ref))*l2*cos(motor1ref+motor2ref)+l2*sin(motor1ref+motor2ref)*(l1*cos(motor1ref)+l2*cos(motor1ref+motor2ref)));
+  
+    motor1ref=motor1ref+q1dot*Ts;
+    motor2ref=motor2ref+q2dot*Ts;
+    }    
     
 void motorControl()
 {
     button1.fall(&togglehoek);
-//    RKI()
+    RKI();
     motor1angle = counts1 * factorin / gearratio; // Angle of motor shaft in rad
     omega1 = deltaCounts1 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s
     motor1error=motor1ref-motor1angle;    
@@ -215,15 +228,16 @@
 
 void Plotje(){
     scope.set(0,motor1ref); //gewenste hoek
-    scope.set(1,motor1angle); //Gemeten hoek
-    scope.set(2,motor1error); //verschil in gewenste en gemeten hoek
+    scope.set(1,motor2ref);
+    scope.set(2,motor1angle); //Gemeten hoek
+    scope.set(3,motor1error); //verschil in gewenste en gemeten hoek
 
     scope.send(); //send what's in scope memory to PC
 }
 
 void toggleMotor()
 {
-    motor1Toggle = !motor1Toggle;
+    motortoggle = !motortoggle;
 }
 
 int main()
@@ -240,11 +254,13 @@
     button2.fall(&toggleMotor);
     
     while (true) {
-        pc.printf("Angle1:  %f   Omega1:       %f\r\n", motor1angle, omega1);
+//        pc.printf("Angle1:  %f   Omega1:       %f\r\n", motor1angle, omega1);
         pc.printf("refangle1: %f   Error1:  %f     \r\n",motor1ref, motor1error);
-        pc.printf("Angle2:  %f   Omega2:       %f\r\n", motor2angle, omega2);
+//        pc.printf("Angle2:  %f   Omega2:       %f\r\n", motor2angle, omega2);
         pc.printf("refangle2: %f   Error2:  %f     \r\n",motor2ref, motor2error);
-        pc.printf("controlsignal2: %d \r\n", controlsignal2);
+//        pc.printf("controlsignal2: %d \r\n", controlsignal2);
+        pc.printf("Vx: %f Vy: %f \r\n",Vx,Vy);
+        pc.printf("q1dot: %f q2dot: %f \r\n",q1dot,q2dot);
         
         wait(0.5);
     }