Junichi Katsu
/
BLE_MPU6050_test6_challenge_sb
テスト用です。
main.cpp@0:8468a4403fea, 2016-10-11 (annotated)
- Committer:
- jksoft
- Date:
- Tue Oct 11 11:09:42 2016 +0000
- Revision:
- 0:8468a4403fea
SB??ver;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
jksoft | 0:8468a4403fea | 1 | #include "mbed.h" |
jksoft | 0:8468a4403fea | 2 | #include "MPU6050.h" |
jksoft | 0:8468a4403fea | 3 | #include "BLEDevice.h" |
jksoft | 0:8468a4403fea | 4 | #include "wallbotble.h" |
jksoft | 0:8468a4403fea | 5 | #include "RCBController.h" |
jksoft | 0:8468a4403fea | 6 | #include "Adafruit_LEDBackpack.h" |
jksoft | 0:8468a4403fea | 7 | #include "Adafruit_GFX.h" |
jksoft | 0:8468a4403fea | 8 | #include "pictLIB.h" |
jksoft | 0:8468a4403fea | 9 | |
jksoft | 0:8468a4403fea | 10 | |
jksoft | 0:8468a4403fea | 11 | #define DBG 0 |
jksoft | 0:8468a4403fea | 12 | |
jksoft | 0:8468a4403fea | 13 | enum _mode { |
jksoft | 0:8468a4403fea | 14 | Nomal = 0, // ????[?h |
jksoft | 0:8468a4403fea | 15 | LineFollow, // ???C???g???[?X???[?h |
jksoft | 0:8468a4403fea | 16 | Challenge, // ?`???????W???[?h |
jksoft | 0:8468a4403fea | 17 | }; |
jksoft | 0:8468a4403fea | 18 | |
jksoft | 0:8468a4403fea | 19 | Serial pc(USBTX, USBRX); |
jksoft | 0:8468a4403fea | 20 | BLEDevice ble; |
jksoft | 0:8468a4403fea | 21 | Ticker ticker; |
jksoft | 0:8468a4403fea | 22 | MPU6050 mpu; |
jksoft | 0:8468a4403fea | 23 | wallbotble wb; |
jksoft | 0:8468a4403fea | 24 | |
jksoft | 0:8468a4403fea | 25 | myI2C i2c(P0_22,P0_21); |
jksoft | 0:8468a4403fea | 26 | |
jksoft | 0:8468a4403fea | 27 | Adafruit_8x8matrix matrix1 = Adafruit_8x8matrix(&i2c); |
jksoft | 0:8468a4403fea | 28 | Adafruit_8x8matrix matrix2 = Adafruit_8x8matrix(&i2c); |
jksoft | 0:8468a4403fea | 29 | |
jksoft | 0:8468a4403fea | 30 | PICTLIB pl(16,16); |
jksoft | 0:8468a4403fea | 31 | |
jksoft | 0:8468a4403fea | 32 | int mode = Nomal; |
jksoft | 0:8468a4403fea | 33 | |
jksoft | 0:8468a4403fea | 34 | char bValue = 0; |
jksoft | 0:8468a4403fea | 35 | |
jksoft | 0:8468a4403fea | 36 | #if 1 |
jksoft | 0:8468a4403fea | 37 | char data[16][16] = |
jksoft | 0:8468a4403fea | 38 | { |
jksoft | 0:8468a4403fea | 39 | {0,0,0,0,0,0,0,1,1,0,0,0,0,0,0,0}, |
jksoft | 0:8468a4403fea | 40 | {0,0,0,0,0,0,1,1,1,1,0,0,0,0,0,0}, |
jksoft | 0:8468a4403fea | 41 | {0,0,0,0,0,1,1,1,1,1,1,0,0,0,0,0}, |
jksoft | 0:8468a4403fea | 42 | {0,0,0,0,1,1,1,1,1,1,1,1,0,0,0,0}, |
jksoft | 0:8468a4403fea | 43 | {0,0,0,1,1,1,1,1,1,1,1,1,1,0,0,0}, |
jksoft | 0:8468a4403fea | 44 | {0,0,1,1,1,1,1,1,1,1,1,1,1,1,0,0}, |
jksoft | 0:8468a4403fea | 45 | {0,0,0,0,0,0,1,1,1,1,0,0,0,0,0,0}, |
jksoft | 0:8468a4403fea | 46 | {0,0,0,0,0,0,1,1,1,1,0,0,0,0,0,0}, |
jksoft | 0:8468a4403fea | 47 | {0,0,0,0,0,0,1,1,1,1,0,0,0,0,0,0}, |
jksoft | 0:8468a4403fea | 48 | {0,0,0,0,0,0,1,1,1,1,0,0,0,0,0,0}, |
jksoft | 0:8468a4403fea | 49 | {0,0,0,0,0,0,1,1,1,1,0,0,0,0,0,0}, |
jksoft | 0:8468a4403fea | 50 | {0,0,0,0,0,0,1,1,1,1,0,0,0,0,0,0}, |
jksoft | 0:8468a4403fea | 51 | {0,0,0,0,0,0,1,1,1,1,0,0,0,0,0,0}, |
jksoft | 0:8468a4403fea | 52 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}, |
jksoft | 0:8468a4403fea | 53 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}, |
jksoft | 0:8468a4403fea | 54 | {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}, |
jksoft | 0:8468a4403fea | 55 | }; |
jksoft | 0:8468a4403fea | 56 | char data2[16][16] = |
jksoft | 0:8468a4403fea | 57 | { |
jksoft | 0:8468a4403fea | 58 | {0,0,0,0,0,1,1,1,1,1,1,0,0,0,0,0}, |
jksoft | 0:8468a4403fea | 59 | {0,0,0,0,1,0,0,0,0,0,0,1,0,0,0,0}, |
jksoft | 0:8468a4403fea | 60 | {0,0,0,1,0,0,0,0,0,0,0,0,1,0,0,0}, |
jksoft | 0:8468a4403fea | 61 | {0,0,1,0,0,0,0,0,0,0,0,0,0,1,0,0}, |
jksoft | 0:8468a4403fea | 62 | {0,1,0,0,0,0,0,0,0,0,0,0,0,0,1,0}, |
jksoft | 0:8468a4403fea | 63 | {1,0,0,0,1,0,0,0,0,0,0,1,0,0,0,1}, |
jksoft | 0:8468a4403fea | 64 | {1,0,0,0,1,0,0,0,0,0,0,1,0,0,0,1}, |
jksoft | 0:8468a4403fea | 65 | {1,0,0,0,1,0,0,0,0,0,0,1,0,0,0,1}, |
jksoft | 0:8468a4403fea | 66 | {1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1}, |
jksoft | 0:8468a4403fea | 67 | {1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1}, |
jksoft | 0:8468a4403fea | 68 | {1,0,0,0,1,0,0,0,0,0,0,1,0,0,0,1}, |
jksoft | 0:8468a4403fea | 69 | {0,1,0,0,0,1,0,0,0,0,1,0,0,0,1,0}, |
jksoft | 0:8468a4403fea | 70 | {0,0,1,0,0,0,1,1,1,1,0,0,0,1,0,0}, |
jksoft | 0:8468a4403fea | 71 | {0,0,0,1,0,0,0,0,0,0,0,0,1,0,0,0}, |
jksoft | 0:8468a4403fea | 72 | {0,0,0,0,1,0,0,0,0,0,0,1,0,0,0,0}, |
jksoft | 0:8468a4403fea | 73 | {0,0,0,0,0,1,1,1,1,1,1,0,0,0,0,0}, |
jksoft | 0:8468a4403fea | 74 | }; |
jksoft | 0:8468a4403fea | 75 | #else |
jksoft | 0:8468a4403fea | 76 | char data[16][16] = |
jksoft | 0:8468a4403fea | 77 | { |
jksoft | 0:8468a4403fea | 78 | {0,0,0,0,1,0,0,0,0,0,0,1,0,0,0,0}, |
jksoft | 0:8468a4403fea | 79 | {0,0,0,1,0,1,0,0,0,0,1,0,1,0,0,0}, |
jksoft | 0:8468a4403fea | 80 | {0,0,0,1,0,1,0,0,0,0,1,0,1,0,0,0}, |
jksoft | 0:8468a4403fea | 81 | {0,0,1,0,0,0,1,0,0,1,0,0,0,1,0,0}, |
jksoft | 0:8468a4403fea | 82 | {0,0,1,0,0,0,1,1,1,1,0,0,0,1,0,0}, |
jksoft | 0:8468a4403fea | 83 | {0,1,0,0,0,0,0,0,0,0,0,0,0,0,1,0}, |
jksoft | 0:8468a4403fea | 84 | {0,1,0,0,0,1,0,0,0,0,1,0,0,0,1,0}, |
jksoft | 0:8468a4403fea | 85 | {0,1,0,0,0,1,0,0,0,0,1,0,0,0,1,0}, |
jksoft | 0:8468a4403fea | 86 | {0,1,0,0,0,1,0,0,0,0,1,0,0,0,1,0}, |
jksoft | 0:8468a4403fea | 87 | {0,1,0,0,0,0,0,1,1,0,0,0,0,0,1,0}, |
jksoft | 0:8468a4403fea | 88 | {0,1,0,0,0,0,1,0,0,1,0,0,0,0,1,0}, |
jksoft | 0:8468a4403fea | 89 | {0,1,0,0,0,0,0,1,1,0,0,0,0,0,1,0}, |
jksoft | 0:8468a4403fea | 90 | {0,1,0,0,1,0,0,1,1,0,0,1,0,0,1,0}, |
jksoft | 0:8468a4403fea | 91 | {0,1,0,0,0,1,1,0,0,1,1,0,0,0,1,0}, |
jksoft | 0:8468a4403fea | 92 | {0,0,1,0,0,0,0,0,0,0,0,0,0,1,0,0}, |
jksoft | 0:8468a4403fea | 93 | {0,0,0,1,1,1,1,1,1,1,1,1,1,0,0,0}, |
jksoft | 0:8468a4403fea | 94 | }; |
jksoft | 0:8468a4403fea | 95 | #endif |
jksoft | 0:8468a4403fea | 96 | |
jksoft | 0:8468a4403fea | 97 | char dsp_data[16][16]; |
jksoft | 0:8468a4403fea | 98 | char dsp_data2[16][16]; |
jksoft | 0:8468a4403fea | 99 | |
jksoft | 0:8468a4403fea | 100 | /* RCBController Service */ |
jksoft | 0:8468a4403fea | 101 | static const uint16_t RCBController_service_uuid = 0xFFF0; |
jksoft | 0:8468a4403fea | 102 | static const uint16_t RCBController_Characteristic_uuid = 0xFFF1; |
jksoft | 0:8468a4403fea | 103 | static const uint16_t RCBController_b_Characteristic_uuid = 0xFFF3; |
jksoft | 0:8468a4403fea | 104 | uint8_t RCBControllerPayload[10] = {0,}; |
jksoft | 0:8468a4403fea | 105 | |
jksoft | 0:8468a4403fea | 106 | GattCharacteristic ControllerChar (RCBController_Characteristic_uuid,RCBControllerPayload,10, 10, |
jksoft | 0:8468a4403fea | 107 | GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE | |
jksoft | 0:8468a4403fea | 108 | GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE_WITHOUT_RESPONSE); |
jksoft | 0:8468a4403fea | 109 | //static uint8_t _bValue = 0x00; |
jksoft | 0:8468a4403fea | 110 | static uint8_t _mValue[10] = {0,}; |
jksoft | 0:8468a4403fea | 111 | GattCharacteristic b_Char(RCBController_b_Characteristic_uuid, _mValue, sizeof(_mValue), sizeof(_mValue), |
jksoft | 0:8468a4403fea | 112 | GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ | GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY); |
jksoft | 0:8468a4403fea | 113 | |
jksoft | 0:8468a4403fea | 114 | GattCharacteristic *ControllerChars[] = {&ControllerChar,&b_Char}; |
jksoft | 0:8468a4403fea | 115 | GattService RCBControllerService(RCBController_service_uuid, ControllerChars, sizeof(ControllerChars) / sizeof(GattCharacteristic *)); |
jksoft | 0:8468a4403fea | 116 | |
jksoft | 0:8468a4403fea | 117 | RCBController controller; |
jksoft | 0:8468a4403fea | 118 | |
jksoft | 0:8468a4403fea | 119 | |
jksoft | 0:8468a4403fea | 120 | void SendMessage(char *msg) |
jksoft | 0:8468a4403fea | 121 | { |
jksoft | 0:8468a4403fea | 122 | if (!ble.getGapState().connected) { |
jksoft | 0:8468a4403fea | 123 | return; |
jksoft | 0:8468a4403fea | 124 | } |
jksoft | 0:8468a4403fea | 125 | int len = strlen(msg); |
jksoft | 0:8468a4403fea | 126 | |
jksoft | 0:8468a4403fea | 127 | if(len < 10) |
jksoft | 0:8468a4403fea | 128 | { |
jksoft | 0:8468a4403fea | 129 | strcpy( (char*)_mValue ,msg ); |
jksoft | 0:8468a4403fea | 130 | ble.updateCharacteristicValue(b_Char.getValueAttribute().getHandle(), (uint8_t *)_mValue, sizeof(_mValue)); |
jksoft | 0:8468a4403fea | 131 | } |
jksoft | 0:8468a4403fea | 132 | } |
jksoft | 0:8468a4403fea | 133 | |
jksoft | 0:8468a4403fea | 134 | void onConnected(Gap::Handle_t handle, Gap::addr_type_t peerAddrType ,const Gap::address_t peerAddr,const Gap::ConnectionParams_t *params) |
jksoft | 0:8468a4403fea | 135 | { |
jksoft | 0:8468a4403fea | 136 | wb.set_led2(1); |
jksoft | 0:8468a4403fea | 137 | #if DBG |
jksoft | 0:8468a4403fea | 138 | pc.printf("Connected\n\r"); |
jksoft | 0:8468a4403fea | 139 | #endif |
jksoft | 0:8468a4403fea | 140 | } |
jksoft | 0:8468a4403fea | 141 | |
jksoft | 0:8468a4403fea | 142 | void onDisconnected(Gap::Handle_t handle, Gap::DisconnectionReason_t reason) |
jksoft | 0:8468a4403fea | 143 | { |
jksoft | 0:8468a4403fea | 144 | wb.stop(); |
jksoft | 0:8468a4403fea | 145 | |
jksoft | 0:8468a4403fea | 146 | ble.startAdvertising(); |
jksoft | 0:8468a4403fea | 147 | wb.set_led2(0); |
jksoft | 0:8468a4403fea | 148 | #if DBG |
jksoft | 0:8468a4403fea | 149 | pc.printf("Disconnected\n\r"); |
jksoft | 0:8468a4403fea | 150 | #endif |
jksoft | 0:8468a4403fea | 151 | } |
jksoft | 0:8468a4403fea | 152 | |
jksoft | 0:8468a4403fea | 153 | void periodicCallback(void) |
jksoft | 0:8468a4403fea | 154 | { |
jksoft | 0:8468a4403fea | 155 | if (!ble.getGapState().connected) { |
jksoft | 0:8468a4403fea | 156 | return; |
jksoft | 0:8468a4403fea | 157 | } |
jksoft | 0:8468a4403fea | 158 | if( (bValue == 0)&&(wb.GetLinePosition() != 0) ) |
jksoft | 0:8468a4403fea | 159 | { |
jksoft | 0:8468a4403fea | 160 | // Game over |
jksoft | 0:8468a4403fea | 161 | wb.stop(); |
jksoft | 0:8468a4403fea | 162 | bValue = 10; |
jksoft | 0:8468a4403fea | 163 | } |
jksoft | 0:8468a4403fea | 164 | if( bValue > 0 ) |
jksoft | 0:8468a4403fea | 165 | { |
jksoft | 0:8468a4403fea | 166 | wb.stop(); |
jksoft | 0:8468a4403fea | 167 | SendMessage("GAME OVER"); |
jksoft | 0:8468a4403fea | 168 | bValue--; |
jksoft | 0:8468a4403fea | 169 | if( bValue == 0 ) |
jksoft | 0:8468a4403fea | 170 | { |
jksoft | 0:8468a4403fea | 171 | wb.set_led1(0); |
jksoft | 0:8468a4403fea | 172 | SendMessage(" NOMAL "); |
jksoft | 0:8468a4403fea | 173 | mode = Nomal; |
jksoft | 0:8468a4403fea | 174 | ticker.detach(); |
jksoft | 0:8468a4403fea | 175 | } |
jksoft | 0:8468a4403fea | 176 | } |
jksoft | 0:8468a4403fea | 177 | } |
jksoft | 0:8468a4403fea | 178 | |
jksoft | 0:8468a4403fea | 179 | |
jksoft | 0:8468a4403fea | 180 | // GattEvent |
jksoft | 0:8468a4403fea | 181 | void onDataWritten(const GattCharacteristicWriteCBParams *params) |
jksoft | 0:8468a4403fea | 182 | { |
jksoft | 0:8468a4403fea | 183 | if( (params->charHandle == ControllerChar.getValueAttribute().getHandle()) && ( mode != LineFollow )) |
jksoft | 0:8468a4403fea | 184 | { |
jksoft | 0:8468a4403fea | 185 | float right_factor; |
jksoft | 0:8468a4403fea | 186 | float left_factor; |
jksoft | 0:8468a4403fea | 187 | |
jksoft | 0:8468a4403fea | 188 | memcpy( &controller.data[0], params->data , params->len ); |
jksoft | 0:8468a4403fea | 189 | #if DBG |
jksoft | 0:8468a4403fea | 190 | 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], |
jksoft | 0:8468a4403fea | 191 | controller.data[5],controller.data[6],controller.data[7],controller.data[8],controller.data[9]); |
jksoft | 0:8468a4403fea | 192 | #endif |
jksoft | 0:8468a4403fea | 193 | if(mode == Challenge) |
jksoft | 0:8468a4403fea | 194 | { |
jksoft | 0:8468a4403fea | 195 | float factor = ((float)((int)controller.status.AcceleX -128) / 128.0); |
jksoft | 0:8468a4403fea | 196 | |
jksoft | 0:8468a4403fea | 197 | float right_factor = ((factor <= 0.0) ? 1.0 : 1.0 - (factor*2)); |
jksoft | 0:8468a4403fea | 198 | float left_factor = ((factor >= 0.0) ? 1.0 : 1.0 - (-factor*2)); |
jksoft | 0:8468a4403fea | 199 | |
jksoft | 0:8468a4403fea | 200 | if( /*controller.status.B ==*/ 1 ) |
jksoft | 0:8468a4403fea | 201 | { |
jksoft | 0:8468a4403fea | 202 | wb.left_motor(left_factor); |
jksoft | 0:8468a4403fea | 203 | wb.right_motor(right_factor); |
jksoft | 0:8468a4403fea | 204 | } |
jksoft | 0:8468a4403fea | 205 | else if( controller.status.A == 1 ) |
jksoft | 0:8468a4403fea | 206 | { |
jksoft | 0:8468a4403fea | 207 | wb.left_motor(-right_factor); |
jksoft | 0:8468a4403fea | 208 | wb.right_motor(-left_factor); |
jksoft | 0:8468a4403fea | 209 | } |
jksoft | 0:8468a4403fea | 210 | else |
jksoft | 0:8468a4403fea | 211 | { |
jksoft | 0:8468a4403fea | 212 | wb.forward(1.0); |
jksoft | 0:8468a4403fea | 213 | } |
jksoft | 0:8468a4403fea | 214 | } |
jksoft | 0:8468a4403fea | 215 | else |
jksoft | 0:8468a4403fea | 216 | { |
jksoft | 0:8468a4403fea | 217 | if( (controller.status.LeftAnalogUD != 128)||(controller.status.RightAnalogUD != 128) ) |
jksoft | 0:8468a4403fea | 218 | { |
jksoft | 0:8468a4403fea | 219 | left_factor = ((float)((int)controller.status.LeftAnalogUD -128) / 128.0); |
jksoft | 0:8468a4403fea | 220 | right_factor = ((float)((int)controller.status.RightAnalogUD -128) / 128.0); |
jksoft | 0:8468a4403fea | 221 | |
jksoft | 0:8468a4403fea | 222 | wb.left_motor(left_factor); |
jksoft | 0:8468a4403fea | 223 | wb.right_motor(right_factor); |
jksoft | 0:8468a4403fea | 224 | } |
jksoft | 0:8468a4403fea | 225 | else |
jksoft | 0:8468a4403fea | 226 | { |
jksoft | 0:8468a4403fea | 227 | float factor = ((float)((int)controller.status.AcceleX -128) / 128.0); |
jksoft | 0:8468a4403fea | 228 | |
jksoft | 0:8468a4403fea | 229 | right_factor = ((factor <= 0.0) ? 1.0 : 1.0 - (factor*2)); |
jksoft | 0:8468a4403fea | 230 | left_factor = ((factor >= 0.0) ? 1.0 : 1.0 - (-factor*2)); |
jksoft | 0:8468a4403fea | 231 | |
jksoft | 0:8468a4403fea | 232 | if( controller.status.B == 1 ) |
jksoft | 0:8468a4403fea | 233 | { |
jksoft | 0:8468a4403fea | 234 | wb.left_motor(left_factor); |
jksoft | 0:8468a4403fea | 235 | wb.right_motor(right_factor); |
jksoft | 0:8468a4403fea | 236 | } |
jksoft | 0:8468a4403fea | 237 | else if( controller.status.A == 1 ) |
jksoft | 0:8468a4403fea | 238 | { |
jksoft | 0:8468a4403fea | 239 | wb.left_motor(-right_factor); |
jksoft | 0:8468a4403fea | 240 | wb.right_motor(-left_factor); |
jksoft | 0:8468a4403fea | 241 | } |
jksoft | 0:8468a4403fea | 242 | else if( controller.status.UP == 1 ) |
jksoft | 0:8468a4403fea | 243 | { |
jksoft | 0:8468a4403fea | 244 | wb.forward(1.0); |
jksoft | 0:8468a4403fea | 245 | } |
jksoft | 0:8468a4403fea | 246 | else if( controller.status.DOWN == 1 ) |
jksoft | 0:8468a4403fea | 247 | { |
jksoft | 0:8468a4403fea | 248 | wb.backward(1.0); |
jksoft | 0:8468a4403fea | 249 | } |
jksoft | 0:8468a4403fea | 250 | else if( controller.status.RIGHT == 1 ) |
jksoft | 0:8468a4403fea | 251 | { |
jksoft | 0:8468a4403fea | 252 | wb.right_turn(1.0); |
jksoft | 0:8468a4403fea | 253 | } |
jksoft | 0:8468a4403fea | 254 | else if( controller.status.LEFT == 1 ) |
jksoft | 0:8468a4403fea | 255 | { |
jksoft | 0:8468a4403fea | 256 | wb.left_turn(1.0); |
jksoft | 0:8468a4403fea | 257 | } |
jksoft | 0:8468a4403fea | 258 | else |
jksoft | 0:8468a4403fea | 259 | { |
jksoft | 0:8468a4403fea | 260 | wb.stop(); |
jksoft | 0:8468a4403fea | 261 | } |
jksoft | 0:8468a4403fea | 262 | if((controller.status.UP == 1)&&(controller.status.DOWN == 1)) // START BUTTON |
jksoft | 0:8468a4403fea | 263 | { |
jksoft | 0:8468a4403fea | 264 | SendMessage("Challenge"); |
jksoft | 0:8468a4403fea | 265 | wb.set_led1(1); |
jksoft | 0:8468a4403fea | 266 | mode = Challenge; |
jksoft | 0:8468a4403fea | 267 | wb.stop(); |
jksoft | 0:8468a4403fea | 268 | ticker.attach(periodicCallback, 0.1); |
jksoft | 0:8468a4403fea | 269 | } |
jksoft | 0:8468a4403fea | 270 | } |
jksoft | 0:8468a4403fea | 271 | } |
jksoft | 0:8468a4403fea | 272 | } |
jksoft | 0:8468a4403fea | 273 | } |
jksoft | 0:8468a4403fea | 274 | |
jksoft | 0:8468a4403fea | 275 | void line_follow_mode(void) |
jksoft | 0:8468a4403fea | 276 | { |
jksoft | 0:8468a4403fea | 277 | switch(wb.GetLinePosition()) |
jksoft | 0:8468a4403fea | 278 | { |
jksoft | 0:8468a4403fea | 279 | case 1: // ???????? |
jksoft | 0:8468a4403fea | 280 | wb.left_motor(1.0); |
jksoft | 0:8468a4403fea | 281 | wb.right_motor(-1.0); |
jksoft | 0:8468a4403fea | 282 | break; |
jksoft | 0:8468a4403fea | 283 | case 3: // ???????? |
jksoft | 0:8468a4403fea | 284 | wb.left_motor(1.0); |
jksoft | 0:8468a4403fea | 285 | wb.right_motor(-0.5); |
jksoft | 0:8468a4403fea | 286 | break; |
jksoft | 0:8468a4403fea | 287 | case 2: // ???????? |
jksoft | 0:8468a4403fea | 288 | wb.left_motor(1.0); |
jksoft | 0:8468a4403fea | 289 | wb.right_motor(0.5); |
jksoft | 0:8468a4403fea | 290 | break; |
jksoft | 0:8468a4403fea | 291 | case 6: // ???????? |
jksoft | 0:8468a4403fea | 292 | wb.left_motor(1.0); |
jksoft | 0:8468a4403fea | 293 | wb.right_motor(1.0); |
jksoft | 0:8468a4403fea | 294 | break; |
jksoft | 0:8468a4403fea | 295 | case 4: // ???????? |
jksoft | 0:8468a4403fea | 296 | wb.left_motor(0.5); |
jksoft | 0:8468a4403fea | 297 | wb.right_motor(1.0); |
jksoft | 0:8468a4403fea | 298 | break; |
jksoft | 0:8468a4403fea | 299 | case 12: // ???????? |
jksoft | 0:8468a4403fea | 300 | wb.left_motor(-0.5); |
jksoft | 0:8468a4403fea | 301 | wb.right_motor(1.0); |
jksoft | 0:8468a4403fea | 302 | break; |
jksoft | 0:8468a4403fea | 303 | case 8: // ???????? |
jksoft | 0:8468a4403fea | 304 | wb.left_motor(-1.0); |
jksoft | 0:8468a4403fea | 305 | wb.right_motor(1.0); |
jksoft | 0:8468a4403fea | 306 | break; |
jksoft | 0:8468a4403fea | 307 | default: |
jksoft | 0:8468a4403fea | 308 | wb.left_motor(1.0); |
jksoft | 0:8468a4403fea | 309 | wb.right_motor(1.0); |
jksoft | 0:8468a4403fea | 310 | break; |
jksoft | 0:8468a4403fea | 311 | } |
jksoft | 0:8468a4403fea | 312 | } |
jksoft | 0:8468a4403fea | 313 | |
jksoft | 0:8468a4403fea | 314 | float read_rad() |
jksoft | 0:8468a4403fea | 315 | { |
jksoft | 0:8468a4403fea | 316 | int16_t ax, ay, az; |
jksoft | 0:8468a4403fea | 317 | int16_t gx, gy, gz; |
jksoft | 0:8468a4403fea | 318 | static float old_rad = 1000.0; |
jksoft | 0:8468a4403fea | 319 | |
jksoft | 0:8468a4403fea | 320 | mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); |
jksoft | 0:8468a4403fea | 321 | //pc.printf("%d;%d;%d;%d;%d;%d\n\r",ax,ay,az,gx,gy,gz); |
jksoft | 0:8468a4403fea | 322 | float rad; |
jksoft | 0:8468a4403fea | 323 | if( ax > 0.0 ) |
jksoft | 0:8468a4403fea | 324 | { |
jksoft | 0:8468a4403fea | 325 | rad = ((float)ay / 16000.0) * 90.0; |
jksoft | 0:8468a4403fea | 326 | } |
jksoft | 0:8468a4403fea | 327 | else |
jksoft | 0:8468a4403fea | 328 | { |
jksoft | 0:8468a4403fea | 329 | rad = ((float)ay / 16000.0) * 90.0; |
jksoft | 0:8468a4403fea | 330 | if( ay > 0 ) |
jksoft | 0:8468a4403fea | 331 | { |
jksoft | 0:8468a4403fea | 332 | rad = 180 - rad; |
jksoft | 0:8468a4403fea | 333 | } |
jksoft | 0:8468a4403fea | 334 | else |
jksoft | 0:8468a4403fea | 335 | { |
jksoft | 0:8468a4403fea | 336 | rad = -180 - rad; |
jksoft | 0:8468a4403fea | 337 | } |
jksoft | 0:8468a4403fea | 338 | } |
jksoft | 0:8468a4403fea | 339 | int d; |
jksoft | 0:8468a4403fea | 340 | if( rad > 0 ) |
jksoft | 0:8468a4403fea | 341 | { |
jksoft | 0:8468a4403fea | 342 | d = (rad + 0.5) / 10; |
jksoft | 0:8468a4403fea | 343 | } |
jksoft | 0:8468a4403fea | 344 | else |
jksoft | 0:8468a4403fea | 345 | { |
jksoft | 0:8468a4403fea | 346 | d = (rad - 0.5) / 10; |
jksoft | 0:8468a4403fea | 347 | } |
jksoft | 0:8468a4403fea | 348 | d *= 10; |
jksoft | 0:8468a4403fea | 349 | rad = -d; |
jksoft | 0:8468a4403fea | 350 | //pc.printf("%f;%d;%d\n\r",rad,ax,ay); |
jksoft | 0:8468a4403fea | 351 | |
jksoft | 0:8468a4403fea | 352 | if( old_rad != 1000.0 ) |
jksoft | 0:8468a4403fea | 353 | { |
jksoft | 0:8468a4403fea | 354 | if( (old_rad + 20.0) >= rad) |
jksoft | 0:8468a4403fea | 355 | { |
jksoft | 0:8468a4403fea | 356 | old_rad = rad; |
jksoft | 0:8468a4403fea | 357 | } |
jksoft | 0:8468a4403fea | 358 | else if( (old_rad - 20.0) <= rad) |
jksoft | 0:8468a4403fea | 359 | { |
jksoft | 0:8468a4403fea | 360 | |
jksoft | 0:8468a4403fea | 361 | } |
jksoft | 0:8468a4403fea | 362 | else |
jksoft | 0:8468a4403fea | 363 | { |
jksoft | 0:8468a4403fea | 364 | rad = old_rad; |
jksoft | 0:8468a4403fea | 365 | } |
jksoft | 0:8468a4403fea | 366 | } |
jksoft | 0:8468a4403fea | 367 | return(rad); |
jksoft | 0:8468a4403fea | 368 | } |
jksoft | 0:8468a4403fea | 369 | |
jksoft | 0:8468a4403fea | 370 | |
jksoft | 0:8468a4403fea | 371 | void drowDisplay() |
jksoft | 0:8468a4403fea | 372 | { |
jksoft | 0:8468a4403fea | 373 | for (int y = 0; y < 8; y++) { |
jksoft | 0:8468a4403fea | 374 | for (int x = 0; x < 16; x++) { |
jksoft | 0:8468a4403fea | 375 | if( dsp_data[x][y+8] == 1 ) |
jksoft | 0:8468a4403fea | 376 | { |
jksoft | 0:8468a4403fea | 377 | matrix1.drawPixel(x, y, LED_ON); |
jksoft | 0:8468a4403fea | 378 | } |
jksoft | 0:8468a4403fea | 379 | else |
jksoft | 0:8468a4403fea | 380 | { |
jksoft | 0:8468a4403fea | 381 | matrix1.drawPixel(x, y, LED_OFF); |
jksoft | 0:8468a4403fea | 382 | } |
jksoft | 0:8468a4403fea | 383 | if( dsp_data[x][y] == 1 ) |
jksoft | 0:8468a4403fea | 384 | { |
jksoft | 0:8468a4403fea | 385 | matrix2.drawPixel(x, y, LED_ON); |
jksoft | 0:8468a4403fea | 386 | } |
jksoft | 0:8468a4403fea | 387 | else |
jksoft | 0:8468a4403fea | 388 | { |
jksoft | 0:8468a4403fea | 389 | matrix2.drawPixel(x, y, LED_OFF); |
jksoft | 0:8468a4403fea | 390 | } |
jksoft | 0:8468a4403fea | 391 | } |
jksoft | 0:8468a4403fea | 392 | } |
jksoft | 0:8468a4403fea | 393 | matrix1.writeDisplay(); // write the changes we just made to the display |
jksoft | 0:8468a4403fea | 394 | matrix2.writeDisplay(); // write the changes we just made to the display |
jksoft | 0:8468a4403fea | 395 | |
jksoft | 0:8468a4403fea | 396 | } |
jksoft | 0:8468a4403fea | 397 | |
jksoft | 0:8468a4403fea | 398 | /**************************************************************************/ |
jksoft | 0:8468a4403fea | 399 | /*! |
jksoft | 0:8468a4403fea | 400 | @brief Program entry point |
jksoft | 0:8468a4403fea | 401 | */ |
jksoft | 0:8468a4403fea | 402 | /**************************************************************************/ |
jksoft | 0:8468a4403fea | 403 | int main(void) |
jksoft | 0:8468a4403fea | 404 | { |
jksoft | 0:8468a4403fea | 405 | char in[ 16 ][ 16 ]; // |
jksoft | 0:8468a4403fea | 406 | char in2[ 16 ][ 16 ]; // |
jksoft | 0:8468a4403fea | 407 | float rad_set = 0.0; |
jksoft | 0:8468a4403fea | 408 | float rad = 0.0; |
jksoft | 0:8468a4403fea | 409 | int count = 0; |
jksoft | 0:8468a4403fea | 410 | |
jksoft | 0:8468a4403fea | 411 | pc.printf("START\n\r"); |
jksoft | 0:8468a4403fea | 412 | |
jksoft | 0:8468a4403fea | 413 | mpu.initialize(); |
jksoft | 0:8468a4403fea | 414 | #if 1 |
jksoft | 0:8468a4403fea | 415 | matrix1.begin(0x71); |
jksoft | 0:8468a4403fea | 416 | matrix2.begin(0x70); |
jksoft | 0:8468a4403fea | 417 | #else |
jksoft | 0:8468a4403fea | 418 | matrix1.begin(0x70); |
jksoft | 0:8468a4403fea | 419 | matrix2.begin(0x74); |
jksoft | 0:8468a4403fea | 420 | #endif |
jksoft | 0:8468a4403fea | 421 | matrix1.setBrightness(15); |
jksoft | 0:8468a4403fea | 422 | matrix2.setBrightness(15); |
jksoft | 0:8468a4403fea | 423 | matrix1.clear(); |
jksoft | 0:8468a4403fea | 424 | matrix2.clear(); |
jksoft | 0:8468a4403fea | 425 | |
jksoft | 0:8468a4403fea | 426 | for ( int i = 0; i < 16; i++ ) |
jksoft | 0:8468a4403fea | 427 | for ( int j = 0; j < 16; j++ ) |
jksoft | 0:8468a4403fea | 428 | in[15-i][j] = data[i][j]; |
jksoft | 0:8468a4403fea | 429 | |
jksoft | 0:8468a4403fea | 430 | for ( int i = 0; i < 16; i++ ) |
jksoft | 0:8468a4403fea | 431 | for ( int j = 0; j < 16; j++ ) |
jksoft | 0:8468a4403fea | 432 | dsp_data[i][j] = in[i][j]; |
jksoft | 0:8468a4403fea | 433 | |
jksoft | 0:8468a4403fea | 434 | for ( int i = 0; i < 16; i++ ) |
jksoft | 0:8468a4403fea | 435 | for ( int j = 0; j < 16; j++ ) |
jksoft | 0:8468a4403fea | 436 | in2[15-i][j] = data2[i][j]; |
jksoft | 0:8468a4403fea | 437 | |
jksoft | 0:8468a4403fea | 438 | for ( int i = 0; i < 16; i++ ) |
jksoft | 0:8468a4403fea | 439 | for ( int j = 0; j < 16; j++ ) |
jksoft | 0:8468a4403fea | 440 | dsp_data[i][j] = in2[i][j]; |
jksoft | 0:8468a4403fea | 441 | |
jksoft | 0:8468a4403fea | 442 | drowDisplay(); |
jksoft | 0:8468a4403fea | 443 | |
jksoft | 0:8468a4403fea | 444 | wb.f_sensor_calibrate(); |
jksoft | 0:8468a4403fea | 445 | |
jksoft | 0:8468a4403fea | 446 | #if DBG |
jksoft | 0:8468a4403fea | 447 | pc.baud(9600); |
jksoft | 0:8468a4403fea | 448 | DBG(pc.printf("Start\n\r");) |
jksoft | 0:8468a4403fea | 449 | #endif |
jksoft | 0:8468a4403fea | 450 | |
jksoft | 0:8468a4403fea | 451 | ble.init(); |
jksoft | 0:8468a4403fea | 452 | ble.onConnection(onConnected); |
jksoft | 0:8468a4403fea | 453 | ble.onDisconnection(onDisconnected); |
jksoft | 0:8468a4403fea | 454 | ble.onDataWritten(onDataWritten); |
jksoft | 0:8468a4403fea | 455 | |
jksoft | 0:8468a4403fea | 456 | /* setup advertising */ |
jksoft | 0:8468a4403fea | 457 | ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED); |
jksoft | 0:8468a4403fea | 458 | ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED); |
jksoft | 0:8468a4403fea | 459 | ble.accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME, |
jksoft | 0:8468a4403fea | 460 | (const uint8_t *)"mbed WallbotBLE", sizeof("mbed WallbotBLE") - 1); |
jksoft | 0:8468a4403fea | 461 | ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, |
jksoft | 0:8468a4403fea | 462 | (const uint8_t *)RCBController_service_uuid, sizeof(RCBController_service_uuid)); |
jksoft | 0:8468a4403fea | 463 | |
jksoft | 0:8468a4403fea | 464 | ble.setAdvertisingInterval(160); /* 100ms; in multiples of 0.625ms. */ |
jksoft | 0:8468a4403fea | 465 | ble.startAdvertising(); |
jksoft | 0:8468a4403fea | 466 | |
jksoft | 0:8468a4403fea | 467 | ble.addService(RCBControllerService); |
jksoft | 0:8468a4403fea | 468 | |
jksoft | 0:8468a4403fea | 469 | |
jksoft | 0:8468a4403fea | 470 | while (true) { |
jksoft | 0:8468a4403fea | 471 | |
jksoft | 0:8468a4403fea | 472 | if(wb.GetSw()) |
jksoft | 0:8468a4403fea | 473 | { |
jksoft | 0:8468a4403fea | 474 | if( mode == LineFollow) |
jksoft | 0:8468a4403fea | 475 | { |
jksoft | 0:8468a4403fea | 476 | wb.stop(); |
jksoft | 0:8468a4403fea | 477 | |
jksoft | 0:8468a4403fea | 478 | mode = Nomal; |
jksoft | 0:8468a4403fea | 479 | wb.set_led1(0); |
jksoft | 0:8468a4403fea | 480 | } |
jksoft | 0:8468a4403fea | 481 | else |
jksoft | 0:8468a4403fea | 482 | { |
jksoft | 0:8468a4403fea | 483 | mode = LineFollow; |
jksoft | 0:8468a4403fea | 484 | wb.set_led1(1); |
jksoft | 0:8468a4403fea | 485 | } |
jksoft | 0:8468a4403fea | 486 | wait(1.0); |
jksoft | 0:8468a4403fea | 487 | } |
jksoft | 0:8468a4403fea | 488 | if(mode == LineFollow) |
jksoft | 0:8468a4403fea | 489 | { |
jksoft | 0:8468a4403fea | 490 | line_follow_mode(); |
jksoft | 0:8468a4403fea | 491 | } |
jksoft | 0:8468a4403fea | 492 | else |
jksoft | 0:8468a4403fea | 493 | { |
jksoft | 0:8468a4403fea | 494 | ble.waitForEvent(); |
jksoft | 0:8468a4403fea | 495 | } |
jksoft | 0:8468a4403fea | 496 | if(mode == Challenge) |
jksoft | 0:8468a4403fea | 497 | { |
jksoft | 0:8468a4403fea | 498 | count++; |
jksoft | 0:8468a4403fea | 499 | if(count > 50) |
jksoft | 0:8468a4403fea | 500 | { |
jksoft | 0:8468a4403fea | 501 | count = 0; |
jksoft | 0:8468a4403fea | 502 | pl.scroll(&in[0][0],&dsp_data[0][0]); |
jksoft | 0:8468a4403fea | 503 | drowDisplay(); |
jksoft | 0:8468a4403fea | 504 | for ( int i = 0; i < 16; i++ ) |
jksoft | 0:8468a4403fea | 505 | for ( int j = 0; j < 16; j++ ) |
jksoft | 0:8468a4403fea | 506 | in[i][j] = dsp_data[i][j]; |
jksoft | 0:8468a4403fea | 507 | } |
jksoft | 0:8468a4403fea | 508 | } |
jksoft | 0:8468a4403fea | 509 | else |
jksoft | 0:8468a4403fea | 510 | { |
jksoft | 0:8468a4403fea | 511 | rad = read_rad(); |
jksoft | 0:8468a4403fea | 512 | |
jksoft | 0:8468a4403fea | 513 | if( (rad > -45.0)&&(rad < 45.0 ) ) |
jksoft | 0:8468a4403fea | 514 | { |
jksoft | 0:8468a4403fea | 515 | rad = 0.0; |
jksoft | 0:8468a4403fea | 516 | } |
jksoft | 0:8468a4403fea | 517 | else if( (rad > 45.0)&&(rad < 135.0 ) ) |
jksoft | 0:8468a4403fea | 518 | { |
jksoft | 0:8468a4403fea | 519 | rad = 90.0; |
jksoft | 0:8468a4403fea | 520 | } |
jksoft | 0:8468a4403fea | 521 | else if( ((rad > 135.0)&&(rad < 180.0 ))||((rad > -180.0)&&(rad < -135.0 )) ) |
jksoft | 0:8468a4403fea | 522 | { |
jksoft | 0:8468a4403fea | 523 | rad = 180.0; |
jksoft | 0:8468a4403fea | 524 | } |
jksoft | 0:8468a4403fea | 525 | else if( (rad > -135.0)&&(rad < -45.0 ) ) |
jksoft | 0:8468a4403fea | 526 | { |
jksoft | 0:8468a4403fea | 527 | rad = -90.0; |
jksoft | 0:8468a4403fea | 528 | } |
jksoft | 0:8468a4403fea | 529 | else |
jksoft | 0:8468a4403fea | 530 | { |
jksoft | 0:8468a4403fea | 531 | |
jksoft | 0:8468a4403fea | 532 | } |
jksoft | 0:8468a4403fea | 533 | |
jksoft | 0:8468a4403fea | 534 | if( rad_set > rad ) |
jksoft | 0:8468a4403fea | 535 | { |
jksoft | 0:8468a4403fea | 536 | if( (rad_set == 180)&&(rad <0) ) |
jksoft | 0:8468a4403fea | 537 | { |
jksoft | 0:8468a4403fea | 538 | rad_set = -175; |
jksoft | 0:8468a4403fea | 539 | } |
jksoft | 0:8468a4403fea | 540 | else |
jksoft | 0:8468a4403fea | 541 | { |
jksoft | 0:8468a4403fea | 542 | rad_set -= 5.0; |
jksoft | 0:8468a4403fea | 543 | } |
jksoft | 0:8468a4403fea | 544 | } |
jksoft | 0:8468a4403fea | 545 | else if( rad_set < rad ) |
jksoft | 0:8468a4403fea | 546 | { |
jksoft | 0:8468a4403fea | 547 | if( (rad == 180.0)&&(rad_set <0)) |
jksoft | 0:8468a4403fea | 548 | { |
jksoft | 0:8468a4403fea | 549 | rad_set -= 5.0; |
jksoft | 0:8468a4403fea | 550 | } |
jksoft | 0:8468a4403fea | 551 | else |
jksoft | 0:8468a4403fea | 552 | { |
jksoft | 0:8468a4403fea | 553 | rad_set += 5.0; |
jksoft | 0:8468a4403fea | 554 | } |
jksoft | 0:8468a4403fea | 555 | } |
jksoft | 0:8468a4403fea | 556 | else |
jksoft | 0:8468a4403fea | 557 | { |
jksoft | 0:8468a4403fea | 558 | ; |
jksoft | 0:8468a4403fea | 559 | } |
jksoft | 0:8468a4403fea | 560 | if(rad_set == -180.0) |
jksoft | 0:8468a4403fea | 561 | { |
jksoft | 0:8468a4403fea | 562 | rad_set = 180.0; |
jksoft | 0:8468a4403fea | 563 | } |
jksoft | 0:8468a4403fea | 564 | pl.rotation(rad_set,&in2[0][0],&dsp_data[0][0]); |
jksoft | 0:8468a4403fea | 565 | drowDisplay(); |
jksoft | 0:8468a4403fea | 566 | } |
jksoft | 0:8468a4403fea | 567 | } |
jksoft | 0:8468a4403fea | 568 | } |
jksoft | 0:8468a4403fea | 569 |