Diff: main.cpp

Revision:

0:76dfa9657d9d

Child:

1:a86c84ead24a

--- /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();
+    }
+}
+