Diff: main.cpp

Revision:

5:8583f3df3514

Parent:

4:9bed0d58673d

Child:

6:916dc537bada

diff -r 9bed0d58673d -r 8583f3df3514 main.cpp
--- a/main.cpp	Thu Dec 11 20:52:47 2014 +0000
+++ b/main.cpp	Sun Dec 21 12:47:02 2014 +0000
@@ -3,6 +3,7 @@
 //   © 2014 Catalin Craciun
 
 #include "mbed.h"
+#include "rtos.h"
 #include "Buzzer.h"
 #include "ZumoRobotManager.h"
 
@@ -15,7 +16,7 @@
 DigitalOut LEDBLUE(LED_BLUE);
 DigitalOut LEDGREEN(LED_GREEN);
 
-Buzzer buzzer(PTC8);
+Buzzer buzzer(PTA12);
 ZumoRobotManager zumoRobotManager;
 
 Timer inputTimer;
@@ -37,105 +38,124 @@
 void update();
 /**
  *  Method for initialising the robot
- *  It initialises the bluetooth's baud rate, the leds, creates a new instance of ZumoRobotManager and sets the 
+ *  It initialises the bluetooth's baud rate, the leds, creates a new instance of ZumoRobotManager and sets the
  * timer for controlling the bluetooth's transmission flow
  */
 void init();
 
-void waitForPassword() {
+void waitForPassword()
+{
 
     // Turning off the leds
-    LEDBLUE = RGBLED_OFF; LEDRED = RGBLED_OFF; LEDGREEN = RGBLED_OFF;
+    LEDBLUE = RGBLED_OFF;
+    LEDRED = RGBLED_OFF;
+    LEDGREEN = RGBLED_OFF;
     while (true) {
         // Waiting to read the password of the bluetooth
         if (bluetooth.readable()) {
             bluetooth.gets(bluetoothInput, 15);
+
             if (zumoRobotManager.checkPassword(bluetoothInput)) {
                 // Sending a "successfuly message" back to the device which sent the password
                 bluetooth.printf("$suc");
                 break;
             } else {
                 // Sending a "failure message" back to the device which sent the password
-                bluetooth.printf("$fai");   
+                bluetooth.printf("$fai");
             }
         }
     }
-    
+
     update();
 }
 
-void update() {
-    
+void update()
+{
+
     // Updating loop
 
     // Setting the timer for controlling the bluetooth flow
-    inputTimer.start(); Serial pc(USBTX, USBRX);
+    inputTimer.start();
+    Serial pc(USBTX, USBRX);
     // Lighting the green led
-    LEDBLUE = RGBLED_OFF; LEDRED = RGBLED_OFF; LEDGREEN = RGBLED_ON;
+    LEDBLUE = RGBLED_OFF;
+    LEDRED = RGBLED_OFF;
+    LEDGREEN = RGBLED_ON;
     while (true) {
         if (inputTimer.read_ms() >= 10 && bluetooth.readable()) {
             inputTimer.reset();
             bluetooth.gets(bluetoothInput, 15);
+
             // Checking the type of input received
-            if ((bluetoothInput[0] == '+' || bluetoothInput[0] == '-') && 
-                (bluetoothInput[1] == '+' || bluetoothInput[1] == '-') &&
-                bluetoothInput[2] >= '0' && bluetoothInput[2] <='9' &&
-                bluetoothInput[3] >= '0' && bluetoothInput[3] <='9' &&
-                bluetoothInput[4] >= '0' && bluetoothInput[4] <='9' &&
-                bluetoothInput[5] >= '0' && bluetoothInput[5] <='9') {
-                    /// Got a "velocity descriptor" string
-                    velocity = ((bluetoothInput[2] - '0') * 10.0f + (bluetoothInput[3] - '0') * 1.0f) * 0.01f;
-                    if (bluetoothInput[0] == '-')
-                        velocity = -velocity;
-                    zumoRobotManager.setVelocityX(velocity);
-                    
-                    velocity = ((bluetoothInput[4] - '0') * 10.0f + (bluetoothInput[5] - '0') * 1.0f) * 0.01f;
-                    if (bluetoothInput[1] == '-')
-                        velocity = -velocity;
-                    zumoRobotManager.setVelocityY(velocity);
+            if ((bluetoothInput[0] == '+' || bluetoothInput[0] == '-') &&
+                    (bluetoothInput[1] == '+' || bluetoothInput[1] == '-') &&
+                    bluetoothInput[2] >= '0' && bluetoothInput[2] <='9' &&
+                    bluetoothInput[3] >= '0' && bluetoothInput[3] <='9' &&
+                    bluetoothInput[4] >= '0' && bluetoothInput[4] <='9' &&
+                    bluetoothInput[5] >= '0' && bluetoothInput[5] <='9') {
+                /// Got a "velocity descriptor" string
+                velocity = ((bluetoothInput[2] - '0') * 10.0f + (bluetoothInput[3] - '0') * 1.0f) * 0.01f;
+                if (bluetoothInput[0] == '-')
+                    velocity = -velocity;
+                zumoRobotManager.setVelocityX(velocity);
+
+                velocity = ((bluetoothInput[4] - '0') * 10.0f + (bluetoothInput[5] - '0') * 1.0f) * 0.01f;
+                if (bluetoothInput[1] == '-')
+                    velocity = -velocity;
+                zumoRobotManager.setVelocityY(velocity);
             } else if (bluetoothInput[0] == 'c' &&
                        bluetoothInput[1] == '$') {
                 // Got a "colour macro command" (macro command referring to the color of the RGB led on robot)
                 char colour = bluetoothInput[2];
-                if (colour == 'r') { 
-                    LEDRED = RGBLED_ON; LEDBLUE = RGBLED_OFF; LEDGREEN = RGBLED_OFF;  
-                    buzzer.startBeep(1000, 0.25); 
+                if (colour == 'r') {
+                    LEDRED = RGBLED_ON;
+                    LEDBLUE = RGBLED_OFF;
+                    LEDGREEN = RGBLED_OFF;
+                    buzzer.startBeep(1000, 0.25);
                 }  else if (colour == 'g') {
-                    LEDRED = RGBLED_OFF; LEDBLUE = RGBLED_OFF; LEDGREEN = RGBLED_ON;
+                    LEDRED = RGBLED_OFF;
+                    LEDBLUE = RGBLED_OFF;
+                    LEDGREEN = RGBLED_ON;
                     buzzer.startBeep(1000, 0.25);
                 } else if (colour == 'b') {
-                    LEDRED = RGBLED_OFF; LEDBLUE = RGBLED_ON; LEDGREEN = RGBLED_OFF;
+                    LEDRED = RGBLED_OFF;
+                    LEDBLUE = RGBLED_ON;
+                    LEDGREEN = RGBLED_OFF;
                     buzzer.startBeep(1000, 0.25);
-                }  
+                }
             } else if (bluetoothInput[0] == '$') {
                 // Received a "macro command" (macro command = a command controlling the status of the device or so)
                 if (bluetoothInput[1] == 'o' &&
-                    bluetoothInput[2] == 'u' &&
-                    bluetoothInput[3] == 't') {
+                        bluetoothInput[2] == 'u' &&
+                        bluetoothInput[3] == 't') {
                     // Connection should be ended
-                    break;   
+                    break;
                 }
             }
         }
     }
-    
+
     // Connection was terminated, retrieving the "locked" status
     waitForPassword();
 }
 
-void init() {
+void init()
+{
 
     // Initialising the robot
-    
+
     // Turning off the leds
-    LEDBLUE = RGBLED_OFF; LEDRED = RGBLED_OFF; LEDGREEN = RGBLED_OFF;
+    LEDBLUE = RGBLED_OFF;
+    LEDRED = RGBLED_OFF;
+    LEDGREEN = RGBLED_OFF;
 
     // Setting the bluetoot's baud rate for communication
     bluetooth.baud(9600);
 }
 
-int main() {
-    
+int main()
+{
+
     // Calling init
     init();
     // Locking the robot