Revision 6:916dc537bada, committed 2015-10-15

Comitter:

ccostin

Date:

Thu Oct 15 20:58:08 2015 +0000

Parent:

5:8583f3df3514

Commit message:

Zumo robot with FDRM-KL25Z; + bluetooth controller

Changed in this revision

diff -r 8583f3df3514 -r 916dc537bada main.cpp
--- a/main.cpp	Sun Dec 21 12:47:02 2014 +0000
+++ b/main.cpp	Thu Oct 15 20:58:08 2015 +0000
@@ -16,11 +16,18 @@
 DigitalOut LEDBLUE(LED_BLUE);
 DigitalOut LEDGREEN(LED_GREEN);
 
+//AnalogIn SEN0(PTA5);
+//AnalogIn SEN1(PTB2);
+DigitalIn SEN2(PTB2);
+//AnalogIn SEN3(PTD2); // aici cred ca e conflict cu bluetooth
+//AnalogIn SEN4(PTB3);
+//AnalogIn SEN5(PTA4);
+
 Buzzer buzzer(PTA12);
 ZumoRobotManager zumoRobotManager;
 
 Timer inputTimer;
-Serial bluetooth(PTC4, PTC3);
+Serial bluetooth(PTD3, PTD2);
 
 /**
  *  The string received from bluetooth
@@ -85,9 +92,39 @@
         if (inputTimer.read_ms() >= 10 && bluetooth.readable()) {
             inputTimer.reset();
             bluetooth.gets(bluetoothInput, 15);
+            
+            bluetooth.printf("Am primit: %s", bluetoothInput);
+            
+            
+            if (bluetoothInput[0] == 'q') {
+                zumoRobotManager.setVelocityX(0);
+                zumoRobotManager.setVelocityY(0);
+                bluetooth.printf("stop\n");
+            } else if (bluetoothInput[0] == 'w') {
+                zumoRobotManager.setVelocityX(velocity);
+                zumoRobotManager.setVelocityY(0);
+                bluetooth.printf("inainte\n");
+            } else if (bluetoothInput[0] == 's') {
+                zumoRobotManager.setVelocityX(-velocity);
+                zumoRobotManager.setVelocityY(0);
+                bluetooth.printf("inapoi\n");
+            } else if (bluetoothInput[0] == 'd') {
+                zumoRobotManager.setVelocityX(0);
+                zumoRobotManager.setVelocityY(velocity);
+                bluetooth.printf("dreapta\n");
+            } else if (bluetoothInput[0] == 'a') {
+                zumoRobotManager.setVelocityX(0);
+                zumoRobotManager.setVelocityY(-velocity);
+                bluetooth.printf("stanga\n");
+            } else if (bluetoothInput[0] == 'r') {
+                bluetooth.printf("Senzor: (%i)\n", SEN2.read());
+                //bluetooth.printf("Senzor: (%i, %i, %i, %i, %i, %i)\n", SEN0.read(), SEN1.read(), SEN2.read(), SEN3.read(), SEN4.read(), SEN5.read());
+                //bluetooth.printf("Senzor1: (%i, %i, %i, %i, %i, %i)\n", SEN0, SEN1, SEN2, SEN3, SEN4, SEN5); 
+            }
 
+            
             // Checking the type of input received
-            if ((bluetoothInput[0] == '+' || bluetoothInput[0] == '-') &&
+            /*if ((bluetoothInput[0] == '+' || bluetoothInput[0] == '-') &&
                     (bluetoothInput[1] == '+' || bluetoothInput[1] == '-') &&
                     bluetoothInput[2] >= '0' && bluetoothInput[2] <='9' &&
                     bluetoothInput[3] >= '0' && bluetoothInput[3] <='9' &&
@@ -103,46 +140,19 @@
                 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);
-                }  else if (colour == 'g') {
-                    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;
-                    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') {
-                    // Connection should be ended
-                    break;
-                }
-            }
+            } */
         }
     }
 
     // Connection was terminated, retrieving the "locked" status
-    waitForPassword();
+    //waitForPassword();
 }
 
 void init()
 {
 
     // Initialising the robot
+    velocity = 2;
 
     // Turning off the leds
     LEDBLUE = RGBLED_OFF;
@@ -150,7 +160,8 @@
     LEDGREEN = RGBLED_OFF;
 
     // Setting the bluetoot's baud rate for communication
-    bluetooth.baud(9600);
+    bluetooth.baud(19200);
+//    bluetooth.baud(9600);
 }
 
 int main()
@@ -159,7 +170,8 @@
     // Calling init
     init();
     // Locking the robot
-    waitForPassword();
+    update ();
+    //waitForPassword();
 
     return 0;
 }