David Whitney / Mbed 2 deprecated adrianrobotproject

ble controlled magician robot

Dependencies:   BLE_nRF8001 Motordriver mbed

main.cpp@0:1be634fcf3e8, 2015-04-16 (annotated)

Committer:
K_J
Date:
Thu Apr 16 21:07:54 2015 +0000
Revision:
0:1be634fcf3e8
upload

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
K_J 0:1be634fcf3e8 14 SPI spi(p5, p6, p7);
K_J 0:1be634fcf3e8 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
K_J 0:1be634fcf3e8 19 //Motor Pins
K_J 0:1be634fcf3e8 20 Motor left(p21, p22, p23, 1); //pwm fwd ref has brake feature
K_J 0:1be634fcf3e8 21 Motor right(p26,p25,p24, 1);
K_J 0:1be634fcf3e8 22
K_J 0:1be634fcf3e8 23 //maybe ir sensors
K_J 0:1be634fcf3e8 24
K_J 0:1be634fcf3e8 25 /*----- BLE Utility -------------------------------------------------------------------------*/
K_J 0:1be634fcf3e8 26 // create peripheral instance, see pinouts above
K_J 0:1be634fcf3e8 27 BLEPeripheral blePeripheral = BLEPeripheral(&BLE_REQ, &BLE_RDY, &BLE_RST);
K_J 0:1be634fcf3e8 28
K_J 0:1be634fcf3e8 29 // create service
K_J 0:1be634fcf3e8 30 BLEService uartService = BLEService("713d0000503e4c75ba943148f18d941e");
K_J 0:1be634fcf3e8 31
K_J 0:1be634fcf3e8 32 // create characteristic
K_J 0:1be634fcf3e8 33 BLECharacteristic txCharacteristic = BLECharacteristic("713d0002503e4c75ba943148f18d941e", BLENotify, 20);
K_J 0:1be634fcf3e8 34 BLECharacteristic rxCharacteristic = BLECharacteristic("713d0003503e4c75ba943148f18d941e", BLEWriteWithoutResponse, 20);
K_J 0:1be634fcf3e8 35 /*--------------------------------------------------------------------------------------------*/
K_J 0:1be634fcf3e8 36
K_J 0:1be634fcf3e8 37
K_J 0:1be634fcf3e8 38 unsigned char buf[16] = {0};
K_J 0:1be634fcf3e8 39 unsigned char len = 0;
K_J 0:1be634fcf3e8 40
K_J 0:1be634fcf3e8 41
K_J 0:1be634fcf3e8 42 int main()
K_J 0:1be634fcf3e8 43 {
K_J 0:1be634fcf3e8 44
K_J 0:1be634fcf3e8 45 serial.baud(9600);
K_J 0:1be634fcf3e8 46 serial.printf("Serial begin!\r\n");
K_J 0:1be634fcf3e8 47
K_J 0:1be634fcf3e8 48 /*----- BLE Utility ---------------------------------------------*/
K_J 0:1be634fcf3e8 49 // set advertised local name and service UUID
K_J 0:1be634fcf3e8 50 blePeripheral.setLocalName("BLE Shield");
K_J 0:1be634fcf3e8 51
K_J 0:1be634fcf3e8 52 blePeripheral.setAdvertisedServiceUuid(uartService.uuid());
K_J 0:1be634fcf3e8 53
K_J 0:1be634fcf3e8 54 // add service and characteristic
K_J 0:1be634fcf3e8 55 blePeripheral.addAttribute(uartService);
K_J 0:1be634fcf3e8 56 blePeripheral.addAttribute(rxCharacteristic);
K_J 0:1be634fcf3e8 57 blePeripheral.addAttribute(txCharacteristic);
K_J 0:1be634fcf3e8 58
K_J 0:1be634fcf3e8 59 // begin initialization
K_J 0:1be634fcf3e8 60 blePeripheral.begin();
K_J 0:1be634fcf3e8 61 /*---------------------------------------------------------------*/
K_J 0:1be634fcf3e8 62 serial.printf("BLE UART Peripheral begin!\r\n");
K_J 0:1be634fcf3e8 63
K_J 0:1be634fcf3e8 64 while(1)
K_J 0:1be634fcf3e8 65 {
K_J 0:1be634fcf3e8 66 BLECentral central = blePeripheral.central();
K_J 0:1be634fcf3e8 67
K_J 0:1be634fcf3e8 68 if (central)
K_J 0:1be634fcf3e8 69 {
K_J 0:1be634fcf3e8 70 // central connected to peripheral
K_J 0:1be634fcf3e8 71 serial.printf("Connected to central\r\n");
K_J 0:1be634fcf3e8 72
K_J 0:1be634fcf3e8 73 int count = 0;
K_J 0:1be634fcf3e8 74 float leftval = 0;
K_J 0:1be634fcf3e8 75 float rightval = 0;
K_J 0:1be634fcf3e8 76 while (central.connected())
K_J 0:1be634fcf3e8 77 {
K_J 0:1be634fcf3e8 78 // central still connected to peripheral
K_J 0:1be634fcf3e8 79 if (rxCharacteristic.written())
K_J 0:1be634fcf3e8 80 {
K_J 0:1be634fcf3e8 81 count = 0;
K_J 0:1be634fcf3e8 82 unsigned char len = rxCharacteristic.valueLength();
K_J 0:1be634fcf3e8 83 const unsigned char *val = rxCharacteristic.value();
K_J 0:1be634fcf3e8 84
K_J 0:1be634fcf3e8 85 //1 = north, 2 = south, 3 = east, 4 = west
K_J 0:1be634fcf3e8 86 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));
K_J 0:1be634fcf3e8 87 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));
K_J 0:1be634fcf3e8 88
K_J 0:1be634fcf3e8 89 if (leftval > 1) leftval = 1;
K_J 0:1be634fcf3e8 90 if (leftval < -1) leftval = -1;
K_J 0:1be634fcf3e8 91 if (rightval > 1) rightval = 1;
K_J 0:1be634fcf3e8 92 if (rightval < -1) rightval = -1;
K_J 0:1be634fcf3e8 93
K_J 0:1be634fcf3e8 94 left.speed(leftval);
K_J 0:1be634fcf3e8 95 right.speed(rightval);
K_J 0:1be634fcf3e8 96
K_J 0:1be634fcf3e8 97 serial.printf("%f, %f\n", leftval, rightval);
K_J 0:1be634fcf3e8 98 }
K_J 0:1be634fcf3e8 99 }
K_J 0:1be634fcf3e8 100 // central disconnected
K_J 0:1be634fcf3e8 101 serial.printf("Disconnected from central\r\n");
K_J 0:1be634fcf3e8 102 }
K_J 0:1be634fcf3e8 103 }
K_J 0:1be634fcf3e8 104 }