Diff: main.cpp

Revision:

12:55b76c319364

Parent:

11:cd7be08f46b0

Child:

13:41fc5b6262c0

diff -r cd7be08f46b0 -r 55b76c319364 main.cpp
--- a/main.cpp	Tue Oct 29 20:57:56 2019 +0000
+++ b/main.cpp	Tue Oct 29 21:00:06 2019 +0000
@@ -209,12 +209,6 @@
 void RKI(){
   
   if (motor_RKI==true){
-    //Vy=potmeter1.read()*10.0*motortoggle;
-    //Vy=potmeter2.read()*10*motortoggle;
-    static float t=0;
-    Vx=6.0f*sin(1.0f*t);
-    Vy=0.0f;
-    t+=Ts;
     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;
@@ -316,7 +310,7 @@
         motor_state_changed = false;
         // More functions
     }
-
+     motor_RKI=true;
     // Do nothing until end condition is met 
     
     // State transition guard  
@@ -335,9 +329,11 @@
         motor_state_changed = false;
         // More functions
     }    
-    
-    motor_RKI=true;
-
+    static float t=0;
+    Vx=6.0f*sin(1.0f*t);
+    Vy=0.0f;
+    t+=Ts;
+   
 if ( button2_pressed ) {   
         button2_pressed = false;   
         motor_RKI = false;