File content as of revision 15:e98de98d9328:

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

#define DBG 1

BLEDevice  ble;

MPU6050 mpu(I2C_SDA, I2C_SCL);

#if DBG
Serial  pc(USBTX, USBRX);
#endif
/* 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;

// Wallbot State
int stt_Mode;

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

//動作パターン関数（工事中）
void wb_control(void)
{
    ModeLed = 0;
    wait(1);    
    
    // Wallbot State Initialize
    // Up Straight  : 1
    // Up Back      : 2
    // Up Rotate    : 3
    // Down Straight  : 4
    // Down Back      : 5
    // Down Rotate    : 6
    stt_Mode = 1;
    
    // 値を格納する用の配列、変数
    float   acData[3];
//    float   gyData[3];
    float ax = 0;
    float ay = 0;
//    float az = 0;
//    float a = 0;
//    float buf_ax[10];
//    float mean_ax = 0;
//    float dev_ax = 0;
//    float sum_ax = 0;
//    float log_ax[100];
    int cnt_loop = 0;
    int cnt_straight = 0;
//    float thre_bump = 8.0;
//    float thre_bump_back = 11.0;
    bool enable_ChangeMode = false;
    
//        for(int i = 0; i < 10; i++)
//        {
//            buf_ax[i] = 0;
//        }
    
        
//        for(int i = 0; i < 100; i++)
//        {
//            log_ax[i] = 0;
//        }
    
    while(sw1 != 0)
    {
//        Wait(0.1);
        
        // Get value                   
        //加速度を取得
//        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]);  
//        pc.printf("%5.3f, ", acData[2]);

        ax = acData[0];
        ay = acData[1];
//        az = acData[2];
//        a = sqrt(ax * ax + ay * ay + az * az);
//        pc.printf("%5.3f, ", ax);
        
//        log_ax[cnt_loop] = ax;
        
//        if(!enble_ChangeMode)
//        {
//            if(cnt_loop > 20)
//            {
//                enble_ChangeMode = true;
//                }
//            }
        
        // ax Buffer Store
//        sum_ax -= buf_ax[9];
//        for(int i = 9; i > 0; i--)
//        {
//            buf_ax[i] = buf_ax[i - 1];
//            }
//        buf_ax[0] = ax;
//        sum_ax += buf_ax[5];
//        mean_ax = sum_ax / 5;
        
        // Calculate Deviation
//        dev_ax = 0;
//        for(int i = 0; i < 5; i++)
//        {
//            dev_ax += (mean_ax - buf_ax[i]) * (mean_ax - buf_ax[i]);
//            }
//        dev_ax = sqrt(dev_ax);
        //
//        pc.printf("%5.3f, 5.3f \n", ax, ay);
        
        
#if 1
        switch(stt_Mode)
        {
            case 1:                 // Up Straight
//                pc.printf("Mode 1 \n");
                        
                if(ay > 0.5){
                        left = 0.8;
                        right = 1.0;
//                        pc.printf("Lean Right");
                    }
                    else if(ay < -0.5){
                        left = 1.0;
                        right = 0.8;
//                        pc.printf("Lean Left");
                    }
                    else
                    {
                        left = 1.0;
                        right = 1.0;
                        }
                        
                    // Judge Bump
                    if(enable_ChangeMode)
                    {                        
                        if(ay > 4/*ax - mean_ax < -thre_bump || ax - mean_ax > thre_bump*/)
                        {
                            stt_Mode = 2;
//                            pc.printf("Mode 1 -> 2 \n");
//                            cnt_back = 0;
                            
                            enable_ChangeMode = false;
                            }
                    }else
                    {
                        cnt_straight++;
                        if(cnt_straight > 100)
                        {
                            enable_ChangeMode = true;
                            cnt_straight = 0;
                            }
                        }
                
                break;
            case 2:             // Up Back
//                pc.printf("Mode 2 \n");
//                if(cnt_back < 120)
//                {
                    left = -1.0;
                    right = -1.0;
//                    cnt_back++;
                    wait(0.6);
//                    }
//                    else{
//                            cnt_back = 0;
                            stt_Mode = 3;
//                            pc.printf("Mode 2 -> 3 \n");
//                        }
                
                break;
            case 3:                 // Up Rotate
//                pc.printf("Mode 3 \n");
                if(!(ax < -9.7 && ay < 0/* && ay >= -0.3*/))      // Change Using Gyro??
                {
                    left = 1.0;
                    right = -1.0;                    
                    }
                    else
                    {
                        stt_Mode = 4;
//                        pc.printf("Mode 3 - > 4 \n");
                        
                        }
                
                break;            
            case 4:                 // Down Straight
//                pc.printf("Mode 4 \n");
                if(ay > 0.5){
                        left = 1.0;
                        right = 0.8;
                    }
                    else if(ay < -0.5){
                        left = 0.8;
                        right = 1.0;
                    }
                    else
                    {
                        left = 1.0;
                        right = 1.0;
                        }
                        
                    // Judge Bump
                    if(enable_ChangeMode)
                    {
                        if(ay < -4/*ax - mean_ax < -thre_bump_back || ax - mean_ax > thre_bump_back*/)
                        {
                            stt_Mode = 5;
//                            pc.printf("Mode 4 -> 5 \n");
                            
                            enable_ChangeMode = false;
                            }
                    }else
                    {                        
                        cnt_straight++;
                        if(cnt_straight > 100)
                        {
                            enable_ChangeMode = true;
                            cnt_straight = 0;
                            }
                        }
                
                break;
            case 5:           
//                pc.printf("Mode 5 \n");
//                if(cnt_back < 200)
//                {
                    left = -1.0;
                    right = -1.0;
                    wait(1.0);
//                    cnt_back++;
//                    }
//                    else{
//                            cnt_back = 0;
                            stt_Mode = 6;
//                            pc.printf(" 5 -> 6 \n");
//                        }
                
                break;
            case 6:                 //
//                pc.printf("Mode 6 \n");
                if(!(ax > 9.7 && ay < 0/* && ay >= 0*/))      // Change Using Gyro??
                {
                    left = -1.0;
                    right = 1.0;                    
                    }
                    else
                    {
                        stt_Mode = 1;
                        pc.printf(" 6 -> 1 \n");
//                        
//                        for(int i = 0; i < 3; i++)
//                        {
//                            buf_ax[i] = ax;
//                            }
                        }
                        
                
                break;
            default:
                break;
        }
#endif

        cnt_loop++;

        if(cnt_loop > 2000)
        {
            cnt_loop = 0;
            }
    }
    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
*/
/**************************************************************************/
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);

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

    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();
            wb_control(); //動作モード関数
            line_mode = 0;
            bValue = 0;
        }
        ble.waitForEvent();
    }
}