Adrian Winata / Mbed 2 deprecated 4180_Project_BluetoothRCBot

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

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 }