Maiko Matsumoto
0611
Fork of
BLE_WallbotBLE_Challenge2
by 
Maiko Matsumoto
Diff: main.cpp
- Revision:
- 0:76dfa9657d9d
- Child:
- 1:a86c84ead24a
diff -r 000000000000 -r 76dfa9657d9d main.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Wed Nov 12 02:40:34 2014 +0000
@@ -0,0 +1,415 @@
+#include "mbed.h"
+#include "BLEDevice.h"
+#include "RCBController.h"
+#include "TB6612.h"
+
+#define DBG 0
+
+BLEDevice ble;
+Serial pc(USBTX, USBRX);
+/* LEDs for indication: */
+DigitalOut ModeLed(P0_19);
+DigitalOut ConnectStateLed(P0_18);
+DigitalOut outlow(P0_20);
+//PwmOut ControllerStateLed(LED2);
+
+AnalogIn fsen1(P0_2);
+AnalogIn fsen2(P0_3);
+AnalogIn fsen3(P0_4);
+AnalogIn fsen4(P0_5);
+#if 1
+TB6612 left(P0_29,P0_23,P0_24);
+TB6612 right(P0_28,P0_0,P0_30);
+#else
+TB6612 left(P0_29,P0_24,P0_23);
+TB6612 right(P0_28,P0_0,P0_30);
+#endif
+Ticker ticker;
+
+DigitalIn sw1(P0_16);
+DigitalIn sw2(P0_17);
+
+DigitalIn pin1(P0_8);
+DigitalIn pin2(P0_10);
+
+
+int base_fsen[4];
+int line_mode = 0;
+int challenge_mode = 0;
+char bValue = 0;
+
+int get_line(int num);
+
+/* RCBController Service */
+static const uint16_t RCBController_service_uuid = 0xFFF0;
+static const uint16_t RCBController_Characteristic_uuid = 0xFFF1;
+static const uint16_t RCBController_b_Characteristic_uuid = 0xFFF3;
+uint8_t RCBControllerPayload[10] = {0,};
+
+GattCharacteristic ControllerChar (RCBController_Characteristic_uuid,RCBControllerPayload,10, 10,
+ GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE |
+ GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE_WITHOUT_RESPONSE);
+static uint8_t _bValue = 0x00;
+static uint8_t _mValue[10] = {0,};
+GattCharacteristic b_Char(RCBController_b_Characteristic_uuid, _mValue, sizeof(_mValue), sizeof(_mValue),
+ GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ | GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY);
+
+GattCharacteristic *ControllerChars[] = {&ControllerChar,&b_Char};
+GattService RCBControllerService(RCBController_service_uuid, ControllerChars, sizeof(ControllerChars) / sizeof(GattCharacteristic *));
+
+RCBController controller;
+
+void onConnected(Gap::Handle_t handle, const Gap::ConnectionParams_t *params)
+{
+ ConnectStateLed = 0;
+#if DBG
+ pc.printf("Connected\n\r");
+#endif
+}
+
+void onDisconnected(Gap::Handle_t handle, Gap::DisconnectionReason_t reason)
+{
+ ble.startAdvertising();
+ ConnectStateLed = 1;
+#if DBG
+ pc.printf("Disconnected\n\r");
+#endif
+}
+
+void periodicCallback(void)
+{
+ if (!ble.getGapState().connected) {
+ return;
+ }
+ int line = get_line(0) ? 1 : 0;
+ line |= get_line(1) ? 2 : 0;
+ line |= get_line(2) ? 4 : 0;
+ line |= get_line(3) ? 8 : 0;
+ if( (bValue == 0)&&(line != 0) )
+ {
+ // game over
+ left = 0.0;
+ right = 0.0;
+ bValue = 10;
+ }
+ if( bValue > 0 )
+ {
+ memcpy( _mValue , "GAME OVER",10);
+ ble.updateCharacteristicValue(b_Char.getValueAttribute().getHandle(), (uint8_t *)_mValue, sizeof(_mValue));
+ ModeLed = !ModeLed;
+ bValue--;
+ if( bValue == 0 )
+ {
+ ModeLed = 1;
+ challenge_mode = 0;
+ ticker.detach();
+ }
+ }
+}
+
+
+// GattEvent
+void onDataWritten(const GattCharacteristicWriteCBParams *params)
+{
+ if( (params->charHandle == ControllerChar.getValueAttribute().getHandle()) && (line_mode == 0))
+ {
+ memcpy( &controller.data[0], params->data , params->len );
+ //memcpy( &controller.data[0], RCBControllerPayload, sizeof(controller));
+#if DBG
+
+ 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],
+ controller.data[5],controller.data[6],controller.data[7],controller.data[8],controller.data[9]);
+#endif
+ float right_factor;
+ float left_factor;
+
+ left_factor = ((float)((int)controller.status.LeftAnalogUD -128) / 128.0);
+ right_factor = ((float)((int)controller.status.RightAnalogUD -128) / 128.0);
+
+ if(challenge_mode == 1)
+ {
+ if( bValue == 0 )
+ {
+ float factor = ((float)((int)controller.status.AcceleX -128) / 128.0);
+
+ float right_factor = ((factor <= 0.0) ? 1.0 : 1.0 - (factor*2));
+ float left_factor = ((factor >= 0.0) ? 1.0 : 1.0 - (-factor*2));
+
+ if( controller.status.B == 1 )
+ {
+ left = left_factor;
+ right = right_factor;
+ }
+ else if( controller.status.A == 1 )
+ {
+ left = -right_factor;
+ right = -left_factor;
+ }
+ else
+ {
+ left = 0;
+ right = 0;
+ }
+ }
+ }
+ else if( (left_factor != 0.0)||(right_factor != 0.0) )
+ {
+ left = left_factor;
+ right = right_factor;
+ }
+ else
+ {
+ float factor = ((float)((int)controller.status.AcceleX -128) / 128.0);
+
+ float right_factor = ((factor <= 0.0) ? 1.0 : 1.0 - (factor*2));
+ float left_factor = ((factor >= 0.0) ? 1.0 : 1.0 - (-factor*2));
+
+ if( controller.status.B == 1 )
+ {
+ left = left_factor;
+ right = right_factor;
+ }
+ else if( controller.status.A == 1 )
+ {
+ left = -right_factor;
+ right = -left_factor;
+ }
+ else if( controller.status.UP == 1 )
+ {
+ left = 1.0;
+ right = 1.0;
+ }
+ else if( controller.status.DOWN == 1 )
+ {
+ left = -1.0;
+ right = -1.0;
+ }
+ else if( controller.status.RIGHT == 1 )
+ {
+ left = 1.0;
+ right = -1.0;
+ }
+ else if( controller.status.LEFT == 1 )
+ {
+ left = -1.0;
+ right = 1.0;
+ }
+ else
+ {
+ left = 0.0;
+ right = 0.0;
+ }
+ if((controller.status.UP == 1)&&(controller.status.DOWN == 1))
+ {
+ left = 0.0;
+ right = 0.0;
+ ModeLed = 0;
+ challenge_mode = 1;
+ ticker.attach(periodicCallback, 0.1);
+
+ }
+ }
+ //ControllerStateLed = (float)controller.status.LeftAnalogLR / 255.0;
+ }
+}
+
+int get_fsen(int num)
+{
+ switch(num)
+ {
+ case 0:
+ return((int)fsen1.read_u16());
+ case 1:
+ return((int)fsen2.read_u16());
+ case 2:
+ return((int)fsen3.read_u16());
+ case 3:
+ return((int)fsen4.read_u16());
+ }
+ return(0);
+}
+
+void base()
+{
+ wait(0.5);
+
+ for(int i=0;i<4;i++)
+ {
+ base_fsen[i] = 0;
+ }
+
+ for(int j=0;j<10;j++)
+ {
+ for(int i=0;i<4;i++)
+ {
+ base_fsen[i] += get_fsen(i);
+ }
+ wait_ms(50);
+ }
+ for(int i=0;i<4;i++)
+ {
+ base_fsen[i] = base_fsen[i] / 10;
+ }
+ pc.printf("[0]:%05d[1]:%05d[2]:%05d[3]:%05d\n\r",base_fsen[0],base_fsen[1],base_fsen[2],base_fsen[3]);
+
+}
+
+int get_line(int num)
+{
+ int in = get_fsen(num);
+ int ret = 0;
+
+#if 1
+ if(in > 700)
+#else
+ if( (in > (base_fsen[num] + 200))||(in < (base_fsen[num] - 200)))
+#endif
+ {
+ ret = 1;
+ }
+ return(ret);
+}
+
+void line(void)
+{
+ ModeLed = 0;
+ wait(1);
+ while(sw1 != 0)
+ {
+#if 0
+ int line = get_line(0) ? 0 : 1;
+ line |= get_line(1) ? 0 : 2;
+ line |= get_line(2) ? 0 : 4;
+ line |= get_line(3) ? 0 : 8;
+#else
+ int line = get_line(0) ? 1 : 0;
+ line |= get_line(1) ? 2 : 0;
+ line |= get_line(2) ? 4 : 0;
+ line |= get_line(3) ? 8 : 0;
+#endif
+ switch(line)
+ {
+ case 1: // ○○○●
+ left = 1.0;
+ right = -1.0;
+ break;
+ case 3: // ○○●●
+ left = 1.0;
+ right = -0.5;
+ break;
+ case 2: // ○○●○
+ left = 1.0;
+ right = 0.5;
+ break;
+ case 6: // ○●●○
+ left = 1.0;
+ right = 1.0;
+ break;
+ case 4: // ○●○○
+ left = 0.5;
+ right = 1.0;
+ break;
+ case 12: // ●●○○
+ left = -0.5;
+ right = 1.0;
+ break;
+ case 8: // ●○○○
+ left = -1.0;
+ right = 1.0;
+ break;
+ default:
+ left = 1.0;
+ right = 1.0;
+ break;
+ }
+ }
+ ModeLed = 1;
+ left = 0.0;
+ right = 0.0;
+ wait(1);
+}
+int counter1 = 0;
+void p1_rise()
+{
+ if( pin2 == 1 )
+ {
+ counter1++;
+ }
+ else
+ {
+ counter1--;
+ }
+}
+/**************************************************************************/
+/*!
+ @brief Program entry point
+*/
+/**************************************************************************/
+int main(void)
+{
+ sw1.mode(PullUp);
+ sw2.mode(PullUp);
+
+ ModeLed = 1;
+ ConnectStateLed = 1;
+#if DBG
+ //pc.baud(921600);
+ pc.baud(9600);
+ pc.printf("Start\n\r");
+#endif
+ outlow = 0;
+
+ if(sw2 == 0)
+ {
+ pin1.mode(PullDown);
+// pin1.rise(&p1_rise);
+ while(1)
+ {
+ //int in1 = pin1;
+ //int in2 = pin2;
+ //ModeLed = pin1;
+ //pc.printf("dat = %d %d\r\n",in1,in2);
+ base();
+#if 0
+ left = 1.0;
+ right = 1.0;
+ wait(5);
+ left = -1.0;
+ right = -1.0;
+ wait(5);
+#endif
+ }
+ }
+
+ ble.init();
+ ble.onConnection(onConnected);
+ ble.onDisconnection(onDisconnected);
+ ble.onDataWritten(onDataWritten);
+
+ /* setup advertising */
+ ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
+ ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
+ ble.accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME,
+ (const uint8_t *)"mbed WallbotBLE", sizeof("mbed WallbotBLE") - 1);
+ ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS,
+ (const uint8_t *)RCBController_service_uuid, sizeof(RCBController_service_uuid));
+
+ ble.setAdvertisingInterval(160); /* 100ms; in multiples of 0.625ms. */
+ ble.startAdvertising();
+
+ ble.addService(RCBControllerService);
+
+
+ while (true) {
+ if(sw1 == 0)
+ {
+ bValue = 1;
+ line_mode = 1;
+ line();
+ line_mode = 0;
+ bValue = 0;
+ }
+ ble.waitForEvent();
+ }
+}
+