Maiko Matsumoto / Mbed 2 deprecated BLE_WallbotBLE_Challenge_byYUTAKA3

6/18 wallbot

Dependencies:   mbed

Fork of BLE_WallbotBLE_Challenge_byYUTAKA by Yutaka Yoshida

main.cpp@3:f707552ec50a, 2018-06-03 (annotated)

Committer:
Dyotty
Date:
Sun Jun 03 07:04:34 2018 +0000
Revision:
3:f707552ec50a
Parent:
2:3c406d25860e
Child:
4:951fabc31ba6
MPU6050????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jksoft 0:76dfa9657d9d 1 #include "mbed.h"
jksoft 0:76dfa9657d9d 2 #include "BLEDevice.h"
jksoft 0:76dfa9657d9d 3 #include "RCBController.h"
jksoft 0:76dfa9657d9d 4 #include "TB6612.h"
Dyotty 3:f707552ec50a 5 #include "MPU6050.h"
jksoft 0:76dfa9657d9d 6
jksoft 0:76dfa9657d9d 7 #define DBG 0
jksoft 0:76dfa9657d9d 8
jksoft 0:76dfa9657d9d 9 BLEDevice ble;
Dyotty 3:f707552ec50a 10
Dyotty 3:f707552ec50a 11 MPU6050 mpu(I2C_SDA, I2C_SCL);
Dyotty 3:f707552ec50a 12
sibu2 1:a86c84ead24a 13 #if DBG
jksoft 0:76dfa9657d9d 14 Serial pc(USBTX, USBRX);
sibu2 1:a86c84ead24a 15 #endif
jksoft 0:76dfa9657d9d 16 /* LEDs for indication: */
jksoft 0:76dfa9657d9d 17 DigitalOut ModeLed(P0_19);
jksoft 0:76dfa9657d9d 18 DigitalOut ConnectStateLed(P0_18);
jksoft 0:76dfa9657d9d 19 DigitalOut outlow(P0_20);
jksoft 0:76dfa9657d9d 20 //PwmOut ControllerStateLed(LED2);
jksoft 0:76dfa9657d9d 21
jksoft 0:76dfa9657d9d 22 AnalogIn fsen1(P0_2);
jksoft 0:76dfa9657d9d 23 AnalogIn fsen2(P0_3);
jksoft 0:76dfa9657d9d 24 AnalogIn fsen3(P0_4);
jksoft 0:76dfa9657d9d 25 AnalogIn fsen4(P0_5);
Dyotty 3:f707552ec50a 26 #if 1
jksoft 0:76dfa9657d9d 27 TB6612 left(P0_29,P0_23,P0_24);
sibu2 1:a86c84ead24a 28 TB6612 right(P0_28,P0_30,P0_0);
jksoft 0:76dfa9657d9d 29 #else
jksoft 0:76dfa9657d9d 30 TB6612 left(P0_29,P0_24,P0_23);
jksoft 0:76dfa9657d9d 31 TB6612 right(P0_28,P0_0,P0_30);
jksoft 0:76dfa9657d9d 32 #endif
jksoft 0:76dfa9657d9d 33 Ticker ticker;
jksoft 0:76dfa9657d9d 34
jksoft 0:76dfa9657d9d 35 DigitalIn sw1(P0_16);
jksoft 0:76dfa9657d9d 36 DigitalIn sw2(P0_17);
jksoft 0:76dfa9657d9d 37
sibu2 1:a86c84ead24a 38 DigitalIn encl1(P0_6);
sibu2 1:a86c84ead24a 39 DigitalIn encl2(P0_7);
sibu2 1:a86c84ead24a 40 DigitalIn encr1(P0_8);
sibu2 1:a86c84ead24a 41 DigitalIn encr2(P0_10);
jksoft 0:76dfa9657d9d 42
jksoft 0:76dfa9657d9d 43
jksoft 0:76dfa9657d9d 44 int base_fsen[4];
jksoft 0:76dfa9657d9d 45 int line_mode = 0;
jksoft 0:76dfa9657d9d 46 int challenge_mode = 0;
jksoft 0:76dfa9657d9d 47 char bValue = 0;
jksoft 0:76dfa9657d9d 48
jksoft 0:76dfa9657d9d 49 int get_line(int num);
jksoft 0:76dfa9657d9d 50
jksoft 0:76dfa9657d9d 51 /* RCBController Service */
jksoft 0:76dfa9657d9d 52 static const uint16_t RCBController_service_uuid = 0xFFF0;
jksoft 0:76dfa9657d9d 53 static const uint16_t RCBController_Characteristic_uuid = 0xFFF1;
jksoft 0:76dfa9657d9d 54 static const uint16_t RCBController_b_Characteristic_uuid = 0xFFF3;
jksoft 0:76dfa9657d9d 55 uint8_t RCBControllerPayload[10] = {0,};
jksoft 0:76dfa9657d9d 56
jksoft 0:76dfa9657d9d 57 GattCharacteristic ControllerChar (RCBController_Characteristic_uuid,RCBControllerPayload,10, 10,
jksoft 0:76dfa9657d9d 58 GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE |
jksoft 0:76dfa9657d9d 59 GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE_WITHOUT_RESPONSE);
sibu2 1:a86c84ead24a 60 //static uint8_t _bValue = 0x00;
jksoft 0:76dfa9657d9d 61 static uint8_t _mValue[10] = {0,};
jksoft 0:76dfa9657d9d 62 GattCharacteristic b_Char(RCBController_b_Characteristic_uuid, _mValue, sizeof(_mValue), sizeof(_mValue),
jksoft 0:76dfa9657d9d 63 GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ | GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY);
jksoft 0:76dfa9657d9d 64
jksoft 0:76dfa9657d9d 65 GattCharacteristic *ControllerChars[] = {&ControllerChar,&b_Char};
jksoft 0:76dfa9657d9d 66 GattService RCBControllerService(RCBController_service_uuid, ControllerChars, sizeof(ControllerChars) / sizeof(GattCharacteristic *));
jksoft 0:76dfa9657d9d 67
jksoft 0:76dfa9657d9d 68 RCBController controller;
jksoft 0:76dfa9657d9d 69
jksoft 0:76dfa9657d9d 70 void onConnected(Gap::Handle_t handle, const Gap::ConnectionParams_t *params)
jksoft 0:76dfa9657d9d 71 {
jksoft 0:76dfa9657d9d 72 ConnectStateLed = 0;
jksoft 0:76dfa9657d9d 73 #if DBG
jksoft 0:76dfa9657d9d 74 pc.printf("Connected\n\r");
jksoft 0:76dfa9657d9d 75 #endif
jksoft 0:76dfa9657d9d 76 }
jksoft 0:76dfa9657d9d 77
jksoft 0:76dfa9657d9d 78 void onDisconnected(Gap::Handle_t handle, Gap::DisconnectionReason_t reason)
jksoft 0:76dfa9657d9d 79 {
sibu2 1:a86c84ead24a 80 left = 0;
sibu2 1:a86c84ead24a 81 right = 0;
sibu2 1:a86c84ead24a 82
jksoft 0:76dfa9657d9d 83 ble.startAdvertising();
jksoft 0:76dfa9657d9d 84 ConnectStateLed = 1;
jksoft 0:76dfa9657d9d 85 #if DBG
jksoft 0:76dfa9657d9d 86 pc.printf("Disconnected\n\r");
jksoft 0:76dfa9657d9d 87 #endif
jksoft 0:76dfa9657d9d 88 }
jksoft 0:76dfa9657d9d 89
jksoft 0:76dfa9657d9d 90 void periodicCallback(void)
jksoft 0:76dfa9657d9d 91 {
jksoft 0:76dfa9657d9d 92 if (!ble.getGapState().connected) {
jksoft 0:76dfa9657d9d 93 return;
jksoft 0:76dfa9657d9d 94 }
jksoft 0:76dfa9657d9d 95 int line = get_line(0) ? 1 : 0;
jksoft 0:76dfa9657d9d 96 line |= get_line(1) ? 2 : 0;
jksoft 0:76dfa9657d9d 97 line |= get_line(2) ? 4 : 0;
jksoft 0:76dfa9657d9d 98 line |= get_line(3) ? 8 : 0;
jksoft 0:76dfa9657d9d 99 if( (bValue == 0)&&(line != 0) )
jksoft 0:76dfa9657d9d 100 {
jksoft 0:76dfa9657d9d 101 // game over
jksoft 0:76dfa9657d9d 102 left = 0.0;
jksoft 0:76dfa9657d9d 103 right = 0.0;
jksoft 0:76dfa9657d9d 104 bValue = 10;
jksoft 0:76dfa9657d9d 105 }
jksoft 0:76dfa9657d9d 106 if( bValue > 0 )
jksoft 0:76dfa9657d9d 107 {
jksoft 0:76dfa9657d9d 108 memcpy( _mValue , "GAME OVER",10);
jksoft 0:76dfa9657d9d 109 ble.updateCharacteristicValue(b_Char.getValueAttribute().getHandle(), (uint8_t *)_mValue, sizeof(_mValue));
jksoft 0:76dfa9657d9d 110 ModeLed = !ModeLed;
jksoft 0:76dfa9657d9d 111 bValue--;
jksoft 0:76dfa9657d9d 112 if( bValue == 0 )
jksoft 0:76dfa9657d9d 113 {
jksoft 0:76dfa9657d9d 114 ModeLed = 1;
jksoft 0:76dfa9657d9d 115 challenge_mode = 0;
jksoft 0:76dfa9657d9d 116 ticker.detach();
jksoft 0:76dfa9657d9d 117 }
jksoft 0:76dfa9657d9d 118 }
jksoft 0:76dfa9657d9d 119 }
jksoft 0:76dfa9657d9d 120
jksoft 0:76dfa9657d9d 121
jksoft 0:76dfa9657d9d 122 // GattEvent
jksoft 0:76dfa9657d9d 123 void onDataWritten(const GattCharacteristicWriteCBParams *params)
jksoft 0:76dfa9657d9d 124 {
jksoft 0:76dfa9657d9d 125 if( (params->charHandle == ControllerChar.getValueAttribute().getHandle()) && (line_mode == 0))
jksoft 0:76dfa9657d9d 126 {
jksoft 0:76dfa9657d9d 127 memcpy( &controller.data[0], params->data , params->len );
jksoft 0:76dfa9657d9d 128 //memcpy( &controller.data[0], RCBControllerPayload, sizeof(controller));
jksoft 0:76dfa9657d9d 129 #if DBG
jksoft 0:76dfa9657d9d 130
jksoft 0:76dfa9657d9d 131 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:76dfa9657d9d 132 controller.data[5],controller.data[6],controller.data[7],controller.data[8],controller.data[9]);
jksoft 0:76dfa9657d9d 133 #endif
jksoft 0:76dfa9657d9d 134 float right_factor;
jksoft 0:76dfa9657d9d 135 float left_factor;
jksoft 0:76dfa9657d9d 136
jksoft 0:76dfa9657d9d 137 left_factor = ((float)((int)controller.status.LeftAnalogUD -128) / 128.0);
jksoft 0:76dfa9657d9d 138 right_factor = ((float)((int)controller.status.RightAnalogUD -128) / 128.0);
jksoft 0:76dfa9657d9d 139
jksoft 0:76dfa9657d9d 140 if(challenge_mode == 1)
jksoft 0:76dfa9657d9d 141 {
jksoft 0:76dfa9657d9d 142 if( bValue == 0 )
jksoft 0:76dfa9657d9d 143 {
jksoft 0:76dfa9657d9d 144 float factor = ((float)((int)controller.status.AcceleX -128) / 128.0);
jksoft 0:76dfa9657d9d 145
jksoft 0:76dfa9657d9d 146 float right_factor = ((factor <= 0.0) ? 1.0 : 1.0 - (factor*2));
jksoft 0:76dfa9657d9d 147 float left_factor = ((factor >= 0.0) ? 1.0 : 1.0 - (-factor*2));
jksoft 0:76dfa9657d9d 148
jksoft 0:76dfa9657d9d 149 if( controller.status.B == 1 )
jksoft 0:76dfa9657d9d 150 {
jksoft 0:76dfa9657d9d 151 left = left_factor;
jksoft 0:76dfa9657d9d 152 right = right_factor;
jksoft 0:76dfa9657d9d 153 }
jksoft 0:76dfa9657d9d 154 else if( controller.status.A == 1 )
jksoft 0:76dfa9657d9d 155 {
jksoft 0:76dfa9657d9d 156 left = -right_factor;
jksoft 0:76dfa9657d9d 157 right = -left_factor;
jksoft 0:76dfa9657d9d 158 }
jksoft 0:76dfa9657d9d 159 else
jksoft 0:76dfa9657d9d 160 {
jksoft 0:76dfa9657d9d 161 left = 0;
jksoft 0:76dfa9657d9d 162 right = 0;
jksoft 0:76dfa9657d9d 163 }
jksoft 0:76dfa9657d9d 164 }
jksoft 0:76dfa9657d9d 165 }
jksoft 0:76dfa9657d9d 166 else if( (left_factor != 0.0)||(right_factor != 0.0) )
jksoft 0:76dfa9657d9d 167 {
jksoft 0:76dfa9657d9d 168 left = left_factor;
jksoft 0:76dfa9657d9d 169 right = right_factor;
jksoft 0:76dfa9657d9d 170 }
jksoft 0:76dfa9657d9d 171 else
jksoft 0:76dfa9657d9d 172 {
jksoft 0:76dfa9657d9d 173 float factor = ((float)((int)controller.status.AcceleX -128) / 128.0);
jksoft 0:76dfa9657d9d 174
jksoft 0:76dfa9657d9d 175 float right_factor = ((factor <= 0.0) ? 1.0 : 1.0 - (factor*2));
jksoft 0:76dfa9657d9d 176 float left_factor = ((factor >= 0.0) ? 1.0 : 1.0 - (-factor*2));
jksoft 0:76dfa9657d9d 177
jksoft 0:76dfa9657d9d 178 if( controller.status.B == 1 )
jksoft 0:76dfa9657d9d 179 {
jksoft 0:76dfa9657d9d 180 left = left_factor;
jksoft 0:76dfa9657d9d 181 right = right_factor;
jksoft 0:76dfa9657d9d 182 }
jksoft 0:76dfa9657d9d 183 else if( controller.status.A == 1 )
jksoft 0:76dfa9657d9d 184 {
jksoft 0:76dfa9657d9d 185 left = -right_factor;
jksoft 0:76dfa9657d9d 186 right = -left_factor;
jksoft 0:76dfa9657d9d 187 }
jksoft 0:76dfa9657d9d 188 else if( controller.status.UP == 1 )
jksoft 0:76dfa9657d9d 189 {
jksoft 0:76dfa9657d9d 190 left = 1.0;
jksoft 0:76dfa9657d9d 191 right = 1.0;
jksoft 0:76dfa9657d9d 192 }
jksoft 0:76dfa9657d9d 193 else if( controller.status.DOWN == 1 )
jksoft 0:76dfa9657d9d 194 {
jksoft 0:76dfa9657d9d 195 left = -1.0;
jksoft 0:76dfa9657d9d 196 right = -1.0;
jksoft 0:76dfa9657d9d 197 }
jksoft 0:76dfa9657d9d 198 else if( controller.status.RIGHT == 1 )
jksoft 0:76dfa9657d9d 199 {
jksoft 0:76dfa9657d9d 200 left = 1.0;
jksoft 0:76dfa9657d9d 201 right = -1.0;
jksoft 0:76dfa9657d9d 202 }
jksoft 0:76dfa9657d9d 203 else if( controller.status.LEFT == 1 )
jksoft 0:76dfa9657d9d 204 {
jksoft 0:76dfa9657d9d 205 left = -1.0;
jksoft 0:76dfa9657d9d 206 right = 1.0;
jksoft 0:76dfa9657d9d 207 }
jksoft 0:76dfa9657d9d 208 else
jksoft 0:76dfa9657d9d 209 {
jksoft 0:76dfa9657d9d 210 left = 0.0;
jksoft 0:76dfa9657d9d 211 right = 0.0;
jksoft 0:76dfa9657d9d 212 }
jksoft 0:76dfa9657d9d 213 if((controller.status.UP == 1)&&(controller.status.DOWN == 1))
jksoft 0:76dfa9657d9d 214 {
jksoft 0:76dfa9657d9d 215 left = 0.0;
jksoft 0:76dfa9657d9d 216 right = 0.0;
jksoft 0:76dfa9657d9d 217 ModeLed = 0;
jksoft 0:76dfa9657d9d 218 challenge_mode = 1;
jksoft 0:76dfa9657d9d 219 ticker.attach(periodicCallback, 0.1);
jksoft 0:76dfa9657d9d 220
jksoft 0:76dfa9657d9d 221 }
jksoft 0:76dfa9657d9d 222 }
jksoft 0:76dfa9657d9d 223 //ControllerStateLed = (float)controller.status.LeftAnalogLR / 255.0;
jksoft 0:76dfa9657d9d 224 }
jksoft 0:76dfa9657d9d 225 }
jksoft 0:76dfa9657d9d 226
jksoft 0:76dfa9657d9d 227 int get_fsen(int num)
jksoft 0:76dfa9657d9d 228 {
jksoft 0:76dfa9657d9d 229 switch(num)
jksoft 0:76dfa9657d9d 230 {
jksoft 0:76dfa9657d9d 231 case 0:
jksoft 0:76dfa9657d9d 232 return((int)fsen1.read_u16());
jksoft 0:76dfa9657d9d 233 case 1:
jksoft 0:76dfa9657d9d 234 return((int)fsen2.read_u16());
jksoft 0:76dfa9657d9d 235 case 2:
jksoft 0:76dfa9657d9d 236 return((int)fsen3.read_u16());
jksoft 0:76dfa9657d9d 237 case 3:
jksoft 0:76dfa9657d9d 238 return((int)fsen4.read_u16());
jksoft 0:76dfa9657d9d 239 }
jksoft 0:76dfa9657d9d 240 return(0);
jksoft 0:76dfa9657d9d 241 }
jksoft 0:76dfa9657d9d 242
jksoft 0:76dfa9657d9d 243 void base()
jksoft 0:76dfa9657d9d 244 {
jksoft 0:76dfa9657d9d 245 wait(0.5);
jksoft 0:76dfa9657d9d 246
jksoft 0:76dfa9657d9d 247 for(int i=0;i<4;i++)
jksoft 0:76dfa9657d9d 248 {
jksoft 0:76dfa9657d9d 249 base_fsen[i] = 0;
jksoft 0:76dfa9657d9d 250 }
jksoft 0:76dfa9657d9d 251
jksoft 0:76dfa9657d9d 252 for(int j=0;j<10;j++)
jksoft 0:76dfa9657d9d 253 {
jksoft 0:76dfa9657d9d 254 for(int i=0;i<4;i++)
jksoft 0:76dfa9657d9d 255 {
jksoft 0:76dfa9657d9d 256 base_fsen[i] += get_fsen(i);
jksoft 0:76dfa9657d9d 257 }
jksoft 0:76dfa9657d9d 258 wait_ms(50);
jksoft 0:76dfa9657d9d 259 }
jksoft 0:76dfa9657d9d 260 for(int i=0;i<4;i++)
jksoft 0:76dfa9657d9d 261 {
jksoft 0:76dfa9657d9d 262 base_fsen[i] = base_fsen[i] / 10;
jksoft 0:76dfa9657d9d 263 }
sibu2 1:a86c84ead24a 264 #if DBG
jksoft 0:76dfa9657d9d 265 pc.printf("[0]:%05d[1]:%05d[2]:%05d[3]:%05d\n\r",base_fsen[0],base_fsen[1],base_fsen[2],base_fsen[3]);
sibu2 1:a86c84ead24a 266 #endif
jksoft 0:76dfa9657d9d 267 }
jksoft 0:76dfa9657d9d 268
jksoft 0:76dfa9657d9d 269 int get_line(int num)
jksoft 0:76dfa9657d9d 270 {
jksoft 0:76dfa9657d9d 271 int in = get_fsen(num);
jksoft 0:76dfa9657d9d 272 int ret = 0;
jksoft 0:76dfa9657d9d 273
jksoft 0:76dfa9657d9d 274 #if 1
jksoft 0:76dfa9657d9d 275 if(in > 700)
jksoft 0:76dfa9657d9d 276 #else
jksoft 0:76dfa9657d9d 277 if( (in > (base_fsen[num] + 200))||(in < (base_fsen[num] - 200)))
jksoft 0:76dfa9657d9d 278 #endif
jksoft 0:76dfa9657d9d 279 {
jksoft 0:76dfa9657d9d 280 ret = 1;
jksoft 0:76dfa9657d9d 281 }
jksoft 0:76dfa9657d9d 282 return(ret);
jksoft 0:76dfa9657d9d 283 }
jksoft 0:76dfa9657d9d 284
jksoft 0:76dfa9657d9d 285 void line(void)
jksoft 0:76dfa9657d9d 286 {
jksoft 0:76dfa9657d9d 287 ModeLed = 0;
jksoft 0:76dfa9657d9d 288 wait(1);
jksoft 0:76dfa9657d9d 289 while(sw1 != 0)
jksoft 0:76dfa9657d9d 290 {
jksoft 0:76dfa9657d9d 291 #if 0
jksoft 0:76dfa9657d9d 292 int line = get_line(0) ? 0 : 1;
jksoft 0:76dfa9657d9d 293 line |= get_line(1) ? 0 : 2;
jksoft 0:76dfa9657d9d 294 line |= get_line(2) ? 0 : 4;
jksoft 0:76dfa9657d9d 295 line |= get_line(3) ? 0 : 8;
jksoft 0:76dfa9657d9d 296 #else
jksoft 0:76dfa9657d9d 297 int line = get_line(0) ? 1 : 0;
jksoft 0:76dfa9657d9d 298 line |= get_line(1) ? 2 : 0;
jksoft 0:76dfa9657d9d 299 line |= get_line(2) ? 4 : 0;
jksoft 0:76dfa9657d9d 300 line |= get_line(3) ? 8 : 0;
jksoft 0:76dfa9657d9d 301 #endif
sibu2 1:a86c84ead24a 302
sibu2 1:a86c84ead24a 303 #if DBG
sibu2 1:a86c84ead24a 304 pc.printf("line=%02x %04x %04x %04x %04x\n\r",line,base_fsen[0],base_fsen[1],base_fsen[2],base_fsen[3]);
sibu2 1:a86c84ead24a 305 #endif
sibu2 1:a86c84ead24a 306 #if 1
jksoft 0:76dfa9657d9d 307 switch(line)
jksoft 0:76dfa9657d9d 308 {
jksoft 0:76dfa9657d9d 309 case 1: // ○○○●
jksoft 0:76dfa9657d9d 310 left = 1.0;
jksoft 0:76dfa9657d9d 311 right = -1.0;
jksoft 0:76dfa9657d9d 312 break;
jksoft 0:76dfa9657d9d 313 case 3: // ○○●●
jksoft 0:76dfa9657d9d 314 left = 1.0;
jksoft 0:76dfa9657d9d 315 right = -0.5;
jksoft 0:76dfa9657d9d 316 break;
jksoft 0:76dfa9657d9d 317 case 2: // ○○●○
jksoft 0:76dfa9657d9d 318 left = 1.0;
jksoft 0:76dfa9657d9d 319 right = 0.5;
jksoft 0:76dfa9657d9d 320 break;
jksoft 0:76dfa9657d9d 321 case 6: // ○●●○
jksoft 0:76dfa9657d9d 322 left = 1.0;
jksoft 0:76dfa9657d9d 323 right = 1.0;
jksoft 0:76dfa9657d9d 324 break;
jksoft 0:76dfa9657d9d 325 case 4: // ○●○○
jksoft 0:76dfa9657d9d 326 left = 0.5;
jksoft 0:76dfa9657d9d 327 right = 1.0;
jksoft 0:76dfa9657d9d 328 break;
jksoft 0:76dfa9657d9d 329 case 12: // ●●○○
jksoft 0:76dfa9657d9d 330 left = -0.5;
jksoft 0:76dfa9657d9d 331 right = 1.0;
jksoft 0:76dfa9657d9d 332 break;
jksoft 0:76dfa9657d9d 333 case 8: // ●○○○
jksoft 0:76dfa9657d9d 334 left = -1.0;
jksoft 0:76dfa9657d9d 335 right = 1.0;
jksoft 0:76dfa9657d9d 336 break;
jksoft 0:76dfa9657d9d 337 default:
jksoft 0:76dfa9657d9d 338 left = 1.0;
jksoft 0:76dfa9657d9d 339 right = 1.0;
jksoft 0:76dfa9657d9d 340 break;
jksoft 0:76dfa9657d9d 341 }
sibu2 1:a86c84ead24a 342 #endif
jksoft 0:76dfa9657d9d 343 }
jksoft 0:76dfa9657d9d 344 ModeLed = 1;
jksoft 0:76dfa9657d9d 345 left = 0.0;
jksoft 0:76dfa9657d9d 346 right = 0.0;
jksoft 0:76dfa9657d9d 347 wait(1);
jksoft 0:76dfa9657d9d 348 }
sibu2 1:a86c84ead24a 349
sibu2 1:a86c84ead24a 350 #if 0
jksoft 0:76dfa9657d9d 351 int counter1 = 0;
jksoft 0:76dfa9657d9d 352 void p1_rise()
jksoft 0:76dfa9657d9d 353 {
jksoft 0:76dfa9657d9d 354 if( pin2 == 1 )
jksoft 0:76dfa9657d9d 355 {
jksoft 0:76dfa9657d9d 356 counter1++;
jksoft 0:76dfa9657d9d 357 }
jksoft 0:76dfa9657d9d 358 else
jksoft 0:76dfa9657d9d 359 {
jksoft 0:76dfa9657d9d 360 counter1--;
jksoft 0:76dfa9657d9d 361 }
jksoft 0:76dfa9657d9d 362 }
sibu2 1:a86c84ead24a 363 #endif
sibu2 1:a86c84ead24a 364
jksoft 0:76dfa9657d9d 365 /**************************************************************************/
jksoft 0:76dfa9657d9d 366 /*!
jksoft 0:76dfa9657d9d 367 @brief Program entry point
jksoft 0:76dfa9657d9d 368 */
jksoft 0:76dfa9657d9d 369 /**************************************************************************/
jksoft 0:76dfa9657d9d 370 int main(void)
jksoft 0:76dfa9657d9d 371 {
jksoft 0:76dfa9657d9d 372 sw1.mode(PullUp);
jksoft 0:76dfa9657d9d 373 sw2.mode(PullUp);
sibu2 1:a86c84ead24a 374
sibu2 1:a86c84ead24a 375 encl1.mode(PullNone);
sibu2 1:a86c84ead24a 376 encl2.mode(PullNone);
sibu2 1:a86c84ead24a 377 encr1.mode(PullNone);
sibu2 1:a86c84ead24a 378 encr2.mode(PullNone);
sibu2 1:a86c84ead24a 379
jksoft 0:76dfa9657d9d 380 ModeLed = 1;
jksoft 0:76dfa9657d9d 381 ConnectStateLed = 1;
jksoft 0:76dfa9657d9d 382 #if DBG
jksoft 0:76dfa9657d9d 383 //pc.baud(921600);
jksoft 0:76dfa9657d9d 384 pc.baud(9600);
jksoft 0:76dfa9657d9d 385 pc.printf("Start\n\r");
jksoft 0:76dfa9657d9d 386 #endif
jksoft 0:76dfa9657d9d 387 outlow = 0;
jksoft 0:76dfa9657d9d 388
jksoft 0:76dfa9657d9d 389 if(sw2 == 0)
jksoft 0:76dfa9657d9d 390 {
sibu2 1:a86c84ead24a 391 // pin1.mode(PullDown);
jksoft 0:76dfa9657d9d 392 // pin1.rise(&p1_rise);
jksoft 0:76dfa9657d9d 393 while(1)
jksoft 0:76dfa9657d9d 394 {
jksoft 0:76dfa9657d9d 395 //int in1 = pin1;
jksoft 0:76dfa9657d9d 396 //int in2 = pin2;
jksoft 0:76dfa9657d9d 397 //ModeLed = pin1;
jksoft 0:76dfa9657d9d 398 //pc.printf("dat = %d %d\r\n",in1,in2);
jksoft 0:76dfa9657d9d 399 base();
jksoft 0:76dfa9657d9d 400 #if 0
jksoft 0:76dfa9657d9d 401 left = 1.0;
jksoft 0:76dfa9657d9d 402 right = 1.0;
jksoft 0:76dfa9657d9d 403 wait(5);
jksoft 0:76dfa9657d9d 404 left = -1.0;
jksoft 0:76dfa9657d9d 405 right = -1.0;
jksoft 0:76dfa9657d9d 406 wait(5);
jksoft 0:76dfa9657d9d 407 #endif
jksoft 0:76dfa9657d9d 408 }
jksoft 0:76dfa9657d9d 409 }
jksoft 0:76dfa9657d9d 410
Dyotty 3:f707552ec50a 411 // MPU6050 Initialize
Dyotty 3:f707552ec50a 412 mpu.initialize();
Dyotty 3:f707552ec50a 413 mpu.setAcceleroRange(MPU6050_ACCELERO_RANGE_8G);
Dyotty 3:f707552ec50a 414 mpu.setGyroRange(MPU6050_GYRO_RANGE_1000);
Dyotty 3:f707552ec50a 415
jksoft 0:76dfa9657d9d 416 ble.init();
jksoft 0:76dfa9657d9d 417 ble.onConnection(onConnected);
jksoft 0:76dfa9657d9d 418 ble.onDisconnection(onDisconnected);
jksoft 0:76dfa9657d9d 419 ble.onDataWritten(onDataWritten);
jksoft 0:76dfa9657d9d 420
jksoft 0:76dfa9657d9d 421 /* setup advertising */
jksoft 0:76dfa9657d9d 422 ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
jksoft 0:76dfa9657d9d 423 ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
jksoft 0:76dfa9657d9d 424 ble.accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME,
jksoft 0:76dfa9657d9d 425 (const uint8_t *)"mbed WallbotBLE", sizeof("mbed WallbotBLE") - 1);
jksoft 0:76dfa9657d9d 426 ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS,
jksoft 0:76dfa9657d9d 427 (const uint8_t *)RCBController_service_uuid, sizeof(RCBController_service_uuid));
jksoft 0:76dfa9657d9d 428
jksoft 0:76dfa9657d9d 429 ble.setAdvertisingInterval(160); /* 100ms; in multiples of 0.625ms. */
jksoft 0:76dfa9657d9d 430 ble.startAdvertising();
jksoft 0:76dfa9657d9d 431
jksoft 0:76dfa9657d9d 432 ble.addService(RCBControllerService);
jksoft 0:76dfa9657d9d 433
jksoft 0:76dfa9657d9d 434
jksoft 0:76dfa9657d9d 435 while (true) {
jksoft 0:76dfa9657d9d 436 if(sw1 == 0)
jksoft 0:76dfa9657d9d 437 {
jksoft 0:76dfa9657d9d 438 bValue = 1;
jksoft 0:76dfa9657d9d 439 line_mode = 1;
jksoft 0:76dfa9657d9d 440 line();
jksoft 0:76dfa9657d9d 441 line_mode = 0;
jksoft 0:76dfa9657d9d 442 bValue = 0;
jksoft 0:76dfa9657d9d 443 }
jksoft 0:76dfa9657d9d 444 ble.waitForEvent();
jksoft 0:76dfa9657d9d 445 }
jksoft 0:76dfa9657d9d 446 }
jksoft 0:76dfa9657d9d 447