Constantin Eugen Costescu / Fsl_Zumo

FRDM KL25Z bluetooth controller

Dependencies:   ZumoRobotUtilities mbed-rtos mbed

Fork of ZumoRobotBluetoothControlled by Catalin Craciun

main.cpp@6:916dc537bada, 2015-10-15 (annotated)

Committer:
ccostin
Date:
Thu Oct 15 20:58:08 2015 +0000
Revision:
6:916dc537bada
Parent:
5:8583f3df3514 
Zumo robot with FDRM-KL25Z; + bluetooth controller

Who changed what in which revision?

UserRevisionLine numberNew contents of line
catalincraciun7 1:bac719753a33 1 // Craciun Catalin
catalincraciun7 0:827b012f1913 2 // main.h
catalincraciun7 0:827b012f1913 3 // © 2014 Catalin Craciun
catalincraciun7 0:827b012f1913 4
catalincraciun7 0:827b012f1913 5 #include "mbed.h"
catalincraciun7 5:8583f3df3514 6 #include "rtos.h"
catalincraciun7 4:9bed0d58673d 7 #include "Buzzer.h"
catalincraciun7 0:827b012f1913 8 #include "ZumoRobotManager.h"
catalincraciun7 0:827b012f1913 9
catalincraciun7 2:53d1b48743a3 10 using namespace std;
catalincraciun7 2:53d1b48743a3 11
catalincraciun7 0:827b012f1913 12 #define RGBLED_ON 0
catalincraciun7 0:827b012f1913 13 #define RGBLED_OFF 1
catalincraciun7 0:827b012f1913 14
catalincraciun7 0:827b012f1913 15 DigitalOut LEDRED(LED_RED);
catalincraciun7 0:827b012f1913 16 DigitalOut LEDBLUE(LED_BLUE);
catalincraciun7 0:827b012f1913 17 DigitalOut LEDGREEN(LED_GREEN);
catalincraciun7 0:827b012f1913 18
ccostin 6:916dc537bada 19 //AnalogIn SEN0(PTA5);
ccostin 6:916dc537bada 20 //AnalogIn SEN1(PTB2);
ccostin 6:916dc537bada 21 DigitalIn SEN2(PTB2);
ccostin 6:916dc537bada 22 //AnalogIn SEN3(PTD2); // aici cred ca e conflict cu bluetooth
ccostin 6:916dc537bada 23 //AnalogIn SEN4(PTB3);
ccostin 6:916dc537bada 24 //AnalogIn SEN5(PTA4);
ccostin 6:916dc537bada 25
catalincraciun7 5:8583f3df3514 26 Buzzer buzzer(PTA12);
catalincraciun7 4:9bed0d58673d 27 ZumoRobotManager zumoRobotManager;
catalincraciun7 2:53d1b48743a3 28
catalincraciun7 2:53d1b48743a3 29 Timer inputTimer;
ccostin 6:916dc537bada 30 Serial bluetooth(PTD3, PTD2);
catalincraciun7 2:53d1b48743a3 31
catalincraciun7 3:83ac04b7ee07 32 /**
catalincraciun7 4:9bed0d58673d 33 * The string received from bluetooth
catalincraciun7 3:83ac04b7ee07 34 */
catalincraciun7 3:83ac04b7ee07 35 char bluetoothInput[20];
catalincraciun7 2:53d1b48743a3 36 float velocity;
catalincraciun7 1:bac719753a33 37
catalincraciun7 3:83ac04b7ee07 38 /**
catalincraciun7 3:83ac04b7ee07 39 * This method waits for the robot's password in order to unlock it
catalincraciun7 3:83ac04b7ee07 40 */
catalincraciun7 3:83ac04b7ee07 41 void waitForPassword();
catalincraciun7 3:83ac04b7ee07 42 /**
catalincraciun7 3:83ac04b7ee07 43 * Method for updating the robot
catalincraciun7 3:83ac04b7ee07 44 */
catalincraciun7 3:83ac04b7ee07 45 void update();
catalincraciun7 3:83ac04b7ee07 46 /**
catalincraciun7 3:83ac04b7ee07 47 * Method for initialising the robot
catalincraciun7 5:8583f3df3514 48 * It initialises the bluetooth's baud rate, the leds, creates a new instance of ZumoRobotManager and sets the
catalincraciun7 3:83ac04b7ee07 49 * timer for controlling the bluetooth's transmission flow
catalincraciun7 3:83ac04b7ee07 50 */
catalincraciun7 3:83ac04b7ee07 51 void init();
catalincraciun7 3:83ac04b7ee07 52
catalincraciun7 5:8583f3df3514 53 void waitForPassword()
catalincraciun7 5:8583f3df3514 54 {
catalincraciun7 3:83ac04b7ee07 55
catalincraciun7 3:83ac04b7ee07 56 // Turning off the leds
catalincraciun7 5:8583f3df3514 57 LEDBLUE = RGBLED_OFF;
catalincraciun7 5:8583f3df3514 58 LEDRED = RGBLED_OFF;
catalincraciun7 5:8583f3df3514 59 LEDGREEN = RGBLED_OFF;
catalincraciun7 3:83ac04b7ee07 60 while (true) {
catalincraciun7 3:83ac04b7ee07 61 // Waiting to read the password of the bluetooth
catalincraciun7 3:83ac04b7ee07 62 if (bluetooth.readable()) {
catalincraciun7 3:83ac04b7ee07 63 bluetooth.gets(bluetoothInput, 15);
catalincraciun7 5:8583f3df3514 64
catalincraciun7 4:9bed0d58673d 65 if (zumoRobotManager.checkPassword(bluetoothInput)) {
catalincraciun7 3:83ac04b7ee07 66 // Sending a "successfuly message" back to the device which sent the password
catalincraciun7 3:83ac04b7ee07 67 bluetooth.printf("$suc");
catalincraciun7 3:83ac04b7ee07 68 break;
catalincraciun7 3:83ac04b7ee07 69 } else {
catalincraciun7 3:83ac04b7ee07 70 // Sending a "failure message" back to the device which sent the password
catalincraciun7 5:8583f3df3514 71 bluetooth.printf("$fai");
catalincraciun7 3:83ac04b7ee07 72 }
catalincraciun7 3:83ac04b7ee07 73 }
catalincraciun7 3:83ac04b7ee07 74 }
catalincraciun7 5:8583f3df3514 75
catalincraciun7 3:83ac04b7ee07 76 update();
catalincraciun7 3:83ac04b7ee07 77 }
catalincraciun7 3:83ac04b7ee07 78
catalincraciun7 5:8583f3df3514 79 void update()
catalincraciun7 5:8583f3df3514 80 {
catalincraciun7 5:8583f3df3514 81
catalincraciun7 2:53d1b48743a3 82 // Updating loop
catalincraciun7 3:83ac04b7ee07 83
catalincraciun7 3:83ac04b7ee07 84 // Setting the timer for controlling the bluetooth flow
catalincraciun7 5:8583f3df3514 85 inputTimer.start();
catalincraciun7 5:8583f3df3514 86 Serial pc(USBTX, USBRX);
catalincraciun7 3:83ac04b7ee07 87 // Lighting the green led
catalincraciun7 5:8583f3df3514 88 LEDBLUE = RGBLED_OFF;
catalincraciun7 5:8583f3df3514 89 LEDRED = RGBLED_OFF;
catalincraciun7 5:8583f3df3514 90 LEDGREEN = RGBLED_ON;
catalincraciun7 2:53d1b48743a3 91 while (true) {
catalincraciun7 2:53d1b48743a3 92 if (inputTimer.read_ms() >= 10 && bluetooth.readable()) {
catalincraciun7 2:53d1b48743a3 93 inputTimer.reset();
catalincraciun7 3:83ac04b7ee07 94 bluetooth.gets(bluetoothInput, 15);
ccostin 6:916dc537bada 95
ccostin 6:916dc537bada 96 bluetooth.printf("Am primit: %s", bluetoothInput);
ccostin 6:916dc537bada 97
ccostin 6:916dc537bada 98
ccostin 6:916dc537bada 99 if (bluetoothInput[0] == 'q') {
ccostin 6:916dc537bada 100 zumoRobotManager.setVelocityX(0);
ccostin 6:916dc537bada 101 zumoRobotManager.setVelocityY(0);
ccostin 6:916dc537bada 102 bluetooth.printf("stop\n");
ccostin 6:916dc537bada 103 } else if (bluetoothInput[0] == 'w') {
ccostin 6:916dc537bada 104 zumoRobotManager.setVelocityX(velocity);
ccostin 6:916dc537bada 105 zumoRobotManager.setVelocityY(0);
ccostin 6:916dc537bada 106 bluetooth.printf("inainte\n");
ccostin 6:916dc537bada 107 } else if (bluetoothInput[0] == 's') {
ccostin 6:916dc537bada 108 zumoRobotManager.setVelocityX(-velocity);
ccostin 6:916dc537bada 109 zumoRobotManager.setVelocityY(0);
ccostin 6:916dc537bada 110 bluetooth.printf("inapoi\n");
ccostin 6:916dc537bada 111 } else if (bluetoothInput[0] == 'd') {
ccostin 6:916dc537bada 112 zumoRobotManager.setVelocityX(0);
ccostin 6:916dc537bada 113 zumoRobotManager.setVelocityY(velocity);
ccostin 6:916dc537bada 114 bluetooth.printf("dreapta\n");
ccostin 6:916dc537bada 115 } else if (bluetoothInput[0] == 'a') {
ccostin 6:916dc537bada 116 zumoRobotManager.setVelocityX(0);
ccostin 6:916dc537bada 117 zumoRobotManager.setVelocityY(-velocity);
ccostin 6:916dc537bada 118 bluetooth.printf("stanga\n");
ccostin 6:916dc537bada 119 } else if (bluetoothInput[0] == 'r') {
ccostin 6:916dc537bada 120 bluetooth.printf("Senzor: (%i)\n", SEN2.read());
ccostin 6:916dc537bada 121 //bluetooth.printf("Senzor: (%i, %i, %i, %i, %i, %i)\n", SEN0.read(), SEN1.read(), SEN2.read(), SEN3.read(), SEN4.read(), SEN5.read());
ccostin 6:916dc537bada 122 //bluetooth.printf("Senzor1: (%i, %i, %i, %i, %i, %i)\n", SEN0, SEN1, SEN2, SEN3, SEN4, SEN5);
ccostin 6:916dc537bada 123 }
catalincraciun7 5:8583f3df3514 124
ccostin 6:916dc537bada 125
catalincraciun7 3:83ac04b7ee07 126 // Checking the type of input received
ccostin 6:916dc537bada 127 /*if ((bluetoothInput[0] == '+' || bluetoothInput[0] == '-') &&
catalincraciun7 5:8583f3df3514 128 (bluetoothInput[1] == '+' || bluetoothInput[1] == '-') &&
catalincraciun7 5:8583f3df3514 129 bluetoothInput[2] >= '0' && bluetoothInput[2] <='9' &&
catalincraciun7 5:8583f3df3514 130 bluetoothInput[3] >= '0' && bluetoothInput[3] <='9' &&
catalincraciun7 5:8583f3df3514 131 bluetoothInput[4] >= '0' && bluetoothInput[4] <='9' &&
catalincraciun7 5:8583f3df3514 132 bluetoothInput[5] >= '0' && bluetoothInput[5] <='9') {
catalincraciun7 5:8583f3df3514 133 /// Got a "velocity descriptor" string
catalincraciun7 5:8583f3df3514 134 velocity = ((bluetoothInput[2] - '0') * 10.0f + (bluetoothInput[3] - '0') * 1.0f) * 0.01f;
catalincraciun7 5:8583f3df3514 135 if (bluetoothInput[0] == '-')
catalincraciun7 5:8583f3df3514 136 velocity = -velocity;
catalincraciun7 5:8583f3df3514 137 zumoRobotManager.setVelocityX(velocity);
catalincraciun7 5:8583f3df3514 138
catalincraciun7 5:8583f3df3514 139 velocity = ((bluetoothInput[4] - '0') * 10.0f + (bluetoothInput[5] - '0') * 1.0f) * 0.01f;
catalincraciun7 5:8583f3df3514 140 if (bluetoothInput[1] == '-')
catalincraciun7 5:8583f3df3514 141 velocity = -velocity;
catalincraciun7 5:8583f3df3514 142 zumoRobotManager.setVelocityY(velocity);
ccostin 6:916dc537bada 143 } */
catalincraciun7 2:53d1b48743a3 144 }
catalincraciun7 2:53d1b48743a3 145 }
catalincraciun7 5:8583f3df3514 146
catalincraciun7 3:83ac04b7ee07 147 // Connection was terminated, retrieving the "locked" status
ccostin 6:916dc537bada 148 //waitForPassword();
catalincraciun7 2:53d1b48743a3 149 }
catalincraciun7 2:53d1b48743a3 150
catalincraciun7 5:8583f3df3514 151 void init()
catalincraciun7 5:8583f3df3514 152 {
catalincraciun7 2:53d1b48743a3 153
catalincraciun7 3:83ac04b7ee07 154 // Initialising the robot
ccostin 6:916dc537bada 155 velocity = 2;
catalincraciun7 5:8583f3df3514 156
catalincraciun7 3:83ac04b7ee07 157 // Turning off the leds
catalincraciun7 5:8583f3df3514 158 LEDBLUE = RGBLED_OFF;
catalincraciun7 5:8583f3df3514 159 LEDRED = RGBLED_OFF;
catalincraciun7 5:8583f3df3514 160 LEDGREEN = RGBLED_OFF;
catalincraciun7 2:53d1b48743a3 161
catalincraciun7 3:83ac04b7ee07 162 // Setting the bluetoot's baud rate for communication
ccostin 6:916dc537bada 163 bluetooth.baud(19200);
ccostin 6:916dc537bada 164 // bluetooth.baud(9600);
catalincraciun7 0:827b012f1913 165 }
catalincraciun7 0:827b012f1913 166
catalincraciun7 5:8583f3df3514 167 int main()
catalincraciun7 5:8583f3df3514 168 {
catalincraciun7 5:8583f3df3514 169
catalincraciun7 3:83ac04b7ee07 170 // Calling init
catalincraciun7 3:83ac04b7ee07 171 init();
catalincraciun7 3:83ac04b7ee07 172 // Locking the robot
ccostin 6:916dc537bada 173 update ();
ccostin 6:916dc537bada 174 //waitForPassword();
catalincraciun7 0:827b012f1913 175
catalincraciun7 3:83ac04b7ee07 176 return 0;
catalincraciun7 0:827b012f1913 177 }