Diff: main.cpp

Revision:

3:3a671d01bcb8

Parent:

2:ad4b181a6422

Child:

4:e62a2df0a5b5

--- a/main.cpp	Wed Oct 19 11:03:34 2016 +0000
+++ b/main.cpp	Wed Oct 19 13:13:13 2016 +0000
@@ -1,4 +1,5 @@
 #include "mbed.h"
+#include "motordriver.h"
 
 // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = >
 // Serial communication using MODSERIAL
@@ -13,7 +14,7 @@
 
 // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = >
 // constants
-const int kLedOn = 0;               // LED on if 0
+const int LedOn = 0;               // LED on if 0
 
 // LEDs
 DigitalOut led_r(LED_RED);
@@ -29,53 +30,62 @@
 int num_turned_on_2 = 0;            // count number of times blue LED turned on
 
 
+//Safety for Motor
+int Brakeable; //cna the motor driver break
+int sign; //prevents throwing the motor from full foward to full reverse and stuff melting.
 
-/**
- * Toggle / Switch / Blink the state of the LED.
- * Count the number of times the LED was turned ON.
- * We have quite some duplicate code here; this is not ideal and could be solved
- * using more 'advanced' concepts such as arrays and enums. 
- * However, this is outside the scope of this course for now.
- */
-void ToggleLed()
+//Motor
+Motor A(D6, D7, D7, Brakeable); // pwm, fwd, rev, brake                 rechts
+Motor B(D5, D4, D4, Brakeable); // pwm, fwd, rev, brake                 links
+
+
+
+
+void SwitchPart()
 {
     switch (part_id) {
-        case 0: {
+        case 2: {
             // LED 0 to toggle and count:
-            led_r = kLedOn;
-            if (led_r == kLedOn) {
+            led_r = LedOn;
+            if (led_r == LedOn) {
                 num_turned_on_0++;
             }
             
             // LEDs to turn off:
-            led_g = not kLedOn;
-            led_b = not kLedOn;
+            led_g = not LedOn;
+            led_b = not LedOn;
+            A.speed(0) == 0;
+            B.speed(0) == 0;
             
             break;
         }
-        case 1: {
+        case 3: {
             // LED 1 to toggle and count:
-            led_g = kLedOn;
-            if (led_g == kLedOn) {
+            led_g = LedOn;
+            A.speed(0.5) == 0.5;
+            if (led_g == LedOn) {
                 num_turned_on_1++;
             }
             
             // LEDs to turn off:
-            led_r = not kLedOn;
-            led_b = not kLedOn;
-            
+            led_r = not LedOn;
+            led_b = not LedOn;
+            B.speed(0) == 0;
+                        
             break;
         }
-        case 2: {
+        case 4: {
             // LED 2 to toggle and count:
-            led_b = kLedOn;
-            if (led_b == kLedOn) {
+            led_b = LedOn;
+            B.speed(0.5) == 0.5;
+            if (led_b == LedOn) {
                 num_turned_on_2++;
             }
             
             // LEDs to turn off:
-            led_r = not kLedOn;
-            led_g = not kLedOn;
+            led_r = not LedOn;
+            led_g = not LedOn;
+            A.speed(0) == 0;
             
             break;
         }
@@ -97,16 +107,16 @@
  * @ensure led_blink_id = ++led_blink_id % kNumStates
  */
 void SetValue2() {
-    part_id = 0;
+    part_id = 2;
    }
 
 void SetValue3() {
-    part_id = 1;
+    part_id = 3;
    }
 
 
 void SetValue4() {
-    part_id = 2;
+    part_id = 4;
 }
 
 
@@ -120,27 +130,27 @@
     pc.printf("\r\n**RESET**\r\n");
     
     // Turn off all LEDs initially
-    led_r = not kLedOn;
-    led_g = not kLedOn;
-    led_b = not kLedOn;
+    led_r = not LedOn;
+    led_g = not LedOn;
+    led_b = not LedOn;
     
     // Create ticker and attach LED toggle function
     Ticker tick_toggle_led;
-    tick_toggle_led.attach(&ToggleLed,kTimeLedToggle);
+    tick_toggle_led.attach(&SwitchPart,kTimeLedToggle);
     
     // Create ticker and attach Print function
     Ticker tick_print_serial;
     tick_print_serial.attach(&PrintSerial,kTimePrintSerial);
     
     // Create interrupt and attach switch function
-    InterruptIn btn_base(D2);
-    btn_base.fall(&SetValue2);
+    InterruptIn btn_cart(D2);
+    btn_cart.fall(&SetValue2);
     
-    InterruptIn btn_cart(D3);
-    btn_cart.fall(&SetValue3);
+    InterruptIn btn_arm(D3);
+    btn_arm.fall(&SetValue3);
 
     InterruptIn btn_arm(D4);
-    btn_arm.fall(&SetValue4);
+    btn_claw.fall(&SetValue4);
 
     while (true);
 }