Adrian Winata / Mbed 2 deprecated 4180_Project_BluetoothRCBot

Use a simple iOS application to control a Magician Robot via Bluetooth LE. A virtual joystick is used on the iOS device to manipulate two differentially controlled PWM motors on the Magician Robot. Three degrees of freedom is possible (any direction on the cartesian coordinate, rotation about an axis, and speed control).

Dependencies:   BLE_nRF8001 Motordriver mbed

Fork of adrianrobotproject by David Whitney

main.cpp@1:81cca0074c0d, 2015-04-22 (annotated)

Committer:
awinata
Date:
Wed Apr 22 23:19:46 2015 +0000
Revision:
1:81cca0074c0d
Parent:
0:1be634fcf3e8 
First Release

Who changed what in which revision?

UserRevisionLine numberNew contents of line
K_J 0:1be634fcf3e8 1 #include "mbed.h"
K_J 0:1be634fcf3e8 2 #include "BLEPeripheral.h"
K_J 0:1be634fcf3e8 3 #include "motordriver.h"
K_J 0:1be634fcf3e8 4 #include "stdint.h"
K_J 0:1be634fcf3e8 5
K_J 0:1be634fcf3e8 6 //*******************************
K_J 0:1be634fcf3e8 7 // Must define Serial serial and SPI spi
K_J 0:1be634fcf3e8 8 // BLE_RDY and BLE_REQ
K_J 0:1be634fcf3e8 9 //*******************************
K_J 0:1be634fcf3e8 10
K_J 0:1be634fcf3e8 11 Serial serial(USBTX, USBRX);
K_J 0:1be634fcf3e8 12 // The SPI construct, REQN and RDYN IO construct should be modified manually
K_J 0:1be634fcf3e8 13 // It depend on the board you are using and the REQN&RDYN configuration on BLE Shield
awinata 1:81cca0074c0d 14 SPI spi(p5, p6, p7);
awinata 1:81cca0074c0d 15 DigitalInOut BLE_RDY(p8);
K_J 0:1be634fcf3e8 16 DigitalInOut BLE_REQ(p9);
K_J 0:1be634fcf3e8 17 DigitalInOut BLE_RST(p10);
K_J 0:1be634fcf3e8 18
awinata 1:81cca0074c0d 19 // Motor pins
awinata 1:81cca0074c0d 20 Motor left(p21, p22, p23, 1); // pwm, fwd, rev, has brake feature
K_J 0:1be634fcf3e8 21 Motor right(p26,p25,p24, 1);
K_J 0:1be634fcf3e8 22
awinata 1:81cca0074c0d 23 // IR sensors
awinata 1:81cca0074c0d 24 //AnalogIn ir_c(p20); // center
awinata 1:81cca0074c0d 25 //AnalogIn ir_l(p19); // left
awinata 1:81cca0074c0d 26 //AnalogIn ir_r(p18); // right
K_J 0:1be634fcf3e8 27
K_J 0:1be634fcf3e8 28 /*----- BLE Utility -------------------------------------------------------------------------*/
K_J 0:1be634fcf3e8 29 // create peripheral instance, see pinouts above
K_J 0:1be634fcf3e8 30 BLEPeripheral blePeripheral = BLEPeripheral(&BLE_REQ, &BLE_RDY, &BLE_RST);
awinata 1:81cca0074c0d 31
K_J 0:1be634fcf3e8 32 // create service
K_J 0:1be634fcf3e8 33 BLEService uartService = BLEService("713d0000503e4c75ba943148f18d941e");
awinata 1:81cca0074c0d 34
K_J 0:1be634fcf3e8 35 // create characteristic
K_J 0:1be634fcf3e8 36 BLECharacteristic txCharacteristic = BLECharacteristic("713d0002503e4c75ba943148f18d941e", BLENotify, 20);
K_J 0:1be634fcf3e8 37 BLECharacteristic rxCharacteristic = BLECharacteristic("713d0003503e4c75ba943148f18d941e", BLEWriteWithoutResponse, 20);
K_J 0:1be634fcf3e8 38 /*--------------------------------------------------------------------------------------------*/
K_J 0:1be634fcf3e8 39
awinata 1:81cca0074c0d 40 // Bluetooth variables
K_J 0:1be634fcf3e8 41 unsigned char buf[16] = {0};
K_J 0:1be634fcf3e8 42 unsigned char len = 0;
K_J 0:1be634fcf3e8 43
awinata 1:81cca0074c0d 44 // Motor variables
awinata 1:81cca0074c0d 45 float leftval = 0;
awinata 1:81cca0074c0d 46 float rightval = 0;
K_J 0:1be634fcf3e8 47
K_J 0:1be634fcf3e8 48 int main()
K_J 0:1be634fcf3e8 49 {
awinata 1:81cca0074c0d 50 //serial.baud(9600);
awinata 1:81cca0074c0d 51 //serial.printf("Serial begin!\r\n");
awinata 1:81cca0074c0d 52
awinata 1:81cca0074c0d 53 /*----- BLE Utility ---------------------------------------------*/
awinata 1:81cca0074c0d 54 // set advertised local name and service UUID
awinata 1:81cca0074c0d 55 blePeripheral.setLocalName("BLE Shield");
awinata 1:81cca0074c0d 56
awinata 1:81cca0074c0d 57 blePeripheral.setAdvertisedServiceUuid(uartService.uuid());
awinata 1:81cca0074c0d 58
awinata 1:81cca0074c0d 59 // add service and characteristic
awinata 1:81cca0074c0d 60 blePeripheral.addAttribute(uartService);
awinata 1:81cca0074c0d 61 blePeripheral.addAttribute(rxCharacteristic);
awinata 1:81cca0074c0d 62 blePeripheral.addAttribute(txCharacteristic);
awinata 1:81cca0074c0d 63
awinata 1:81cca0074c0d 64 // begin initialization
awinata 1:81cca0074c0d 65 blePeripheral.begin();
awinata 1:81cca0074c0d 66 //serial.printf("BLE UART Peripheral begin!\r\n");
awinata 1:81cca0074c0d 67 /*---------------------------------------------------------------*/
awinata 1:81cca0074c0d 68
awinata 1:81cca0074c0d 69 while(1) {
awinata 1:81cca0074c0d 70 BLECentral central = blePeripheral.central();
awinata 1:81cca0074c0d 71
awinata 1:81cca0074c0d 72 if(central) {
awinata 1:81cca0074c0d 73 // central connected to peripheral
awinata 1:81cca0074c0d 74 //serial.printf("Connected to central\r\n");
awinata 1:81cca0074c0d 75
awinata 1:81cca0074c0d 76 while (central.connected()) {
awinata 1:81cca0074c0d 77 // central still connected to peripheral
awinata 1:81cca0074c0d 78 if (rxCharacteristic.written()) {
awinata 1:81cca0074c0d 79 unsigned char len = rxCharacteristic.valueLength();
awinata 1:81cca0074c0d 80 const unsigned char *val = rxCharacteristic.value();
awinata 1:81cca0074c0d 81
awinata 1:81cca0074c0d 82 // Calculate differential motor values from cartesian joystick coordinates
awinata 1:81cca0074c0d 83 // 0 = north, 1 = south, 2 = east, 3 = west
awinata 1:81cca0074c0d 84 leftval = sqrt(pow((float)val[0]/255,2) + pow((float)val[2]/255,2)) - sqrt(pow((float)val[1]/255,2) + pow((float)val[3]/255,2));
awinata 1:81cca0074c0d 85 rightval = sqrt(pow((float)val[0]/255,2) + pow((float)val[3]/255,2)) - sqrt(pow((float)val[1]/255,2) + pow((float)val[2]/255,2));
awinata 1:81cca0074c0d 86
awinata 1:81cca0074c0d 87 // Windup compensation
awinata 1:81cca0074c0d 88 if (leftval > 1) leftval = 1;
awinata 1:81cca0074c0d 89 if (leftval < -1) leftval = -1;
awinata 1:81cca0074c0d 90 if (rightval > 1) rightval = 1;
awinata 1:81cca0074c0d 91 if (rightval < -1) rightval = -1;
awinata 1:81cca0074c0d 92
awinata 1:81cca0074c0d 93 // Set motor speed values
awinata 1:81cca0074c0d 94 left.speed(leftval);
awinata 1:81cca0074c0d 95 right.speed(rightval);
awinata 1:81cca0074c0d 96
awinata 1:81cca0074c0d 97 //serial.printf("%f, %f\n", leftval, rightval);
awinata 1:81cca0074c0d 98 }
awinata 1:81cca0074c0d 99 }
awinata 1:81cca0074c0d 100 // central disconnected
awinata 1:81cca0074c0d 101 //serial.printf("Disconnected from central\r\n");
K_J 0:1be634fcf3e8 102 }
K_J 0:1be634fcf3e8 103 }
K_J 0:1be634fcf3e8 104 }