Diff: main.cpp

Revision:

2:4fb733fffd1f

Parent:

1:833c73834749

Child:

3:dfb4b8d37bab

--- a/main.cpp	Wed Oct 12 14:35:38 2016 +0000
+++ b/main.cpp	Fri Oct 14 09:03:35 2016 +0000
@@ -10,6 +10,8 @@
 DigitalOut M3_rotate(D7);   // encoder side pot 1 spatel rotation
 PwmOut M3_Speed(D6);        // encoder side pot 1 spatel rotation
 
+DigitalIn links(SW3);
+DigitalIn rechts(SW2);
 //DigitalOut M2_rotate(D6);
 //PwmOut M2_Speed(D7);
 
@@ -18,8 +20,6 @@
 
 MODSERIAL pc(USBTX, USBRX);
 
-DigitalIn sw2(SW2); // motor 2 off/on
-
 int main()
 {
     pc.baud(115200);
@@ -27,12 +27,113 @@
     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;
+        }
+        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;
+        }
+
+        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;
+        }
+        return draailinks;
+        return draairechts;
+    }
 
     while (true) {
-        
-        if (sw2 == 1) {
-            M2_Speed = 1;
+        GetDirections();
+        if (draailinks == true) {
+            M1_Speed = 0.5;
+            M1_Rotate = 0;
+        } else if (draairechts == true) {
+            M1_Speed = 0.5;
+            M1_Rotate = 1;
+        } else {
+            M1_Speed = 0;
         }
+
+
+
+        /*
         float M2 = M2_rotate.read();
         float potje1 = pot1.read();
         float potje2 = pot2.read();
@@ -40,5 +141,7 @@
         M3_Speed.write(potje2);
         wait(0.1);
         pc.printf("motor 1 %f, motor 2 %f, motor 3 %f \n \r ", potje1, M2, potje2);
+        */
+        
     }
 }