File content as of revision 4:53eceb750885:

#include "mbed.h"
#include "BLEDevice.h"
#include "RCBController.h"
#include "TB6612.h"
#include "MPU6050.h"

#define DBG 0

BLEDevice  ble;

MPU6050 mpu(I2C_SDA, I2C_SCL);
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_30,P0_0);
#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 encl1(P0_6);
DigitalIn encl2(P0_7);
DigitalIn encr1(P0_8);
DigitalIn encr2(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)
{
    left = 0;
    right = 0;

    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;
    }
#if DBG
    pc.printf("[0]:%05d[1]:%05d[2]:%05d[3]:%05d\n\r",base_fsen[0],base_fsen[1],base_fsen[2],base_fsen[3]);
#endif
}

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

#if DBG
        pc.printf("line=%02x %04x %04x %04x %04x\n\r",line,base_fsen[0],base_fsen[1],base_fsen[2],base_fsen[3]);
#endif
#if 1
        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;
        }
#endif
    }
    ModeLed = 1;
    left = 0.0;
    right = 0.0;
    wait(1);
}

#if 0
int counter1 = 0;
void p1_rise()
{
    if( pin2 == 1 )
    {
        counter1++;
    }
    else
    {
        counter1--;
    }
}
#endif

/**************************************************************************/
/*!
    @brief  Program entry point
*/

//#define CONN_INTERVAL 25  /**< connection interval 250ms; in multiples of 0.125ms. (durationInMillis * 1000) / UNIT_0_625_MS; */
//#define CONN_SUP_TIMEOUT  8000 /**< Connection supervisory timeout (6 seconds); in multiples of 0.125ms. */
//#define SLAVE_LATENCY     0
#define TICKER_INTERVAL   2.0f

// ・MPU6050から値を取得
// ・ループ実行されている関数
void updateValue(void){
    // 値を格納する用の配列、変数
    float   acData[3];
    float   gyData[3];
               
    //加速度を取得
    Timer acTimer;
    acTimer.start();
    mpu.getAccelero(acData);    //加速度を取得 acDataに格納
    acTimer.stop();
//  at = acTimer.read_ms();
    acTimer.reset();

    //ジャイロを取得
    Timer gyTimer;
    gyTimer.start();
    mpu.getGyro(gyData);    //ジャイロの値（角加速度）を取得 gyDataに格納
    gyTimer.stop();
//  gt = gyTimer.read_ms();
    gyTimer.reset();

    //floatの値を合計5桁、小数点以下3桁の形でPCへ送信
    pc.printf("%5.3f:%5.3f:%5.3f:%5.3f:%5.3f:%5.3f \n", acData[0], acData[1], acData[2],gyData[0],gyData[1],gyData[2]);   
}

int test()
{
    
    ///180601 MPU6050センサの初期化処理
    mpu.initialize();
//  mpu.setAcceleroRange(MPU6050_ACCELERO_RANGE_8G);    //加速度センサ 計測レンジ設定（今回は2Gか4Gがよさそう）
    mpu.setAcceleroRange(MPU6050_ACCELERO_RANGE_2G); 
//   mpu.setGyroRange(MPU6050_GYRO_RANGE_1000);          //ジャイロセンサ 計測レンジ設定（ここも250か500がよさそう（そんなに早く回転しないので））
    mpu.setGyroRange(MPU6050_GYRO_RANGE_250);
    ///180601 
    
    if( mpu.testConnection() ){
      //  pc.printf("mpu test:OK\n\r");
    }else{
      //  pc.printf("mpu test:NG\n\r");
    }

    float ticker_ms = (TICKER_INTERVAL / 100.0f);
    Ticker ticker;
    ticker.attach(periodicCallback, ticker_ms);//0.02f //.2f-sec
    
     pc.printf("Start!! \n");

    while(true) {
            left = 1.0;
            right = 1.0;
  //          wait(5);   
            updateValue();
            wait(0.5);
    }
}


/**************************************************************************/
int main(void)
{
    sw1.mode(PullUp);
    sw2.mode(PullUp);

    encl1.mode(PullNone);
    encl2.mode(PullNone);
    encr1.mode(PullNone);
    encr2.mode(PullNone);

    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        
        }
    }

    // MPU6050 Initialize
    mpu.initialize();
    mpu.setAcceleroRange(MPU6050_ACCELERO_RANGE_8G);
    mpu.setGyroRange(MPU6050_GYRO_RANGE_1000);

    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;
            test();       
            line_mode = 0;
            bValue = 0;
        }
        ble.waitForEvent();
    }
}