File content as of revision 20:b0e1fa37856d:

#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

Serial pc(USBTX, USBRX);
BLEDevice  ble;
MPU6050 mpu(I2C_SDA,I2C_SCL);
wallbotble wb;

//------------------------------------------------------------
//Service & Characteristic Setting
//------------------------------------------------------------ 
//Service UUID
static const uint16_t base_uuid = 0xFFF0;

//Characteristic UUID
static const uint16_t cmd_uuid = 0xFFF1;
static const uint16_t sen_uuid = 0xFFF2;
static const uint16_t mpu_uuid = 0xFFF3;
 
//Characteristic buffer ?
uint8_t cmdPayload[8] = {0,};
uint8_t senPayload[10] = {0,};
uint8_t mpuPayload[12] = {0,};
 

//Characteristic Property Setting etc
GattCharacteristic  cmdChar (cmd_uuid, cmdPayload, sizeof(cmdPayload), sizeof(cmdPayload), GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE);
GattCharacteristic  senChar (sen_uuid, senPayload, sizeof(senPayload), sizeof(senPayload), GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY);
GattCharacteristic  mpuChar (mpu_uuid, mpuPayload, sizeof(mpuPayload), sizeof(mpuPayload), GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY);
GattCharacteristic *myChars[] = {&cmdChar, &senChar, &mpuChar};
 
//Service Setting
GattService         myService(base_uuid, myChars, sizeof(myChars) / sizeof(GattCharacteristic *));

//======================================================================
//onDisconnection
//======================================================================
void onDisconnected(Gap::Handle_t handle, Gap::DisconnectionReason_t reason)
{   
	wb.control_enable(false);
	wb.stop();

    ble.startAdvertising();
    wb.set_led2(0);
#ifdef DEBUG
	//割り込みハンドラの中でシリアル通信しないこと。
    //pc.printf("Disconnected\n\r");
#endif
}
//======================================================================
//onConnection
//======================================================================
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);
    
	wb.control_enable(true);
	wb.SetRPM(0,0);
#ifdef DEBUG
	//割り込みハンドラの中でシリアル通信しないこと。
    //pc.printf("Connected\n\r");
#endif
}

//======================================================================
//onDataWritten
//======================================================================

void onDataWritten(const GattCharacteristicWriteCBParams *params)
{   
	float right_cmd = 0;
	float left_cmd = 0;
    memcpy( &right_cmd, params->data , sizeof(right_cmd));
    memcpy( &left_cmd, params->data + sizeof(right_cmd), sizeof(left_cmd));
	wb.SetRPM(left_cmd,right_cmd);
#ifdef DEBUG
    //割り込みハンドラの中でシリアル通信しないこと。
    //printf("SetRPM:%f,%f",left_cmd,right_cmd)
#endif
}
/**************************************************************************/
/*!
    @brief  Program entry point
*/
/**************************************************************************/
int main(void)
{

    int16_t mpu_a[3];
    int16_t mpu_g[3];
	short line;
	float rrpm,lrpm;

   	uint8_t sen_buf[10];
   	uint8_t mpu_buf[12];

    pc.baud(9600);
    
	//MPU 成功するまでリトライ
    wb.set_led1(1);
	wait(1);	
	
	if(!mpu.testConnection()){
		pc.printf("MPU connection fail, -> soft reset \n\r");
		NVIC_SystemReset();
 	}

    wb.set_led1(0);
    pc.printf("MPU6050 initialize passed \n\r");

	
	const uint8_t address1[] = {0xe7,0xAA,0xAA,0xAA,0xAA,0xAA};
	ble.setAddress(Gap::ADDR_TYPE_PUBLIC, address1);
	
	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の設定
    //HACK:リバースが必要？「mbed BLE APIで128bit UUIDを含むGATT Serviceを２つ以上登録すると Service UUIDがひっくり返る？」
    ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS,(const uint8_t *)base_uuid, sizeof(base_uuid));

	//Advertiseパケットの送信周期
    ble.setAdvertisingInterval(160); /* 100ms; in multiples of 0.625ms. */

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

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

	//ラインセンサキャリブレーション
	//pc.printf("auto calibrate start\n\r");

	//適当キャリブレーション
	/*
	for(int i = 0 ; i < 4 ; i++)
	{
		wb._calibratedMinimum[i] = 300;
		wb._calibratedMaximum[i] = 900;
	}
	*/
    
    wb.auto_calibrate(); //自動キャリブレーション
#ifdef DEBUG
    for(int i = 0 ; i < 4 ; i++)
	{
		pc.printf("(%d,%d) ",wb._calibratedMinimum[i],wb._calibratedMaximum[i]);
	}
	pc.printf("\n\rauto calibrate end\n\r");
#endif
    
#if 0 //エンコーダキャリブレーション用コード
	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

	//速度制御
	//wb.control_enable(true);
	//wb.SetRPM(-30,30);

    while (true) {
    	
    	//センサ値の取得
    	line = wb.GetLinePosition();
    	rrpm = wb.get_right_rpm();
    	lrpm = wb.get_left_rpm();
        mpu.getAcceleroRaw(mpu_a);
        mpu.getGyroRaw(mpu_g);
    	
    	//送信の準備(リトルエンディアン)
    	memcpy(sen_buf							,&line,sizeof(line));
    	memcpy(sen_buf+sizeof(line)				,&rrpm,sizeof(rrpm));
    	memcpy(sen_buf+sizeof(line)+sizeof(rrpm),&lrpm,sizeof(lrpm));
    	
    	memcpy(mpu_buf				,mpu_a,sizeof(mpu_a));
    	memcpy(mpu_buf+sizeof(mpu_a),mpu_g,sizeof(mpu_g));
    	
    	//送信
        ble.updateCharacteristicValue(senChar.getValueAttribute().getHandle(), (uint8_t *)sen_buf, sizeof(sen_buf));
        ble.updateCharacteristicValue(mpuChar.getValueAttribute().getHandle(), (uint8_t *)mpu_buf, sizeof(mpu_buf));
    	
		//
		#ifdef DEBUG
			pc.printf("Pulse(%d,%d) RPM(%.2f,%.2f) ",wb._left_pulses,wb._right_pulses,lrpm, rrpm);
			pc.printf("LinePos:%d ",line);
			pc.printf("Sens(%d,%d,%d,%d)",wb.sensor_values[0],wb.sensor_values[1],wb.sensor_values[2],wb.sensor_values[3]);
	        pc.printf("MPU(%d,%d,%d,  %d,%d,%d)",mpu_a[0],mpu_a[1],mpu_a[2],mpu_g[0],mpu_g[1],mpu_g[2]);
		
		    pc.printf("\n\r");
		#endif
		wait_ms(10);
  
    }
}