Junichi Katsu
テスト用です。
Diff: main.cpp
- Revision:
- 0:8468a4403fea
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Tue Oct 11 11:09:42 2016 +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();
+ }
+ }
+}
+