File content as of revision 4:6b4563aaee2c:

#include "mbed.h"
#include "MPU6050.h"
#include "BLEDevice.h"
#include "wallbotble.h"
#include "RCBController.h"
#include "Adafruit_LEDBackpack.h"
#include "Adafruit_GFX.h"
#include "pictLIB.h"


#define DEBUG

enum _mode {
    Nomal = 0,
    LineFollow,
    Challenge,
    CaaS
};

Serial pc(USBTX, USBRX);
BLEDevice  ble;
Ticker ticker;
MPU6050 mpu;
wallbotble wb;

myI2C i2c(P0_22,P0_21);
//Adafruit_8x8mat96rix matrix1 = Adafruit_8x8matrix(&i2c);
//Adafruit_8x8matrix matrix2 = Adafruit_8x8matrix(&i2c);
//PICTLIB pl(16,16);

int mode = Nomal;

char bValue = 0;


/* 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 SendMessage(char *msg)
{
    if (!ble.getGapState().connected) {
        return;
    }
    int len = strlen(msg);

    if(len < 10) {
        strcpy( (char*)_mValue ,msg );
        ble.updateCharacteristicValue(b_Char.getValueAttribute().getHandle(), (uint8_t *)_mValue, sizeof(_mValue));
    }
}

void onConnected(Gap::Handle_t handle, Gap::addr_type_t peerAddrType ,const Gap::address_t peerAddr,const Gap::ConnectionParams_t *params)
{
    wb.set_led2(1);
#ifdef DEBUG
    pc.printf("Connected\n\r");
#endif
}

void onDisconnected(Gap::Handle_t handle, Gap::DisconnectionReason_t reason)
{
    wb.stop();

    ble.startAdvertising();
    wb.set_led2(0);
#ifdef DEBUG
    pc.printf("Disconnected\n\r");
#endif
}

void periodicCallback(void)
{
    if (!ble.getGapState().connected) {
        return;
    }
    if( (bValue == 0)&&(wb.GetLinePosition() != 0) ) {
        // Game over
        wb.stop();
        bValue = 10;
    }
    if( bValue > 0 ) {
        wb.stop();
        SendMessage("GAME OVER");
        bValue--;
        if( bValue == 0 ) {
            wb.set_led1(0);
            SendMessage(" NOMAL  ");
            mode = Nomal;
            ticker.detach();
        }
    }
}


// GattEvent
void onDataWritten(const GattCharacteristicWriteCBParams *params)
{
    if( (params->charHandle == ControllerChar.getValueAttribute().getHandle()) && ( mode != LineFollow )) {
        float right_factor;
        float left_factor;

        memcpy( &controller.data[0], params->data , params->len );
#ifdef DEBUG
        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

        if(mode == Challenge) {
            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 ) {
                wb.left_motor(left_factor);
                wb.right_motor(right_factor);
            } else if( controller.status.A == 1 ) {
                wb.left_motor(-right_factor);
                wb.right_motor(-left_factor);
            } else {
                wb.forward(1.0);
            }
        } else {
            if( (controller.status.LeftAnalogUD != 128)||(controller.status.RightAnalogUD != 128) ) {
                left_factor = ((float)((int)controller.status.LeftAnalogUD -128) / 128.0);
                right_factor = ((float)((int)controller.status.RightAnalogUD -128) / 128.0);

                wb.left_motor(left_factor);
                wb.right_motor(right_factor);
            } else {
                float factor = ((float)((int)controller.status.AcceleX -128) / 128.0);

                right_factor = ((factor <= 0.0) ? 1.0 : 1.0 - (factor*2));
                left_factor = ((factor >= 0.0) ? 1.0 : 1.0 - (-factor*2));

                if( controller.status.B == 1 ) {
                    wb.left_motor(left_factor);
                    wb.right_motor(right_factor);
                } else if( controller.status.A == 1 ) {
                    wb.left_motor(-right_factor);
                    wb.right_motor(-left_factor);
                } else if( controller.status.UP == 1 ) {
                    wb.forward(1.0);
                } else if( controller.status.DOWN == 1 ) {
                    wb.backward(1.0);
                } else if( controller.status.RIGHT == 1 ) {
                    wb.right_turn(1.0);
                } else if( controller.status.LEFT == 1 ) {
                    wb.left_turn(1.0);
                } else {
                    wb.stop();
                }
                if((controller.status.UP == 1)&&(controller.status.DOWN == 1)) { // START BUTTON
                    SendMessage("Challenge");
                    wb.set_led1(1);
                    mode = Challenge;
                    wb.stop();
                    ticker.attach(periodicCallback, 0.1);
                }
            }
        }
    }
}


/**************************************************************************/
/*!
    @brief  Program entry point
*/
/**************************************************************************/
int main(void)
{

    int16_t ax, ay, az;
    int16_t gx, gy, gz;

    //mpu.initialize();
    
    //wb.auto_calibrate();

#ifdef DEBUG
    pc.baud(115200);
    pc.printf("Start\n\r");
#endif

    ble.init();

    //イベント時のコールバック関数
    ble.onConnection(onConnected);
    ble.onDisconnection(onDisconnected);
    ble.onDataWritten(onDataWritten);

    /* setup advertising */
    //クラシックBTはサポートせず、BLEデバイスとして認識してもらう
    ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
    //デバイスがセントラルに接続可能であることを設定
    ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
    //LOCAL NAMEの設定、BLEでは終端記号は不要なので-1する
    ble.accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME,
                                     (const uint8_t *)"mbed WallbotBLE", sizeof("mbed WallbotBLE") - 1);
    //16Bit短縮UUIDの設定
    ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS,
                                     (const uint8_t *)RCBController_service_uuid, sizeof(RCBController_service_uuid));
	//Advertiseパケットの送信周期
    ble.setAdvertisingInterval(160); /* 100ms; in multiples of 0.625ms. */

	//サービスの登録
    ble.addService(RCBControllerService);

    //Advertizeの開始
    ble.startAdvertising();

	wb.right_motor(0.5);
	wb.left_motor(0.5);
    
#ifdef ENCCALIB //エンコーダキャリブレーション用コード
	BusIn enc(P0_8,P0_10,P0_6,P0_7);
	enc.mode(PullNone);
	while(1){
		char c = enc.read();
		pc.putc(c);
		wait_ms(10);
	}
#endif
    while (true) {
    	
		//mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
		#ifdef DEBUG
			
			//pc.printf("pulse(%d,%d) state(%d,%d)",wb.left_getPulses(),wb.right_getPulses(),wb._left_enc.getCurrentState(),wb._right_enc.getCurrentState());
			//pc.printf("%d%d%d%d ",((enc.read() >> 3) & 1), ((enc.read() >> 2) & 1), ((enc.read() >> 1) & 1), (enc.read() & 1));
			//pc.printf("LinePos:%d RPM(%d,%d)",wb.GetLinePosition(),wb.get_left_rpm(),wb.get_right_rpm());
			//pc.printf("calib(%d,%d,%d,%d)",wb.sensor_values[0],wb.sensor_values[1],wb.sensor_values[2],wb.sensor_values[3]);
		    //pc.printf("MPU6050(%d;%d;%d;%d;%d;%d)\n\r",ax,ay,az,gx,gy,gz);
		    
		    //pc.printf("\n\r");
		    wait_ms(10);
		#endif
    }
}