Diff: main.cpp

Revision:

59:57b9d3076930

Parent:

58:8d8a98ec2904

Child:

60:762b5c925e13

--- a/main.cpp	Fri Nov 01 00:14:59 2019 +0000
+++ b/main.cpp	Fri Nov 01 00:22:16 2019 +0000
@@ -978,7 +978,7 @@
     // State transition guard
     if ( button1_pressed == true ) { // demo_XY
         button1_pressed = false;
-        demo_curr_state = demo_XY;
+        demo_curr_state = do_demo_positioning;
         demo_state_changed = true;
         // More functions
     } else if ( switch2_pressed == true ) { // Return to global wait
@@ -1009,6 +1009,36 @@
     }
 }
 
+void do_demo_positioning()
+{
+    // Entry function
+    if ( demo_state_changed == true ) {
+        demo_state_changed = false;
+        timerStateChange.reset();
+        timerStateChange.start();
+    }
+    
+    Vx = 10.0;
+    Vy = 10.0;
+
+    PID_controller();
+    motorControl();
+    RKI();
+
+    scope.set(0, motor2_ref * rad2deg );
+    scope.set(1, motor2_angle * rad2deg );
+    //scope.set(2, motor2_ref );
+    //scope.set(3, motor2_angle );
+    scope.send();
+
+    // State transition guard
+    if ( timerStateChange.read() >= 3.0f ) { // demo_wait
+        button1_pressed = false;
+        demo_curr_state = demo_XY;
+        demo_state_changed = true;
+    }
+}
+
 void do_demo_XY()
 {
     // Entry function
@@ -1018,8 +1048,23 @@
 
     // Do stuff until end condition is met
     static float t = 0.0;
-    Vy = 15.0f * sin(1.5f*t);
-    Vx = 0.0f * sin(1.5f*t);
+
+    if ( t >= 0.0f && t < 3.0f ) {
+        Vx = 10;
+        Vy = 0;
+    } else if ( t >= 3.0f && t < 6.0f ) {
+        Vx = 0;
+        Vy = 10;
+    } else if ( t >= 6.0f && t < 9.0f ) {
+        Vx = -10;
+        Vy = 0;
+    }
+    if ( t >= 9.0f && t < 12.0f ) {
+        Vx = 0;
+        Vy = -10;
+    } else if ( t >= 12.0f) {
+        t = 0.0;
+    }
     t += Ts;
 
     PID_controller();
@@ -1034,6 +1079,7 @@
 
     // State transition guard
     if ( button1_pressed == true ) { // demo_wait
+        t = 0.0;
         button1_pressed = false;
         demo_curr_state = demo_wait;
         demo_state_changed = true;