Maiko Matsumoto / Mbed 2 deprecated BLE_WallbotBLE_Challenge2_2

0611

Dependencies:   mbed

Fork of BLE_WallbotBLE_Challenge2 by Maiko Matsumoto

main.cpp@4:53eceb750885, 2018-06-11 (annotated)

Committer:
mmmmmmmmma
Date:
Mon Jun 11 04:55:58 2018 +0000
Revision:
4:53eceb750885
Parent:
3:f707552ec50a 
0611

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);
mmmmmmmmma 4:53eceb750885 12 Serial pc(USBTX, USBRX);
Dyotty 3:f707552ec50a 13
jksoft 0:76dfa9657d9d 14 /* LEDs for indication: */
jksoft 0:76dfa9657d9d 15 DigitalOut ModeLed(P0_19);
jksoft 0:76dfa9657d9d 16 DigitalOut ConnectStateLed(P0_18);
jksoft 0:76dfa9657d9d 17 DigitalOut outlow(P0_20);
jksoft 0:76dfa9657d9d 18 //PwmOut ControllerStateLed(LED2);
jksoft 0:76dfa9657d9d 19
jksoft 0:76dfa9657d9d 20 AnalogIn fsen1(P0_2);
jksoft 0:76dfa9657d9d 21 AnalogIn fsen2(P0_3);
jksoft 0:76dfa9657d9d 22 AnalogIn fsen3(P0_4);
jksoft 0:76dfa9657d9d 23 AnalogIn fsen4(P0_5);
Dyotty 3:f707552ec50a 24 #if 1
jksoft 0:76dfa9657d9d 25 TB6612 left(P0_29,P0_23,P0_24);
sibu2 1:a86c84ead24a 26 TB6612 right(P0_28,P0_30,P0_0);
jksoft 0:76dfa9657d9d 27 #else
jksoft 0:76dfa9657d9d 28 TB6612 left(P0_29,P0_24,P0_23);
jksoft 0:76dfa9657d9d 29 TB6612 right(P0_28,P0_0,P0_30);
jksoft 0:76dfa9657d9d 30 #endif
jksoft 0:76dfa9657d9d 31 Ticker ticker;
jksoft 0:76dfa9657d9d 32
jksoft 0:76dfa9657d9d 33 DigitalIn sw1(P0_16);
jksoft 0:76dfa9657d9d 34 DigitalIn sw2(P0_17);
jksoft 0:76dfa9657d9d 35
sibu2 1:a86c84ead24a 36 DigitalIn encl1(P0_6);
sibu2 1:a86c84ead24a 37 DigitalIn encl2(P0_7);
sibu2 1:a86c84ead24a 38 DigitalIn encr1(P0_8);
sibu2 1:a86c84ead24a 39 DigitalIn encr2(P0_10);
jksoft 0:76dfa9657d9d 40
jksoft 0:76dfa9657d9d 41
jksoft 0:76dfa9657d9d 42 int base_fsen[4];
jksoft 0:76dfa9657d9d 43 int line_mode = 0;
jksoft 0:76dfa9657d9d 44 int challenge_mode = 0;
jksoft 0:76dfa9657d9d 45 char bValue = 0;
jksoft 0:76dfa9657d9d 46
jksoft 0:76dfa9657d9d 47 int get_line(int num);
jksoft 0:76dfa9657d9d 48
jksoft 0:76dfa9657d9d 49 /* RCBController Service */
jksoft 0:76dfa9657d9d 50 static const uint16_t RCBController_service_uuid = 0xFFF0;
jksoft 0:76dfa9657d9d 51 static const uint16_t RCBController_Characteristic_uuid = 0xFFF1;
jksoft 0:76dfa9657d9d 52 static const uint16_t RCBController_b_Characteristic_uuid = 0xFFF3;
jksoft 0:76dfa9657d9d 53 uint8_t RCBControllerPayload[10] = {0,};
jksoft 0:76dfa9657d9d 54
jksoft 0:76dfa9657d9d 55 GattCharacteristic ControllerChar (RCBController_Characteristic_uuid,RCBControllerPayload,10, 10,
jksoft 0:76dfa9657d9d 56 GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE |
jksoft 0:76dfa9657d9d 57 GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE_WITHOUT_RESPONSE);
sibu2 1:a86c84ead24a 58 //static uint8_t _bValue = 0x00;
jksoft 0:76dfa9657d9d 59 static uint8_t _mValue[10] = {0,};
jksoft 0:76dfa9657d9d 60 GattCharacteristic b_Char(RCBController_b_Characteristic_uuid, _mValue, sizeof(_mValue), sizeof(_mValue),
jksoft 0:76dfa9657d9d 61 GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ | GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY);
jksoft 0:76dfa9657d9d 62
jksoft 0:76dfa9657d9d 63 GattCharacteristic *ControllerChars[] = {&ControllerChar,&b_Char};
jksoft 0:76dfa9657d9d 64 GattService RCBControllerService(RCBController_service_uuid, ControllerChars, sizeof(ControllerChars) / sizeof(GattCharacteristic *));
jksoft 0:76dfa9657d9d 65
jksoft 0:76dfa9657d9d 66 RCBController controller;
jksoft 0:76dfa9657d9d 67
jksoft 0:76dfa9657d9d 68 void onConnected(Gap::Handle_t handle, const Gap::ConnectionParams_t *params)
jksoft 0:76dfa9657d9d 69 {
jksoft 0:76dfa9657d9d 70 ConnectStateLed = 0;
jksoft 0:76dfa9657d9d 71 #if DBG
jksoft 0:76dfa9657d9d 72 pc.printf("Connected\n\r");
jksoft 0:76dfa9657d9d 73 #endif
jksoft 0:76dfa9657d9d 74 }
jksoft 0:76dfa9657d9d 75
jksoft 0:76dfa9657d9d 76 void onDisconnected(Gap::Handle_t handle, Gap::DisconnectionReason_t reason)
jksoft 0:76dfa9657d9d 77 {
sibu2 1:a86c84ead24a 78 left = 0;
sibu2 1:a86c84ead24a 79 right = 0;
sibu2 1:a86c84ead24a 80
jksoft 0:76dfa9657d9d 81 ble.startAdvertising();
jksoft 0:76dfa9657d9d 82 ConnectStateLed = 1;
jksoft 0:76dfa9657d9d 83 #if DBG
jksoft 0:76dfa9657d9d 84 pc.printf("Disconnected\n\r");
jksoft 0:76dfa9657d9d 85 #endif
jksoft 0:76dfa9657d9d 86 }
jksoft 0:76dfa9657d9d 87
jksoft 0:76dfa9657d9d 88 void periodicCallback(void)
jksoft 0:76dfa9657d9d 89 {
jksoft 0:76dfa9657d9d 90 if (!ble.getGapState().connected) {
jksoft 0:76dfa9657d9d 91 return;
jksoft 0:76dfa9657d9d 92 }
jksoft 0:76dfa9657d9d 93 int line = get_line(0) ? 1 : 0;
jksoft 0:76dfa9657d9d 94 line |= get_line(1) ? 2 : 0;
jksoft 0:76dfa9657d9d 95 line |= get_line(2) ? 4 : 0;
jksoft 0:76dfa9657d9d 96 line |= get_line(3) ? 8 : 0;
jksoft 0:76dfa9657d9d 97 if( (bValue == 0)&&(line != 0) )
jksoft 0:76dfa9657d9d 98 {
jksoft 0:76dfa9657d9d 99 // game over
jksoft 0:76dfa9657d9d 100 left = 0.0;
jksoft 0:76dfa9657d9d 101 right = 0.0;
jksoft 0:76dfa9657d9d 102 bValue = 10;
jksoft 0:76dfa9657d9d 103 }
jksoft 0:76dfa9657d9d 104 if( bValue > 0 )
jksoft 0:76dfa9657d9d 105 {
jksoft 0:76dfa9657d9d 106 memcpy( _mValue , "GAME OVER",10);
jksoft 0:76dfa9657d9d 107 ble.updateCharacteristicValue(b_Char.getValueAttribute().getHandle(), (uint8_t *)_mValue, sizeof(_mValue));
jksoft 0:76dfa9657d9d 108 ModeLed = !ModeLed;
jksoft 0:76dfa9657d9d 109 bValue--;
jksoft 0:76dfa9657d9d 110 if( bValue == 0 )
jksoft 0:76dfa9657d9d 111 {
jksoft 0:76dfa9657d9d 112 ModeLed = 1;
jksoft 0:76dfa9657d9d 113 challenge_mode = 0;
jksoft 0:76dfa9657d9d 114 ticker.detach();
jksoft 0:76dfa9657d9d 115 }
jksoft 0:76dfa9657d9d 116 }
jksoft 0:76dfa9657d9d 117 }
jksoft 0:76dfa9657d9d 118
jksoft 0:76dfa9657d9d 119
jksoft 0:76dfa9657d9d 120 // GattEvent
jksoft 0:76dfa9657d9d 121 void onDataWritten(const GattCharacteristicWriteCBParams *params)
jksoft 0:76dfa9657d9d 122 {
jksoft 0:76dfa9657d9d 123 if( (params->charHandle == ControllerChar.getValueAttribute().getHandle()) && (line_mode == 0))
jksoft 0:76dfa9657d9d 124 {
jksoft 0:76dfa9657d9d 125 memcpy( &controller.data[0], params->data , params->len );
jksoft 0:76dfa9657d9d 126 //memcpy( &controller.data[0], RCBControllerPayload, sizeof(controller));
jksoft 0:76dfa9657d9d 127 #if DBG
jksoft 0:76dfa9657d9d 128
jksoft 0:76dfa9657d9d 129 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 130 controller.data[5],controller.data[6],controller.data[7],controller.data[8],controller.data[9]);
jksoft 0:76dfa9657d9d 131 #endif
jksoft 0:76dfa9657d9d 132 float right_factor;
jksoft 0:76dfa9657d9d 133 float left_factor;
jksoft 0:76dfa9657d9d 134
jksoft 0:76dfa9657d9d 135 left_factor = ((float)((int)controller.status.LeftAnalogUD -128) / 128.0);
jksoft 0:76dfa9657d9d 136 right_factor = ((float)((int)controller.status.RightAnalogUD -128) / 128.0);
jksoft 0:76dfa9657d9d 137
jksoft 0:76dfa9657d9d 138 if(challenge_mode == 1)
jksoft 0:76dfa9657d9d 139 {
jksoft 0:76dfa9657d9d 140 if( bValue == 0 )
jksoft 0:76dfa9657d9d 141 {
jksoft 0:76dfa9657d9d 142 float factor = ((float)((int)controller.status.AcceleX -128) / 128.0);
jksoft 0:76dfa9657d9d 143
jksoft 0:76dfa9657d9d 144 float right_factor = ((factor <= 0.0) ? 1.0 : 1.0 - (factor*2));
jksoft 0:76dfa9657d9d 145 float left_factor = ((factor >= 0.0) ? 1.0 : 1.0 - (-factor*2));
jksoft 0:76dfa9657d9d 146
jksoft 0:76dfa9657d9d 147 if( controller.status.B == 1 )
jksoft 0:76dfa9657d9d 148 {
jksoft 0:76dfa9657d9d 149 left = left_factor;
jksoft 0:76dfa9657d9d 150 right = right_factor;
jksoft 0:76dfa9657d9d 151 }
jksoft 0:76dfa9657d9d 152 else if( controller.status.A == 1 )
jksoft 0:76dfa9657d9d 153 {
jksoft 0:76dfa9657d9d 154 left = -right_factor;
jksoft 0:76dfa9657d9d 155 right = -left_factor;
jksoft 0:76dfa9657d9d 156 }
jksoft 0:76dfa9657d9d 157 else
jksoft 0:76dfa9657d9d 158 {
jksoft 0:76dfa9657d9d 159 left = 0;
jksoft 0:76dfa9657d9d 160 right = 0;
jksoft 0:76dfa9657d9d 161 }
jksoft 0:76dfa9657d9d 162 }
jksoft 0:76dfa9657d9d 163 }
jksoft 0:76dfa9657d9d 164 else if( (left_factor != 0.0)||(right_factor != 0.0) )
jksoft 0:76dfa9657d9d 165 {
jksoft 0:76dfa9657d9d 166 left = left_factor;
jksoft 0:76dfa9657d9d 167 right = right_factor;
jksoft 0:76dfa9657d9d 168 }
jksoft 0:76dfa9657d9d 169 else
jksoft 0:76dfa9657d9d 170 {
jksoft 0:76dfa9657d9d 171 float factor = ((float)((int)controller.status.AcceleX -128) / 128.0);
jksoft 0:76dfa9657d9d 172
jksoft 0:76dfa9657d9d 173 float right_factor = ((factor <= 0.0) ? 1.0 : 1.0 - (factor*2));
jksoft 0:76dfa9657d9d 174 float left_factor = ((factor >= 0.0) ? 1.0 : 1.0 - (-factor*2));
jksoft 0:76dfa9657d9d 175
jksoft 0:76dfa9657d9d 176 if( controller.status.B == 1 )
jksoft 0:76dfa9657d9d 177 {
jksoft 0:76dfa9657d9d 178 left = left_factor;
jksoft 0:76dfa9657d9d 179 right = right_factor;
jksoft 0:76dfa9657d9d 180 }
jksoft 0:76dfa9657d9d 181 else if( controller.status.A == 1 )
jksoft 0:76dfa9657d9d 182 {
jksoft 0:76dfa9657d9d 183 left = -right_factor;
jksoft 0:76dfa9657d9d 184 right = -left_factor;
jksoft 0:76dfa9657d9d 185 }
jksoft 0:76dfa9657d9d 186 else if( controller.status.UP == 1 )
jksoft 0:76dfa9657d9d 187 {
jksoft 0:76dfa9657d9d 188 left = 1.0;
jksoft 0:76dfa9657d9d 189 right = 1.0;
jksoft 0:76dfa9657d9d 190 }
jksoft 0:76dfa9657d9d 191 else if( controller.status.DOWN == 1 )
jksoft 0:76dfa9657d9d 192 {
jksoft 0:76dfa9657d9d 193 left = -1.0;
jksoft 0:76dfa9657d9d 194 right = -1.0;
jksoft 0:76dfa9657d9d 195 }
jksoft 0:76dfa9657d9d 196 else if( controller.status.RIGHT == 1 )
jksoft 0:76dfa9657d9d 197 {
jksoft 0:76dfa9657d9d 198 left = 1.0;
jksoft 0:76dfa9657d9d 199 right = -1.0;
jksoft 0:76dfa9657d9d 200 }
jksoft 0:76dfa9657d9d 201 else if( controller.status.LEFT == 1 )
jksoft 0:76dfa9657d9d 202 {
jksoft 0:76dfa9657d9d 203 left = -1.0;
jksoft 0:76dfa9657d9d 204 right = 1.0;
jksoft 0:76dfa9657d9d 205 }
jksoft 0:76dfa9657d9d 206 else
jksoft 0:76dfa9657d9d 207 {
jksoft 0:76dfa9657d9d 208 left = 0.0;
jksoft 0:76dfa9657d9d 209 right = 0.0;
jksoft 0:76dfa9657d9d 210 }
jksoft 0:76dfa9657d9d 211 if((controller.status.UP == 1)&&(controller.status.DOWN == 1))
jksoft 0:76dfa9657d9d 212 {
jksoft 0:76dfa9657d9d 213 left = 0.0;
jksoft 0:76dfa9657d9d 214 right = 0.0;
jksoft 0:76dfa9657d9d 215 ModeLed = 0;
jksoft 0:76dfa9657d9d 216 challenge_mode = 1;
jksoft 0:76dfa9657d9d 217 ticker.attach(periodicCallback, 0.1);
jksoft 0:76dfa9657d9d 218
jksoft 0:76dfa9657d9d 219 }
jksoft 0:76dfa9657d9d 220 }
jksoft 0:76dfa9657d9d 221 //ControllerStateLed = (float)controller.status.LeftAnalogLR / 255.0;
jksoft 0:76dfa9657d9d 222 }
jksoft 0:76dfa9657d9d 223 }
jksoft 0:76dfa9657d9d 224
jksoft 0:76dfa9657d9d 225 int get_fsen(int num)
jksoft 0:76dfa9657d9d 226 {
jksoft 0:76dfa9657d9d 227 switch(num)
jksoft 0:76dfa9657d9d 228 {
jksoft 0:76dfa9657d9d 229 case 0:
jksoft 0:76dfa9657d9d 230 return((int)fsen1.read_u16());
jksoft 0:76dfa9657d9d 231 case 1:
jksoft 0:76dfa9657d9d 232 return((int)fsen2.read_u16());
jksoft 0:76dfa9657d9d 233 case 2:
jksoft 0:76dfa9657d9d 234 return((int)fsen3.read_u16());
jksoft 0:76dfa9657d9d 235 case 3:
jksoft 0:76dfa9657d9d 236 return((int)fsen4.read_u16());
jksoft 0:76dfa9657d9d 237 }
jksoft 0:76dfa9657d9d 238 return(0);
jksoft 0:76dfa9657d9d 239 }
jksoft 0:76dfa9657d9d 240
jksoft 0:76dfa9657d9d 241 void base()
jksoft 0:76dfa9657d9d 242 {
jksoft 0:76dfa9657d9d 243 wait(0.5);
jksoft 0:76dfa9657d9d 244
jksoft 0:76dfa9657d9d 245 for(int i=0;i<4;i++)
jksoft 0:76dfa9657d9d 246 {
jksoft 0:76dfa9657d9d 247 base_fsen[i] = 0;
jksoft 0:76dfa9657d9d 248 }
jksoft 0:76dfa9657d9d 249
jksoft 0:76dfa9657d9d 250 for(int j=0;j<10;j++)
jksoft 0:76dfa9657d9d 251 {
jksoft 0:76dfa9657d9d 252 for(int i=0;i<4;i++)
jksoft 0:76dfa9657d9d 253 {
jksoft 0:76dfa9657d9d 254 base_fsen[i] += get_fsen(i);
jksoft 0:76dfa9657d9d 255 }
jksoft 0:76dfa9657d9d 256 wait_ms(50);
jksoft 0:76dfa9657d9d 257 }
jksoft 0:76dfa9657d9d 258 for(int i=0;i<4;i++)
jksoft 0:76dfa9657d9d 259 {
jksoft 0:76dfa9657d9d 260 base_fsen[i] = base_fsen[i] / 10;
jksoft 0:76dfa9657d9d 261 }
sibu2 1:a86c84ead24a 262 #if DBG
jksoft 0:76dfa9657d9d 263 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 264 #endif
jksoft 0:76dfa9657d9d 265 }
jksoft 0:76dfa9657d9d 266
jksoft 0:76dfa9657d9d 267 int get_line(int num)
jksoft 0:76dfa9657d9d 268 {
jksoft 0:76dfa9657d9d 269 int in = get_fsen(num);
jksoft 0:76dfa9657d9d 270 int ret = 0;
jksoft 0:76dfa9657d9d 271
jksoft 0:76dfa9657d9d 272 #if 1
jksoft 0:76dfa9657d9d 273 if(in > 700)
jksoft 0:76dfa9657d9d 274 #else
jksoft 0:76dfa9657d9d 275 if( (in > (base_fsen[num] + 200))||(in < (base_fsen[num] - 200)))
jksoft 0:76dfa9657d9d 276 #endif
jksoft 0:76dfa9657d9d 277 {
jksoft 0:76dfa9657d9d 278 ret = 1;
jksoft 0:76dfa9657d9d 279 }
jksoft 0:76dfa9657d9d 280 return(ret);
jksoft 0:76dfa9657d9d 281 }
jksoft 0:76dfa9657d9d 282
jksoft 0:76dfa9657d9d 283 void line(void)
jksoft 0:76dfa9657d9d 284 {
jksoft 0:76dfa9657d9d 285 ModeLed = 0;
jksoft 0:76dfa9657d9d 286 wait(1);
jksoft 0:76dfa9657d9d 287 while(sw1 != 0)
jksoft 0:76dfa9657d9d 288 {
jksoft 0:76dfa9657d9d 289 #if 0
jksoft 0:76dfa9657d9d 290 int line = get_line(0) ? 0 : 1;
jksoft 0:76dfa9657d9d 291 line |= get_line(1) ? 0 : 2;
jksoft 0:76dfa9657d9d 292 line |= get_line(2) ? 0 : 4;
jksoft 0:76dfa9657d9d 293 line |= get_line(3) ? 0 : 8;
jksoft 0:76dfa9657d9d 294 #else
jksoft 0:76dfa9657d9d 295 int line = get_line(0) ? 1 : 0;
jksoft 0:76dfa9657d9d 296 line |= get_line(1) ? 2 : 0;
jksoft 0:76dfa9657d9d 297 line |= get_line(2) ? 4 : 0;
jksoft 0:76dfa9657d9d 298 line |= get_line(3) ? 8 : 0;
jksoft 0:76dfa9657d9d 299 #endif
sibu2 1:a86c84ead24a 300
sibu2 1:a86c84ead24a 301 #if DBG
sibu2 1:a86c84ead24a 302 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 303 #endif
sibu2 1:a86c84ead24a 304 #if 1
jksoft 0:76dfa9657d9d 305 switch(line)
jksoft 0:76dfa9657d9d 306 {
jksoft 0:76dfa9657d9d 307 case 1: // ○○○●
jksoft 0:76dfa9657d9d 308 left = 1.0;
jksoft 0:76dfa9657d9d 309 right = -1.0;
jksoft 0:76dfa9657d9d 310 break;
jksoft 0:76dfa9657d9d 311 case 3: // ○○●●
jksoft 0:76dfa9657d9d 312 left = 1.0;
jksoft 0:76dfa9657d9d 313 right = -0.5;
jksoft 0:76dfa9657d9d 314 break;
jksoft 0:76dfa9657d9d 315 case 2: // ○○●○
jksoft 0:76dfa9657d9d 316 left = 1.0;
jksoft 0:76dfa9657d9d 317 right = 0.5;
jksoft 0:76dfa9657d9d 318 break;
jksoft 0:76dfa9657d9d 319 case 6: // ○●●○
jksoft 0:76dfa9657d9d 320 left = 1.0;
jksoft 0:76dfa9657d9d 321 right = 1.0;
jksoft 0:76dfa9657d9d 322 break;
jksoft 0:76dfa9657d9d 323 case 4: // ○●○○
jksoft 0:76dfa9657d9d 324 left = 0.5;
jksoft 0:76dfa9657d9d 325 right = 1.0;
jksoft 0:76dfa9657d9d 326 break;
jksoft 0:76dfa9657d9d 327 case 12: // ●●○○
jksoft 0:76dfa9657d9d 328 left = -0.5;
jksoft 0:76dfa9657d9d 329 right = 1.0;
jksoft 0:76dfa9657d9d 330 break;
jksoft 0:76dfa9657d9d 331 case 8: // ●○○○
jksoft 0:76dfa9657d9d 332 left = -1.0;
jksoft 0:76dfa9657d9d 333 right = 1.0;
jksoft 0:76dfa9657d9d 334 break;
jksoft 0:76dfa9657d9d 335 default:
jksoft 0:76dfa9657d9d 336 left = 1.0;
jksoft 0:76dfa9657d9d 337 right = 1.0;
jksoft 0:76dfa9657d9d 338 break;
jksoft 0:76dfa9657d9d 339 }
sibu2 1:a86c84ead24a 340 #endif
jksoft 0:76dfa9657d9d 341 }
jksoft 0:76dfa9657d9d 342 ModeLed = 1;
jksoft 0:76dfa9657d9d 343 left = 0.0;
jksoft 0:76dfa9657d9d 344 right = 0.0;
jksoft 0:76dfa9657d9d 345 wait(1);
jksoft 0:76dfa9657d9d 346 }
sibu2 1:a86c84ead24a 347
sibu2 1:a86c84ead24a 348 #if 0
jksoft 0:76dfa9657d9d 349 int counter1 = 0;
jksoft 0:76dfa9657d9d 350 void p1_rise()
jksoft 0:76dfa9657d9d 351 {
jksoft 0:76dfa9657d9d 352 if( pin2 == 1 )
jksoft 0:76dfa9657d9d 353 {
jksoft 0:76dfa9657d9d 354 counter1++;
jksoft 0:76dfa9657d9d 355 }
jksoft 0:76dfa9657d9d 356 else
jksoft 0:76dfa9657d9d 357 {
jksoft 0:76dfa9657d9d 358 counter1--;
jksoft 0:76dfa9657d9d 359 }
jksoft 0:76dfa9657d9d 360 }
sibu2 1:a86c84ead24a 361 #endif
sibu2 1:a86c84ead24a 362
jksoft 0:76dfa9657d9d 363 /**************************************************************************/
jksoft 0:76dfa9657d9d 364 /*!
jksoft 0:76dfa9657d9d 365 @brief Program entry point
jksoft 0:76dfa9657d9d 366 */
mmmmmmmmma 4:53eceb750885 367
mmmmmmmmma 4:53eceb750885 368 //#define CONN_INTERVAL 25 /**< connection interval 250ms; in multiples of 0.125ms. (durationInMillis * 1000) / UNIT_0_625_MS; */
mmmmmmmmma 4:53eceb750885 369 //#define CONN_SUP_TIMEOUT 8000 /**< Connection supervisory timeout (6 seconds); in multiples of 0.125ms. */
mmmmmmmmma 4:53eceb750885 370 //#define SLAVE_LATENCY 0
mmmmmmmmma 4:53eceb750885 371 #define TICKER_INTERVAL 2.0f
mmmmmmmmma 4:53eceb750885 372
mmmmmmmmma 4:53eceb750885 373 // ・MPU6050から値を取得
mmmmmmmmma 4:53eceb750885 374 // ・ループ実行されている関数
mmmmmmmmma 4:53eceb750885 375 void updateValue(void){
mmmmmmmmma 4:53eceb750885 376 // 値を格納する用の配列、変数
mmmmmmmmma 4:53eceb750885 377 float acData[3];
mmmmmmmmma 4:53eceb750885 378 float gyData[3];
mmmmmmmmma 4:53eceb750885 379
mmmmmmmmma 4:53eceb750885 380 //加速度を取得
mmmmmmmmma 4:53eceb750885 381 Timer acTimer;
mmmmmmmmma 4:53eceb750885 382 acTimer.start();
mmmmmmmmma 4:53eceb750885 383 mpu.getAccelero(acData); //加速度を取得 acDataに格納
mmmmmmmmma 4:53eceb750885 384 acTimer.stop();
mmmmmmmmma 4:53eceb750885 385 // at = acTimer.read_ms();
mmmmmmmmma 4:53eceb750885 386 acTimer.reset();
mmmmmmmmma 4:53eceb750885 387
mmmmmmmmma 4:53eceb750885 388 //ジャイロを取得
mmmmmmmmma 4:53eceb750885 389 Timer gyTimer;
mmmmmmmmma 4:53eceb750885 390 gyTimer.start();
mmmmmmmmma 4:53eceb750885 391 mpu.getGyro(gyData); //ジャイロの値（角加速度）を取得 gyDataに格納
mmmmmmmmma 4:53eceb750885 392 gyTimer.stop();
mmmmmmmmma 4:53eceb750885 393 // gt = gyTimer.read_ms();
mmmmmmmmma 4:53eceb750885 394 gyTimer.reset();
mmmmmmmmma 4:53eceb750885 395
mmmmmmmmma 4:53eceb750885 396 //floatの値を合計5桁、小数点以下3桁の形でPCへ送信
mmmmmmmmma 4:53eceb750885 397 pc.printf("%5.3f:%5.3f:%5.3f:%5.3f:%5.3f:%5.3f \n", acData[0], acData[1], acData[2],gyData[0],gyData[1],gyData[2]);
mmmmmmmmma 4:53eceb750885 398 }
mmmmmmmmma 4:53eceb750885 399
mmmmmmmmma 4:53eceb750885 400 int test()
mmmmmmmmma 4:53eceb750885 401 {
mmmmmmmmma 4:53eceb750885 402
mmmmmmmmma 4:53eceb750885 403 ///180601 MPU6050センサの初期化処理
mmmmmmmmma 4:53eceb750885 404 mpu.initialize();
mmmmmmmmma 4:53eceb750885 405 // mpu.setAcceleroRange(MPU6050_ACCELERO_RANGE_8G); //加速度センサ 計測レンジ設定（今回は2Gか4Gがよさそう）
mmmmmmmmma 4:53eceb750885 406 mpu.setAcceleroRange(MPU6050_ACCELERO_RANGE_2G);
mmmmmmmmma 4:53eceb750885 407 // mpu.setGyroRange(MPU6050_GYRO_RANGE_1000); //ジャイロセンサ 計測レンジ設定（ここも250か500がよさそう（そんなに早く回転しないので））
mmmmmmmmma 4:53eceb750885 408 mpu.setGyroRange(MPU6050_GYRO_RANGE_250);
mmmmmmmmma 4:53eceb750885 409 ///180601
mmmmmmmmma 4:53eceb750885 410
mmmmmmmmma 4:53eceb750885 411 if( mpu.testConnection() ){
mmmmmmmmma 4:53eceb750885 412 // pc.printf("mpu test:OK\n\r");
mmmmmmmmma 4:53eceb750885 413 }else{
mmmmmmmmma 4:53eceb750885 414 // pc.printf("mpu test:NG\n\r");
mmmmmmmmma 4:53eceb750885 415 }
mmmmmmmmma 4:53eceb750885 416
mmmmmmmmma 4:53eceb750885 417 float ticker_ms = (TICKER_INTERVAL / 100.0f);
mmmmmmmmma 4:53eceb750885 418 Ticker ticker;
mmmmmmmmma 4:53eceb750885 419 ticker.attach(periodicCallback, ticker_ms);//0.02f //.2f-sec
mmmmmmmmma 4:53eceb750885 420
mmmmmmmmma 4:53eceb750885 421 pc.printf("Start!! \n");
mmmmmmmmma 4:53eceb750885 422
mmmmmmmmma 4:53eceb750885 423 while(true) {
mmmmmmmmma 4:53eceb750885 424 left = 1.0;
mmmmmmmmma 4:53eceb750885 425 right = 1.0;
mmmmmmmmma 4:53eceb750885 426 // wait(5);
mmmmmmmmma 4:53eceb750885 427 updateValue();
mmmmmmmmma 4:53eceb750885 428 wait(0.5);
mmmmmmmmma 4:53eceb750885 429 }
mmmmmmmmma 4:53eceb750885 430 }
mmmmmmmmma 4:53eceb750885 431
mmmmmmmmma 4:53eceb750885 432
jksoft 0:76dfa9657d9d 433 /**************************************************************************/
jksoft 0:76dfa9657d9d 434 int main(void)
jksoft 0:76dfa9657d9d 435 {
jksoft 0:76dfa9657d9d 436 sw1.mode(PullUp);
jksoft 0:76dfa9657d9d 437 sw2.mode(PullUp);
sibu2 1:a86c84ead24a 438
sibu2 1:a86c84ead24a 439 encl1.mode(PullNone);
sibu2 1:a86c84ead24a 440 encl2.mode(PullNone);
sibu2 1:a86c84ead24a 441 encr1.mode(PullNone);
sibu2 1:a86c84ead24a 442 encr2.mode(PullNone);
sibu2 1:a86c84ead24a 443
jksoft 0:76dfa9657d9d 444 ModeLed = 1;
jksoft 0:76dfa9657d9d 445 ConnectStateLed = 1;
jksoft 0:76dfa9657d9d 446 #if DBG
jksoft 0:76dfa9657d9d 447 //pc.baud(921600);
jksoft 0:76dfa9657d9d 448 pc.baud(9600);
jksoft 0:76dfa9657d9d 449 pc.printf("Start\n\r");
jksoft 0:76dfa9657d9d 450 #endif
jksoft 0:76dfa9657d9d 451 outlow = 0;
jksoft 0:76dfa9657d9d 452
jksoft 0:76dfa9657d9d 453 if(sw2 == 0)
jksoft 0:76dfa9657d9d 454 {
sibu2 1:a86c84ead24a 455 // pin1.mode(PullDown);
jksoft 0:76dfa9657d9d 456 // pin1.rise(&p1_rise);
jksoft 0:76dfa9657d9d 457 while(1)
jksoft 0:76dfa9657d9d 458 {
jksoft 0:76dfa9657d9d 459 //int in1 = pin1;
jksoft 0:76dfa9657d9d 460 //int in2 = pin2;
jksoft 0:76dfa9657d9d 461 //ModeLed = pin1;
jksoft 0:76dfa9657d9d 462 //pc.printf("dat = %d %d\r\n",in1,in2);
jksoft 0:76dfa9657d9d 463 base();
jksoft 0:76dfa9657d9d 464 #if 0
jksoft 0:76dfa9657d9d 465 left = 1.0;
jksoft 0:76dfa9657d9d 466 right = 1.0;
jksoft 0:76dfa9657d9d 467 wait(5);
jksoft 0:76dfa9657d9d 468 left = -1.0;
jksoft 0:76dfa9657d9d 469 right = -1.0;
jksoft 0:76dfa9657d9d 470 wait(5);
jksoft 0:76dfa9657d9d 471 #endif
jksoft 0:76dfa9657d9d 472 }
jksoft 0:76dfa9657d9d 473 }
jksoft 0:76dfa9657d9d 474
Dyotty 3:f707552ec50a 475 // MPU6050 Initialize
Dyotty 3:f707552ec50a 476 mpu.initialize();
Dyotty 3:f707552ec50a 477 mpu.setAcceleroRange(MPU6050_ACCELERO_RANGE_8G);
Dyotty 3:f707552ec50a 478 mpu.setGyroRange(MPU6050_GYRO_RANGE_1000);
Dyotty 3:f707552ec50a 479
jksoft 0:76dfa9657d9d 480 ble.init();
jksoft 0:76dfa9657d9d 481 ble.onConnection(onConnected);
jksoft 0:76dfa9657d9d 482 ble.onDisconnection(onDisconnected);
jksoft 0:76dfa9657d9d 483 ble.onDataWritten(onDataWritten);
jksoft 0:76dfa9657d9d 484
jksoft 0:76dfa9657d9d 485 /* setup advertising */
jksoft 0:76dfa9657d9d 486 ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
jksoft 0:76dfa9657d9d 487 ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
jksoft 0:76dfa9657d9d 488 ble.accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME,
jksoft 0:76dfa9657d9d 489 (const uint8_t *)"mbed WallbotBLE", sizeof("mbed WallbotBLE") - 1);
jksoft 0:76dfa9657d9d 490 ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS,
jksoft 0:76dfa9657d9d 491 (const uint8_t *)RCBController_service_uuid, sizeof(RCBController_service_uuid));
jksoft 0:76dfa9657d9d 492
jksoft 0:76dfa9657d9d 493 ble.setAdvertisingInterval(160); /* 100ms; in multiples of 0.625ms. */
jksoft 0:76dfa9657d9d 494 ble.startAdvertising();
jksoft 0:76dfa9657d9d 495
jksoft 0:76dfa9657d9d 496 ble.addService(RCBControllerService);
jksoft 0:76dfa9657d9d 497
jksoft 0:76dfa9657d9d 498
jksoft 0:76dfa9657d9d 499 while (true) {
jksoft 0:76dfa9657d9d 500 if(sw1 == 0)
jksoft 0:76dfa9657d9d 501 {
jksoft 0:76dfa9657d9d 502 bValue = 1;
jksoft 0:76dfa9657d9d 503 line_mode = 1;
mmmmmmmmma 4:53eceb750885 504 test();
jksoft 0:76dfa9657d9d 505 line_mode = 0;
jksoft 0:76dfa9657d9d 506 bValue = 0;
jksoft 0:76dfa9657d9d 507 }
jksoft 0:76dfa9657d9d 508 ble.waitForEvent();
jksoft 0:76dfa9657d9d 509 }
jksoft 0:76dfa9657d9d 510 }
jksoft 0:76dfa9657d9d 511