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@1:a86c84ead24a, 2014-11-18 (annotated)

Committer:
sibu2
Date:
Tue Nov 18 04:28:02 2014 +0000
Revision:
1:a86c84ead24a
Parent:
0:76dfa9657d9d
Child:
2:3c406d25860e
adjust wheel rotate direction, wheel encoder signals

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