main.cpp

00001 #include "mbed.h"
00002 #include "BLEPeripheral.h"
00003 #include "motordriver.h"
00004 #include "stdint.h"
00005 
00006 //*******************************
00007 // Must define Serial serial and SPI spi
00008 // BLE_RDY and BLE_REQ
00009 //*******************************
00010 
00011 Serial serial(USBTX, USBRX);
00012 // The SPI construct, REQN and RDYN IO construct should be modified manually
00013 // It depend on the board you are using and the REQN&RDYN configuration on BLE Shield
00014 SPI spi(p5, p6, p7);
00015 DigitalInOut BLE_RDY(p8);
00016 DigitalInOut BLE_REQ(p9);
00017 DigitalInOut BLE_RST(p10);
00018 
00019 // Motor pins
00020 Motor left(p21, p22, p23, 1); // pwm, fwd, rev, has brake feature
00021 Motor right(p26,p25,p24, 1);
00022 
00023 // IR sensors
00024 //AnalogIn ir_c(p20); // center
00025 //AnalogIn ir_l(p19); // left
00026 //AnalogIn ir_r(p18); // right
00027 
00028 /*----- BLE Utility -------------------------------------------------------------------------*/
00029 // create peripheral instance, see pinouts above
00030 BLEPeripheral            blePeripheral        = BLEPeripheral(&BLE_REQ, &BLE_RDY, &BLE_RST);
00031 
00032 // create service
00033 BLEService               uartService          = BLEService("713d0000503e4c75ba943148f18d941e");
00034 
00035 // create characteristic
00036 BLECharacteristic    txCharacteristic = BLECharacteristic("713d0002503e4c75ba943148f18d941e", BLENotify, 20);
00037 BLECharacteristic    rxCharacteristic = BLECharacteristic("713d0003503e4c75ba943148f18d941e", BLEWriteWithoutResponse, 20);
00038 /*--------------------------------------------------------------------------------------------*/
00039 
00040 // Bluetooth variables
00041 unsigned char buf[16] = {0};
00042 unsigned char len = 0;
00043 
00044 // Motor variables
00045 float leftval = 0;
00046 float rightval = 0;
00047 
00048 int main()
00049 {
00050     //serial.baud(9600);
00051     //serial.printf("Serial begin!\r\n");
00052 
00053     /*----- BLE Utility ---------------------------------------------*/
00054     // set advertised local name and service UUID
00055     blePeripheral.setLocalName("BLE Shield");
00056 
00057     blePeripheral.setAdvertisedServiceUuid(uartService.uuid());
00058 
00059     // add service and characteristic
00060     blePeripheral.addAttribute(uartService);
00061     blePeripheral.addAttribute(rxCharacteristic);
00062     blePeripheral.addAttribute(txCharacteristic);
00063 
00064     // begin initialization
00065     blePeripheral.begin();
00066     //serial.printf("BLE UART Peripheral begin!\r\n");
00067     /*---------------------------------------------------------------*/
00068 
00069     while(1) {
00070         BLECentral central = blePeripheral.central();
00071 
00072         if(central) {
00073             // central connected to peripheral
00074             //serial.printf("Connected to central\r\n");
00075 
00076             while (central.connected()) {
00077                 // central still connected to peripheral
00078                 if (rxCharacteristic.written()) {
00079                     unsigned char len = rxCharacteristic.valueLength();
00080                     const unsigned char *val = rxCharacteristic.value();
00081 
00082                     // Calculate differential motor values from cartesian joystick coordinates
00083                     // 0 = north, 1 = south, 2 = east, 3 = west
00084                     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));
00085                     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));
00086 
00087                     // Windup compensation
00088                     if (leftval > 1) leftval = 1;
00089                     if (leftval < -1) leftval = -1;
00090                     if (rightval > 1) rightval = 1;
00091                     if (rightval < -1) rightval = -1;
00092 
00093                     // Set motor speed values
00094                     left.speed(leftval);
00095                     right.speed(rightval);
00096 
00097                     //serial.printf("%f,   %f\n", leftval, rightval);
00098                 }
00099             }
00100             // central disconnected
00101             //serial.printf("Disconnected from central\r\n");
00102         }
00103     }
00104 }