main.cpp

00001 #include "mbed.h"
00002 #include "BLEDevice.h"
00003 #include "RCBController.h"
00004 #include "TB6612.h"
00005 
00006 #define DBG 0
00007 
00008 BLEDevice  ble;
00009 Serial  pc(USBTX, USBRX);
00010 /* LEDs for indication: */
00011 DigitalOut  ConnectStateLed(P0_18);
00012 DigitalOut  testLED(P0_19);
00013 DigitalOut  outlow(P0_20);
00014 DigitalIn   sw1(P0_16);
00015 DigitalIn   sw2(P0_17);
00016 
00017 //PwmOut  ControllerStateLed(LED2);
00018 
00019 TB6612 left(P0_29,P0_24,P0_23);
00020 TB6612 right(P0_28,P0_30,P0_0);
00021 
00022 /* RCBController Service */
00023 static const uint16_t RCBController_service_uuid = 0xFFF0;
00024 static const uint16_t RCBController_Characteristic_uuid = 0xFFF1;
00025 uint8_t RCBControllerPayload[10] = {0,};
00026 
00027 GattCharacteristic  ControllerChar (RCBController_Characteristic_uuid,RCBControllerPayload,10, 10,
00028                                 GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE | 
00029                                 GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE_WITHOUT_RESPONSE);
00030 GattCharacteristic *ControllerChars[] = {&ControllerChar};
00031 GattService         RCBControllerService(RCBController_service_uuid, ControllerChars, sizeof(ControllerChars) / sizeof(GattCharacteristic *));
00032 
00033 RCBController controller;
00034 
00035 int cmode = 0;
00036 
00037 void onConnected(Gap::Handle_t handle, const Gap::ConnectionParams_t *params)
00038 {
00039     ConnectStateLed = 0;
00040 #if DBG
00041     pc.printf("Connected\n\r");
00042 #endif
00043 }
00044 
00045 void onDisconnected(Gap::Handle_t handle, Gap::DisconnectionReason_t reason)
00046 {
00047     ble.startAdvertising();
00048     ConnectStateLed = 1;
00049 #if DBG
00050     pc.printf("Disconnected\n\r");
00051 #endif
00052 }
00053 
00054 
00055 // GattEvent
00056 void onDataWritten(const GattCharacteristicWriteCBParams *params)
00057 {
00058     if (params->charHandle == ControllerChar.getValueAttribute().getHandle())
00059     {
00060         uint16_t bytesRead;
00061         memcpy( &controller.data[0], params->data , params->len );
00062      // ble.readCharacteristicValue(ControllerChar.getHandle(),RCBControllerPayload, &bytesRead);
00063      //   memcpy( &controller.data[0], RCBControllerPayload, sizeof(controller));
00064 #if DBG
00065 
00066         pc.printf("DATA:%02X %02X %d %d %d %d %d %d %d %02X\n\r",controller.data[0],controller.data[1],controller.data[2],controller.data[3],controller.data[4],
00067                                                                controller.data[5],controller.data[6],controller.data[7],controller.data[8],controller.data[9]);
00068 #endif
00069         float right_factor;
00070         float left_factor;
00071 
00072         left_factor = ((float)((int)controller.status.LeftAnalogUD -128) / 128.0);
00073         right_factor = ((float)((int)controller.status.RightAnalogUD -128) / 128.0);
00074         
00075         if( (left_factor != 0.0)||(right_factor != 0.0) )
00076         {
00077             left = left_factor;
00078             right = right_factor;
00079         }
00080         else
00081         {
00082             float factor = ((float)((int)controller.status.AcceleX -128) / 128.0); 
00083 
00084             float right_factor = ((factor <= 0.0) ? 1.0 : 1.0 - (factor*2));
00085             float left_factor = ((factor >= 0.0) ? 1.0 : 1.0 - (-factor*2));
00086 
00087             if( controller.status.B == 1 )
00088             {
00089                 left = left_factor;
00090                 right = right_factor;
00091             }
00092             else if( controller.status.A == 1 )
00093             {
00094                 left = -right_factor;
00095                 right = -left_factor;
00096             }
00097             else if( controller.status.UP == 1 )
00098             {
00099                 left = 1.0;
00100                 right = 1.0;
00101             }
00102             else if( controller.status.DOWN == 1 )
00103             {
00104                 left = -1.0;
00105                 right = -1.0;
00106             }
00107             else if( controller.status.RIGHT == 1 )
00108             {
00109                 left = 1.0;
00110                 right = -1.0;
00111             }
00112             else if( controller.status.LEFT == 1 )
00113             {
00114                 left = -1.0;
00115                 right = 1.0;
00116             }
00117             else
00118             {
00119                 left = 0.0;
00120                 right = 0.0;    
00121             }
00122         }
00123         //ControllerStateLed = (float)controller.status.LeftAnalogLR / 255.0;       
00124     }
00125          
00126 }
00127 
00128 /**************************************************************************/
00129 /*!
00130     @brief  Program entry point
00131 */
00132 /**************************************************************************/
00133 int main(void)
00134 {
00135     pc.baud(921600);
00136     
00137     sw1.mode(PullUp);
00138     sw2.mode(PullUp);
00139     
00140     ConnectStateLed = 1;
00141 #if DBG
00142         pc.printf("Start\n\r");
00143 #endif
00144     outlow = 0;
00145     
00146     ble.init(); 
00147     ble.onConnection(onConnected);
00148     ble.onDisconnection(onDisconnected);
00149     ble.onDataWritten(onDataWritten);
00150     
00151     /* setup advertising */
00152     ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
00153     ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
00154     ble.accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME,
00155                                     (const uint8_t *)"mbed WallbotBLE", sizeof("mbed WallbotBLE") - 1);
00156     ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS,
00157                                     (const uint8_t *)RCBController_service_uuid, sizeof(RCBController_service_uuid));
00158 
00159     ble.setAdvertisingInterval(160); /* 100ms; in multiples of 0.625ms. */
00160     ble.startAdvertising();
00161 
00162     ble.addService(RCBControllerService);
00163     
00164     testLED = cmode;
00165     int old_sw = 0;
00166 
00167     while (true) {
00168 #if 0
00169         if(sw1 == 0 && old_sw == 1)
00170         {
00171             cmode = !cmode;
00172             testLED = cmode;
00173             wait(0.1);
00174         }
00175         old_sw = sw1;
00176 #endif
00177         ble.waitForEvent();
00178     }
00179 }
00180