Diff: main.cpp

Revision:

4:f36406c9e42f

Parent:

3:56cbed6caacc

Child:

5:14a68d0ee71a

--- a/main.cpp	Wed Oct 17 13:36:47 2018 +0000
+++ b/main.cpp	Fri Oct 19 12:45:17 2018 +0000
@@ -7,47 +7,60 @@
 DigitalOut directionpin2(D8);
 MODSERIAL pc(USBTX, USBRX);
 
+Ticker ticker;
+
 volatile bool emg0Bool = false;
 volatile bool emg1Bool = false;
 volatile bool emg2Bool = false;
 volatile bool y_direction = true;
+volatile bool a;
 
-volatile double x_position = 0;
-volatile double y_position = 0;
-volatile double motor1_angle;
-volatile double motor2_angle;
+volatile float x_position = 0.0;
+volatile float y_position = 0.0;
+volatile float old_y;
+volatile float old_x;
+volatile int motor1_angle = 0;
+volatile int motor2_angle = 0;
 volatile int direction;
+volatile char c;
 
 const float length = 0.300; //length in m (placeholder)
 const int C1 = 3; //motor 1 gear ratio (placeholder)
 const int C2 = 3; //motor 2 gear ratio
-    
+      
 void xDirection() {
     //direction of the motion
-    if (emg0Bool && !emg1Bool) {
+    if (emg0Bool && !emg1Bool) { //if a is pressed and not d, move to the left
         directionpin1 = true;
         directionpin2 = true;
-        direction = 1;
+        direction = -1;
     }
-    else if (!emg0Bool && emg1Bool) {
+    else if (!emg0Bool && emg1Bool) { //if d is pressed and not a, move to the right
         directionpin1 = false;
         directionpin2 = false;
-        direction = -1;
+        direction = 1;
     }
 
     if (emg0Bool || emg1Bool){
         //calculating the motion
-        x_position = x_position + direction;
-        motor1_angle = pow(sin( x_position ), -1) / length;
-        y_position = y_position + direction;
-        motor2_angle = pow(cos( x_position ), -1) / length;
+        old_x = x_position;
+        x_position = old_x + direction;
+        motor1_angle = asin( x_position ) / length;     //theta = arcsin(x) / L
+        
+        old_y = y_position;
+        y_position = old_y + direction;
+        motor2_angle = acos( x_position ) / length;     //theta = arcsin(x) / L
     }
+    
+    //reset the booleans
+    emg0Bool = false;
+    emg1Bool = false;
 }
 
 void yDirection () {      
-    if (emg2Bool) {
+    if (emg2Bool) { //if w is pressed, move up/down
         //direction of the motion
-        if (y_direction) {
+        if (y_direction) { 
             directionpin2 = true;
             direction = 1;
         }
@@ -57,24 +70,49 @@
         }
         
         //calculating the motion
-        y_position = y_position + direction;
+        old_y = y_position;
+        y_position = old_y + direction;
         motor2_angle = C2 * y_position;
+        
+        //reset the boolean
+        emg2Bool = false; 
     }
 }
 
 int main() {
+    pc.baud(115200);
     pc.printf(" ** program reset **\n\r");
+    
     while (true) {
-        if (button) {
+        //if the button is pressed, reverse the y direction
+        if (!button) {
             y_direction = !y_direction;
+            wait(0.5);
         }
         
-        xDirection();
-        yDirection();
+        //testing the code from keyboard imputs: a-d: left to right, w: forward/backwards
+        a = pc.readable();
+        if (a) {
+            c = pc.getc();
+            switch (c){
+                case 'a': //move to the left
+                    emg0Bool = true; 
+                    break;           
+                case 'd': //move to the right
+                    emg1Bool = true;
+                    break;            
+                case 'w': //move up/down
+                    emg2Bool = true;
+                    break;
+            }
+        }
+        xDirection(); //call the function to move in the x direction
+        yDirection(); //call the function to move in the y direction
         
+        // print the motor angles
         pc.printf("motor1 angle: %i\n\r", motor1_angle);
         pc.printf("motor2 angle: %i\n\r\n", motor2_angle);
-        
-        wait(0.5f); //can also be done with ticker, to be sure that it happens exactly every 0.5 seconds
+
+        wait(0.25f); //can also be done with ticker, to be sure that it happens exactly every 0.5 seconds
     }
 }
\ No newline at end of file