Diff: main.cpp

Revision:

26:6af46ad1e4ea

Parent:

23:9eeac9d1ecbe

--- a/main.cpp	Tue Oct 22 12:06:33 2019 +0000
+++ b/main.cpp	Thu Oct 24 12:21:01 2019 +0000
@@ -174,6 +174,40 @@
     }
 }
 
+float x;
+float y;
+float vx;
+float vy;
+float q1;
+float q2;
+float q_dot_2;
+
+void playing_field()
+{
+    x = q2*cos(q1);
+    y = q2*sin(q1);
+    if (x <= 0.6 && y <= 0.28){
+        if (vx < 0.0){
+            vx = 0.0;
+        }
+        if (vy < 0.0){
+            vy = 0.0;
+            }
+    }
+    if (y > 0.45 || q2 > 0.63){
+        if (vy > 0.0){
+            vy = 0.0;
+            }
+        if (q2 > 0.63) {
+            q_dot_2 = 0.0;
+            }
+        }
+}
+        
+
+    
+    
+
 void moving_magnet_off()
 /*
     Moving with the magnet disabled. This is the part from the home position
@@ -353,6 +387,22 @@
     return speed;
 }
 
+float v[2] = {vx, vy};
+
+void velocity_control() {
+    int c;
+    for (c = 0; c<2; c++) {
+        if (speed[c] == 0){
+            v[c] = 0;
+            }
+        if (speed[c] == 1){
+            v[c] = 0.01;
+            }
+        if (speed[c] == 2){
+            v[c] = 0.02;
+        }
+}
+
 int main()
 {
     pc.baud(115200);