Junichi Katsu / Mbed 2 deprecated BLE_MPU6050_test6_challenge_sb

テスト用です。

Dependencies:   mbed

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?

UserRevisionLine numberNew 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