Wallbot_CaaS
Dependencies: MPU6050 mbed PID
Fork of BLE_MPU6050_test6_challenge_sb by
Diff: old_main.cpp
- Revision:
- 2:64f85c9eb556
- Parent:
- 0:8468a4403fea
diff -r 0ee4281651b3 -r 64f85c9eb556 old_main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/old_main.cpp Tue May 15 04:20:13 2018 +0000 @@ -0,0 +1,569 @@ +#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 DBG 0 + +enum _mode { + Nomal = 0, // ????[?h + LineFollow, // ???C???g???[?X???[?h + Challenge, // ?`???????W???[?h +}; + +Serial pc(USBTX, USBRX); +BLEDevice ble; +Ticker ticker; +MPU6050 mpu; +wallbotble wb; + +myI2C i2c(P0_22,P0_21); + +Adafruit_8x8matrix matrix1 = Adafruit_8x8matrix(&i2c); +Adafruit_8x8matrix matrix2 = Adafruit_8x8matrix(&i2c); + +PICTLIB pl(16,16); + +int mode = Nomal; + +char bValue = 0; + +#if 1 +char data[16][16] = +{ +{0,0,0,0,0,0,0,1,1,0,0,0,0,0,0,0}, +{0,0,0,0,0,0,1,1,1,1,0,0,0,0,0,0}, +{0,0,0,0,0,1,1,1,1,1,1,0,0,0,0,0}, +{0,0,0,0,1,1,1,1,1,1,1,1,0,0,0,0}, +{0,0,0,1,1,1,1,1,1,1,1,1,1,0,0,0}, +{0,0,1,1,1,1,1,1,1,1,1,1,1,1,0,0}, +{0,0,0,0,0,0,1,1,1,1,0,0,0,0,0,0}, +{0,0,0,0,0,0,1,1,1,1,0,0,0,0,0,0}, +{0,0,0,0,0,0,1,1,1,1,0,0,0,0,0,0}, +{0,0,0,0,0,0,1,1,1,1,0,0,0,0,0,0}, +{0,0,0,0,0,0,1,1,1,1,0,0,0,0,0,0}, +{0,0,0,0,0,0,1,1,1,1,0,0,0,0,0,0}, +{0,0,0,0,0,0,1,1,1,1,0,0,0,0,0,0}, +{0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}, +{0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}, +{0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}, +}; +char data2[16][16] = +{ +{0,0,0,0,0,1,1,1,1,1,1,0,0,0,0,0}, +{0,0,0,0,1,0,0,0,0,0,0,1,0,0,0,0}, +{0,0,0,1,0,0,0,0,0,0,0,0,1,0,0,0}, +{0,0,1,0,0,0,0,0,0,0,0,0,0,1,0,0}, +{0,1,0,0,0,0,0,0,0,0,0,0,0,0,1,0}, +{1,0,0,0,1,0,0,0,0,0,0,1,0,0,0,1}, +{1,0,0,0,1,0,0,0,0,0,0,1,0,0,0,1}, +{1,0,0,0,1,0,0,0,0,0,0,1,0,0,0,1}, +{1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1}, +{1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1}, +{1,0,0,0,1,0,0,0,0,0,0,1,0,0,0,1}, +{0,1,0,0,0,1,0,0,0,0,1,0,0,0,1,0}, +{0,0,1,0,0,0,1,1,1,1,0,0,0,1,0,0}, +{0,0,0,1,0,0,0,0,0,0,0,0,1,0,0,0}, +{0,0,0,0,1,0,0,0,0,0,0,1,0,0,0,0}, +{0,0,0,0,0,1,1,1,1,1,1,0,0,0,0,0}, +}; +#else +char data[16][16] = +{ +{0,0,0,0,1,0,0,0,0,0,0,1,0,0,0,0}, +{0,0,0,1,0,1,0,0,0,0,1,0,1,0,0,0}, +{0,0,0,1,0,1,0,0,0,0,1,0,1,0,0,0}, +{0,0,1,0,0,0,1,0,0,1,0,0,0,1,0,0}, +{0,0,1,0,0,0,1,1,1,1,0,0,0,1,0,0}, +{0,1,0,0,0,0,0,0,0,0,0,0,0,0,1,0}, +{0,1,0,0,0,1,0,0,0,0,1,0,0,0,1,0}, +{0,1,0,0,0,1,0,0,0,0,1,0,0,0,1,0}, +{0,1,0,0,0,1,0,0,0,0,1,0,0,0,1,0}, +{0,1,0,0,0,0,0,1,1,0,0,0,0,0,1,0}, +{0,1,0,0,0,0,1,0,0,1,0,0,0,0,1,0}, +{0,1,0,0,0,0,0,1,1,0,0,0,0,0,1,0}, +{0,1,0,0,1,0,0,1,1,0,0,1,0,0,1,0}, +{0,1,0,0,0,1,1,0,0,1,1,0,0,0,1,0}, +{0,0,1,0,0,0,0,0,0,0,0,0,0,1,0,0}, +{0,0,0,1,1,1,1,1,1,1,1,1,1,0,0,0}, +}; +#endif + +char dsp_data[16][16]; +char dsp_data2[16][16]; + +/* 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); +#if DBG + pc.printf("Connected\n\r"); +#endif +} + +void onDisconnected(Gap::Handle_t handle, Gap::DisconnectionReason_t reason) +{ + wb.stop(); + + ble.startAdvertising(); + wb.set_led2(0); +#if DBG + 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 ); +#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 + 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); + } + } + } + } +} + +void line_follow_mode(void) +{ + switch(wb.GetLinePosition()) + { + case 1: // ???????? + wb.left_motor(1.0); + wb.right_motor(-1.0); + break; + case 3: // ???????? + wb.left_motor(1.0); + wb.right_motor(-0.5); + break; + case 2: // ???????? + wb.left_motor(1.0); + wb.right_motor(0.5); + break; + case 6: // ???????? + wb.left_motor(1.0); + wb.right_motor(1.0); + break; + case 4: // ???????? + wb.left_motor(0.5); + wb.right_motor(1.0); + break; + case 12: // ???????? + wb.left_motor(-0.5); + wb.right_motor(1.0); + break; + case 8: // ???????? + wb.left_motor(-1.0); + wb.right_motor(1.0); + break; + default: + wb.left_motor(1.0); + wb.right_motor(1.0); + break; + } +} + +float read_rad() +{ + int16_t ax, ay, az; + int16_t gx, gy, gz; + static float old_rad = 1000.0; + + mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); + //pc.printf("%d;%d;%d;%d;%d;%d\n\r",ax,ay,az,gx,gy,gz); + float rad; + if( ax > 0.0 ) + { + rad = ((float)ay / 16000.0) * 90.0; + } + else + { + rad = ((float)ay / 16000.0) * 90.0; + if( ay > 0 ) + { + rad = 180 - rad; + } + else + { + rad = -180 - rad; + } + } + int d; + if( rad > 0 ) + { + d = (rad + 0.5) / 10; + } + else + { + d = (rad - 0.5) / 10; + } + d *= 10; + rad = -d; + //pc.printf("%f;%d;%d\n\r",rad,ax,ay); + + if( old_rad != 1000.0 ) + { + if( (old_rad + 20.0) >= rad) + { + old_rad = rad; + } + else if( (old_rad - 20.0) <= rad) + { + + } + else + { + rad = old_rad; + } + } + return(rad); +} + + +void drowDisplay() +{ + for (int y = 0; y < 8; y++) { + for (int x = 0; x < 16; x++) { + if( dsp_data[x][y+8] == 1 ) + { + matrix1.drawPixel(x, y, LED_ON); + } + else + { + matrix1.drawPixel(x, y, LED_OFF); + } + if( dsp_data[x][y] == 1 ) + { + matrix2.drawPixel(x, y, LED_ON); + } + else + { + matrix2.drawPixel(x, y, LED_OFF); + } + } + } + matrix1.writeDisplay(); // write the changes we just made to the display + matrix2.writeDisplay(); // write the changes we just made to the display + +} + +/**************************************************************************/ +/*! + @brief Program entry point +*/ +/**************************************************************************/ +int main(void) +{ + char in[ 16 ][ 16 ]; // + char in2[ 16 ][ 16 ]; // + float rad_set = 0.0; + float rad = 0.0; + int count = 0; + + pc.printf("START\n\r"); + + mpu.initialize(); +#if 1 + matrix1.begin(0x71); + matrix2.begin(0x70); +#else + matrix1.begin(0x70); + matrix2.begin(0x74); +#endif + matrix1.setBrightness(15); + matrix2.setBrightness(15); + matrix1.clear(); + matrix2.clear(); + + for ( int i = 0; i < 16; i++ ) + for ( int j = 0; j < 16; j++ ) + in[15-i][j] = data[i][j]; + + for ( int i = 0; i < 16; i++ ) + for ( int j = 0; j < 16; j++ ) + dsp_data[i][j] = in[i][j]; + + for ( int i = 0; i < 16; i++ ) + for ( int j = 0; j < 16; j++ ) + in2[15-i][j] = data2[i][j]; + + for ( int i = 0; i < 16; i++ ) + for ( int j = 0; j < 16; j++ ) + dsp_data[i][j] = in2[i][j]; + + drowDisplay(); + + wb.f_sensor_calibrate(); + +#if DBG + pc.baud(9600); + DBG(pc.printf("Start\n\r");) +#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(wb.GetSw()) + { + if( mode == LineFollow) + { + wb.stop(); + + mode = Nomal; + wb.set_led1(0); + } + else + { + mode = LineFollow; + wb.set_led1(1); + } + wait(1.0); + } + if(mode == LineFollow) + { + line_follow_mode(); + } + else + { + ble.waitForEvent(); + } + if(mode == Challenge) + { + count++; + if(count > 50) + { + count = 0; + pl.scroll(&in[0][0],&dsp_data[0][0]); + drowDisplay(); + for ( int i = 0; i < 16; i++ ) + for ( int j = 0; j < 16; j++ ) + in[i][j] = dsp_data[i][j]; + } + } + else + { + rad = read_rad(); + + if( (rad > -45.0)&&(rad < 45.0 ) ) + { + rad = 0.0; + } + else if( (rad > 45.0)&&(rad < 135.0 ) ) + { + rad = 90.0; + } + else if( ((rad > 135.0)&&(rad < 180.0 ))||((rad > -180.0)&&(rad < -135.0 )) ) + { + rad = 180.0; + } + else if( (rad > -135.0)&&(rad < -45.0 ) ) + { + rad = -90.0; + } + else + { + + } + + if( rad_set > rad ) + { + if( (rad_set == 180)&&(rad <0) ) + { + rad_set = -175; + } + else + { + rad_set -= 5.0; + } + } + else if( rad_set < rad ) + { + if( (rad == 180.0)&&(rad_set <0)) + { + rad_set -= 5.0; + } + else + { + rad_set += 5.0; + } + } + else + { + ; + } + if(rad_set == -180.0) + { + rad_set = 180.0; + } + pl.rotation(rad_set,&in2[0][0],&dsp_data[0][0]); + drowDisplay(); + } + } +} +