Diff: main.cpp

Revision:

3:dfb4b8d37bab

Parent:

2:4fb733fffd1f

Child:

4:7b8bf798bb72

--- a/main.cpp	Fri Oct 14 09:03:35 2016 +0000
+++ b/main.cpp	Fri Oct 14 10:09:44 2016 +0000
@@ -19,7 +19,55 @@
 AnalogIn pot2(A3); // pot 2 motor 3
 
 MODSERIAL pc(USBTX, USBRX);
+bool draairechts;
+bool draailinks;
 
+void GetDirections()
+{
+    if (rechts == 1) {
+        draairechts = !draairechts;
+        draailinks = 0;
+    } else if (rechts == 0) {
+        draairechts = 0;
+    } 
+    
+    if (links == 1) {
+        draailinks = !draailinks;
+        draairechts = 0;
+    } else if (links == 0) {
+        draailinks = 0;
+    }
+
+    /*if ((rechts == 0) && (links == 0)) {
+        draairechts = false;
+        draailinks = false;
+    } else if ((rechts == 1) && (draairechts ==false)) { //Als je rechts aanspant en hij draait nog niet naar rechts
+        draairechts = true;
+        draailinks = false;
+    } else if ((rechts == 1) && (draairechts == true)) { //Als je rechts aanspant en hij draait al naar rechts
+        draairechts = false;
+        draailinks = false;
+    } else if ((links == 1) && (draailinks == false)) { //Als je links aanspant en hij draait nog niet naar links
+        draailinks = true;
+        draairechts = false;
+    } else if ((links == 1) && (draailinks == true)) { // Als je links aanspant en hij draait al naar links
+        draailinks = false;
+        draairechts = false;
+    }
+
+    else if ((links == 1 && draairechts == true)) { // Als je links aanspant en hij draait nog naar rechts
+        draailinks = true;
+        draairechts = false;
+    } else if ((rechts == 1 && draailinks == true)) { // Als je rechts aanspant en hij draait nog naar links
+        draairechts = true;
+        draailinks = false;
+
+    } else {
+        draairechts = false;
+        draailinks = false;
+    }
+    */
+}
 int main()
 {
     pc.baud(115200);
@@ -27,112 +75,91 @@
     M1_rotate = 1;
     M2_rotate = 1;
     M3_rotate = 1;
-    a = 1;
-
-    float GetReferenceVelocity() {
-        // Returns reference velocity in rad/s.
-        // Positive value means clockwise rotation.
-        const float maxVelocity=8.4; // in rad/s of course!    float referenceVelocity;  // in rad/s
-        if (a)   {
-            // Clockwise rotation
-            referenceVelocity = potMeterIn * maxVelocity;
-            
-        } else {
-            // Counterclockwise rotation
-            referenceVelocity = -1*potMeterIn * maxVelocity;
-            
-        }
-        return referenceVelocity;
-    }
-
-    void SetMotor1(float motorValue) {
-        // Given -1<=motorValue<=1, this sets the PWM and direction
-        // bits for motor 1. Positive value makes motor rotating
-        // clockwise. motorValues outside range are truncated to
-        // within range
-        if (motorValue >=0) {
-            motor1DirectionPin=1;
-        } else {
-            motor1DirectionPin=0;
-        }
-        if (fabs(motorValue)>1) {
-            motor1MagnitudePin = 1;
-        } else {
-            motor1MagnitudePin = fabs(motorValue);
-        }
-    }
 
-    void SetMotor2(float motorValue) {
-        // Given -1<=motorValue<=1, this sets the PWM and direction
-        // bits for motor 1. Positive value makes motor rotating
-        // clockwise. motorValues outside range are truncated to
-        // within range
-        if (motorValue >=0) {
-            motor2DirectionPin=1;
-        } else {
-            motor2DirectionPin=0;
+    /*
+        float GetReferenceVelocity() {
+            // Returns reference velocity in rad/s.
+            // Positive value means clockwise rotation.
+            const float maxVelocity=8.4; // in rad/s of course!    float referenceVelocity;  // in rad/s
+            if (a)   {
+                // Clockwise rotation
+                referenceVelocity = potMeterIn * maxVelocity;
+                
+            } else {
+                // Counterclockwise rotation
+                referenceVelocity = -1*potMeterIn * maxVelocity;
+                
+            }
+            return referenceVelocity;
         }
-        if (fabs(motorValue)>1) {
-            motor2MagnitudePin = 1;
-        } else {
-            motor2MagnitudePin = fabs(motorValue);
-        }
-    }
 
-    void SetMotor3(float motorValue) {
-        // Given -1<=motorValue<=1, this sets the PWM and direction
-        // bits for motor 1. Positive value makes motor rotating
-        // clockwise. motorValues outside range are truncated to
-        // within range
-        if (motorValue >=0) {
-            motor3DirectionPin=1;
-        } else {
-            motor3DirectionPin=0;
-        }
-        if (fabs(motorValue)>1) {
-            motor3MagnitudePin = 1;
-        } else {
-            motor3MagnitudePin = fabs(motorValue);
-        }
-    }
-    bool GetDirections() {
-        bool draairechts = false;
-        bool draailinks = false;
-        if (rechts == 1 && draairechts = false) { //Als je rechts aanspant en hij draait nog niet naar rechts
-            draairechts = true;
-        } else if (rechts == 1 && draairechts = true) { //Als je rechts aanspant en hij draait al naar rechts
-            draairechts = false;
-        } else if (links == 1 && draailinks = false) { //Als je links aanspant en hij draait nog niet naar links
-            draailinks = true;
-        } else if (links == 1 && draailinks = true) { // Als je links aanspant en hij draait al naar links
-            draailinks = false;
+        void SetMotor1(float motorValue) {
+            // Given -1<=motorValue<=1, this sets the PWM and direction
+            // bits for motor 1. Positive value makes motor rotating
+            // clockwise. motorValues outside range are truncated to
+            // within range
+            if (motorValue >=0) {
+                motor1DirectionPin=1;
+            } else {
+                motor1DirectionPin=0;
+            }
+            if (fabs(motorValue)>1) {
+                motor1MagnitudePin = 1;
+            } else {
+                motor1MagnitudePin = fabs(motorValue);
+            }
         }
 
-        else if (links == 1 && draairechts = true) { // Als je links aanspant en hij draait nog naar rechts
-            draailinks = true;
-            draairechts = false;
-        } else if (rechts == 1 && draailinks = true) { // Als je rechts aanspant en hij draait nog naar links
-            draairechts = true;
-            draailinks = false;
+        void SetMotor2(float motorValue) {
+            // Given -1<=motorValue<=1, this sets the PWM and direction
+            // bits for motor 1. Positive value makes motor rotating
+            // clockwise. motorValues outside range are truncated to
+            // within range
+            if (motorValue >=0) {
+                motor2DirectionPin=1;
+            } else {
+                motor2DirectionPin=0;
+            }
+            if (fabs(motorValue)>1) {
+                motor2MagnitudePin = 1;
+            } else {
+                motor2MagnitudePin = fabs(motorValue);
+            }
         }
-        return draailinks;
-        return draairechts;
-    }
+
+        void SetMotor3(float motorValue) {
+            // Given -1<=motorValue<=1, this sets the PWM and direction
+            // bits for motor 1. Positive value makes motor rotating
+            // clockwise. motorValues outside range are truncated to
+            // within range
+            if (motorValue >=0) {
+                motor3DirectionPin=1;
+            } else {
+                motor3DirectionPin=0;
+            }
+            if (fabs(motorValue)>1) {
+                motor3MagnitudePin = 1;
+            } else {
+                motor3MagnitudePin = fabs(motorValue);
+            }
+        }
+        */
 
     while (true) {
         GetDirections();
-        if (draailinks == true) {
+        if ((draailinks == true) && (draairechts == false)) {
             M1_Speed = 0.5;
-            M1_Rotate = 0;
-        } else if (draairechts == true) {
+            M1_rotate = 0;
+        } else if ((draairechts == true) && (draailinks == false)) {
             M1_Speed = 0.5;
-            M1_Rotate = 1;
+            M1_rotate = 1;
         } else {
             M1_Speed = 0;
         }
 
 
 
+
         /*
         float M2 = M2_rotate.read();
         float potje1 = pot1.read();
@@ -142,6 +169,6 @@
         wait(0.1);
         pc.printf("motor 1 %f, motor 2 %f, motor 3 %f \n \r ", potje1, M2, potje2);
         */
-        
+
     }
 }