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@12:252acb9c1201, 2018-06-18 (annotated)

Committer:
mmmmmmmmma
Date:
Mon Jun 18 08:43:33 2018 +0000
Revision:
12:252acb9c1201
Parent:
11:fcb6d8929c88 
6/18 ?????

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
Dyotty 6:213041054f97 7 #define DBG 1
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
Dyotty 5:29f7f535d4b2 49 // Wallbot State
Dyotty 5:29f7f535d4b2 50 int stt_Mode;
Dyotty 5:29f7f535d4b2 51
jksoft 0:76dfa9657d9d 52 int get_line(int num);
jksoft 0:76dfa9657d9d 53
jksoft 0:76dfa9657d9d 54 /* RCBController Service */
jksoft 0:76dfa9657d9d 55 static const uint16_t RCBController_service_uuid = 0xFFF0;
jksoft 0:76dfa9657d9d 56 static const uint16_t RCBController_Characteristic_uuid = 0xFFF1;
jksoft 0:76dfa9657d9d 57 static const uint16_t RCBController_b_Characteristic_uuid = 0xFFF3;
jksoft 0:76dfa9657d9d 58 uint8_t RCBControllerPayload[10] = {0,};
jksoft 0:76dfa9657d9d 59
jksoft 0:76dfa9657d9d 60 GattCharacteristic ControllerChar (RCBController_Characteristic_uuid,RCBControllerPayload,10, 10,
jksoft 0:76dfa9657d9d 61 GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE |
jksoft 0:76dfa9657d9d 62 GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE_WITHOUT_RESPONSE);
sibu2 1:a86c84ead24a 63 //static uint8_t _bValue = 0x00;
jksoft 0:76dfa9657d9d 64 static uint8_t _mValue[10] = {0,};
jksoft 0:76dfa9657d9d 65 GattCharacteristic b_Char(RCBController_b_Characteristic_uuid, _mValue, sizeof(_mValue), sizeof(_mValue),
jksoft 0:76dfa9657d9d 66 GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ | GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY);
jksoft 0:76dfa9657d9d 67
jksoft 0:76dfa9657d9d 68 GattCharacteristic *ControllerChars[] = {&ControllerChar,&b_Char};
jksoft 0:76dfa9657d9d 69 GattService RCBControllerService(RCBController_service_uuid, ControllerChars, sizeof(ControllerChars) / sizeof(GattCharacteristic *));
jksoft 0:76dfa9657d9d 70
jksoft 0:76dfa9657d9d 71 RCBController controller;
jksoft 0:76dfa9657d9d 72
jksoft 0:76dfa9657d9d 73 void onConnected(Gap::Handle_t handle, const Gap::ConnectionParams_t *params)
jksoft 0:76dfa9657d9d 74 {
jksoft 0:76dfa9657d9d 75 ConnectStateLed = 0;
jksoft 0:76dfa9657d9d 76 #if DBG
jksoft 0:76dfa9657d9d 77 pc.printf("Connected\n\r");
jksoft 0:76dfa9657d9d 78 #endif
jksoft 0:76dfa9657d9d 79 }
jksoft 0:76dfa9657d9d 80
jksoft 0:76dfa9657d9d 81 void onDisconnected(Gap::Handle_t handle, Gap::DisconnectionReason_t reason)
jksoft 0:76dfa9657d9d 82 {
sibu2 1:a86c84ead24a 83 left = 0;
sibu2 1:a86c84ead24a 84 right = 0;
sibu2 1:a86c84ead24a 85
jksoft 0:76dfa9657d9d 86 ble.startAdvertising();
jksoft 0:76dfa9657d9d 87 ConnectStateLed = 1;
jksoft 0:76dfa9657d9d 88 #if DBG
jksoft 0:76dfa9657d9d 89 pc.printf("Disconnected\n\r");
jksoft 0:76dfa9657d9d 90 #endif
jksoft 0:76dfa9657d9d 91 }
jksoft 0:76dfa9657d9d 92
jksoft 0:76dfa9657d9d 93 void periodicCallback(void)
jksoft 0:76dfa9657d9d 94 {
jksoft 0:76dfa9657d9d 95 if (!ble.getGapState().connected) {
jksoft 0:76dfa9657d9d 96 return;
jksoft 0:76dfa9657d9d 97 }
jksoft 0:76dfa9657d9d 98 int line = get_line(0) ? 1 : 0;
jksoft 0:76dfa9657d9d 99 line |= get_line(1) ? 2 : 0;
jksoft 0:76dfa9657d9d 100 line |= get_line(2) ? 4 : 0;
jksoft 0:76dfa9657d9d 101 line |= get_line(3) ? 8 : 0;
jksoft 0:76dfa9657d9d 102 if( (bValue == 0)&&(line != 0) )
jksoft 0:76dfa9657d9d 103 {
jksoft 0:76dfa9657d9d 104 // game over
jksoft 0:76dfa9657d9d 105 left = 0.0;
jksoft 0:76dfa9657d9d 106 right = 0.0;
jksoft 0:76dfa9657d9d 107 bValue = 10;
jksoft 0:76dfa9657d9d 108 }
jksoft 0:76dfa9657d9d 109 if( bValue > 0 )
jksoft 0:76dfa9657d9d 110 {
jksoft 0:76dfa9657d9d 111 memcpy( _mValue , "GAME OVER",10);
jksoft 0:76dfa9657d9d 112 ble.updateCharacteristicValue(b_Char.getValueAttribute().getHandle(), (uint8_t *)_mValue, sizeof(_mValue));
jksoft 0:76dfa9657d9d 113 ModeLed = !ModeLed;
jksoft 0:76dfa9657d9d 114 bValue--;
jksoft 0:76dfa9657d9d 115 if( bValue == 0 )
jksoft 0:76dfa9657d9d 116 {
jksoft 0:76dfa9657d9d 117 ModeLed = 1;
jksoft 0:76dfa9657d9d 118 challenge_mode = 0;
jksoft 0:76dfa9657d9d 119 ticker.detach();
jksoft 0:76dfa9657d9d 120 }
jksoft 0:76dfa9657d9d 121 }
jksoft 0:76dfa9657d9d 122 }
jksoft 0:76dfa9657d9d 123
jksoft 0:76dfa9657d9d 124
jksoft 0:76dfa9657d9d 125 // GattEvent
jksoft 0:76dfa9657d9d 126 void onDataWritten(const GattCharacteristicWriteCBParams *params)
jksoft 0:76dfa9657d9d 127 {
jksoft 0:76dfa9657d9d 128 if( (params->charHandle == ControllerChar.getValueAttribute().getHandle()) && (line_mode == 0))
jksoft 0:76dfa9657d9d 129 {
jksoft 0:76dfa9657d9d 130 memcpy( &controller.data[0], params->data , params->len );
jksoft 0:76dfa9657d9d 131 //memcpy( &controller.data[0], RCBControllerPayload, sizeof(controller));
jksoft 0:76dfa9657d9d 132 #if DBG
jksoft 0:76dfa9657d9d 133
jksoft 0:76dfa9657d9d 134 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 135 controller.data[5],controller.data[6],controller.data[7],controller.data[8],controller.data[9]);
jksoft 0:76dfa9657d9d 136 #endif
jksoft 0:76dfa9657d9d 137 float right_factor;
jksoft 0:76dfa9657d9d 138 float left_factor;
jksoft 0:76dfa9657d9d 139
jksoft 0:76dfa9657d9d 140 left_factor = ((float)((int)controller.status.LeftAnalogUD -128) / 128.0);
jksoft 0:76dfa9657d9d 141 right_factor = ((float)((int)controller.status.RightAnalogUD -128) / 128.0);
jksoft 0:76dfa9657d9d 142
jksoft 0:76dfa9657d9d 143 if(challenge_mode == 1)
jksoft 0:76dfa9657d9d 144 {
jksoft 0:76dfa9657d9d 145 if( bValue == 0 )
jksoft 0:76dfa9657d9d 146 {
jksoft 0:76dfa9657d9d 147 float factor = ((float)((int)controller.status.AcceleX -128) / 128.0);
jksoft 0:76dfa9657d9d 148
jksoft 0:76dfa9657d9d 149 float right_factor = ((factor <= 0.0) ? 1.0 : 1.0 - (factor*2));
jksoft 0:76dfa9657d9d 150 float left_factor = ((factor >= 0.0) ? 1.0 : 1.0 - (-factor*2));
jksoft 0:76dfa9657d9d 151
jksoft 0:76dfa9657d9d 152 if( controller.status.B == 1 )
jksoft 0:76dfa9657d9d 153 {
jksoft 0:76dfa9657d9d 154 left = left_factor;
jksoft 0:76dfa9657d9d 155 right = right_factor;
jksoft 0:76dfa9657d9d 156 }
jksoft 0:76dfa9657d9d 157 else if( controller.status.A == 1 )
jksoft 0:76dfa9657d9d 158 {
jksoft 0:76dfa9657d9d 159 left = -right_factor;
jksoft 0:76dfa9657d9d 160 right = -left_factor;
jksoft 0:76dfa9657d9d 161 }
jksoft 0:76dfa9657d9d 162 else
jksoft 0:76dfa9657d9d 163 {
jksoft 0:76dfa9657d9d 164 left = 0;
jksoft 0:76dfa9657d9d 165 right = 0;
jksoft 0:76dfa9657d9d 166 }
jksoft 0:76dfa9657d9d 167 }
jksoft 0:76dfa9657d9d 168 }
jksoft 0:76dfa9657d9d 169 else if( (left_factor != 0.0)||(right_factor != 0.0) )
jksoft 0:76dfa9657d9d 170 {
jksoft 0:76dfa9657d9d 171 left = left_factor;
jksoft 0:76dfa9657d9d 172 right = right_factor;
jksoft 0:76dfa9657d9d 173 }
jksoft 0:76dfa9657d9d 174 else
jksoft 0:76dfa9657d9d 175 {
jksoft 0:76dfa9657d9d 176 float factor = ((float)((int)controller.status.AcceleX -128) / 128.0);
jksoft 0:76dfa9657d9d 177
jksoft 0:76dfa9657d9d 178 float right_factor = ((factor <= 0.0) ? 1.0 : 1.0 - (factor*2));
jksoft 0:76dfa9657d9d 179 float left_factor = ((factor >= 0.0) ? 1.0 : 1.0 - (-factor*2));
jksoft 0:76dfa9657d9d 180
jksoft 0:76dfa9657d9d 181 if( controller.status.B == 1 )
jksoft 0:76dfa9657d9d 182 {
jksoft 0:76dfa9657d9d 183 left = left_factor;
jksoft 0:76dfa9657d9d 184 right = right_factor;
jksoft 0:76dfa9657d9d 185 }
jksoft 0:76dfa9657d9d 186 else if( controller.status.A == 1 )
jksoft 0:76dfa9657d9d 187 {
jksoft 0:76dfa9657d9d 188 left = -right_factor;
jksoft 0:76dfa9657d9d 189 right = -left_factor;
jksoft 0:76dfa9657d9d 190 }
jksoft 0:76dfa9657d9d 191 else if( controller.status.UP == 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.DOWN == 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.RIGHT == 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 if( controller.status.LEFT == 1 )
jksoft 0:76dfa9657d9d 207 {
jksoft 0:76dfa9657d9d 208 left = -1.0;
jksoft 0:76dfa9657d9d 209 right = 1.0;
jksoft 0:76dfa9657d9d 210 }
jksoft 0:76dfa9657d9d 211 else
jksoft 0:76dfa9657d9d 212 {
jksoft 0:76dfa9657d9d 213 left = 0.0;
jksoft 0:76dfa9657d9d 214 right = 0.0;
jksoft 0:76dfa9657d9d 215 }
jksoft 0:76dfa9657d9d 216 if((controller.status.UP == 1)&&(controller.status.DOWN == 1))
jksoft 0:76dfa9657d9d 217 {
jksoft 0:76dfa9657d9d 218 left = 0.0;
jksoft 0:76dfa9657d9d 219 right = 0.0;
jksoft 0:76dfa9657d9d 220 ModeLed = 0;
jksoft 0:76dfa9657d9d 221 challenge_mode = 1;
jksoft 0:76dfa9657d9d 222 ticker.attach(periodicCallback, 0.1);
jksoft 0:76dfa9657d9d 223
jksoft 0:76dfa9657d9d 224 }
jksoft 0:76dfa9657d9d 225 }
jksoft 0:76dfa9657d9d 226 //ControllerStateLed = (float)controller.status.LeftAnalogLR / 255.0;
jksoft 0:76dfa9657d9d 227 }
jksoft 0:76dfa9657d9d 228 }
jksoft 0:76dfa9657d9d 229
jksoft 0:76dfa9657d9d 230 int get_fsen(int num)
jksoft 0:76dfa9657d9d 231 {
jksoft 0:76dfa9657d9d 232 switch(num)
jksoft 0:76dfa9657d9d 233 {
jksoft 0:76dfa9657d9d 234 case 0:
jksoft 0:76dfa9657d9d 235 return((int)fsen1.read_u16());
jksoft 0:76dfa9657d9d 236 case 1:
jksoft 0:76dfa9657d9d 237 return((int)fsen2.read_u16());
jksoft 0:76dfa9657d9d 238 case 2:
jksoft 0:76dfa9657d9d 239 return((int)fsen3.read_u16());
jksoft 0:76dfa9657d9d 240 case 3:
jksoft 0:76dfa9657d9d 241 return((int)fsen4.read_u16());
jksoft 0:76dfa9657d9d 242 }
jksoft 0:76dfa9657d9d 243 return(0);
jksoft 0:76dfa9657d9d 244 }
jksoft 0:76dfa9657d9d 245
jksoft 0:76dfa9657d9d 246 void base()
jksoft 0:76dfa9657d9d 247 {
jksoft 0:76dfa9657d9d 248 wait(0.5);
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] = 0;
jksoft 0:76dfa9657d9d 253 }
jksoft 0:76dfa9657d9d 254
jksoft 0:76dfa9657d9d 255 for(int j=0;j<10;j++)
jksoft 0:76dfa9657d9d 256 {
jksoft 0:76dfa9657d9d 257 for(int i=0;i<4;i++)
jksoft 0:76dfa9657d9d 258 {
jksoft 0:76dfa9657d9d 259 base_fsen[i] += get_fsen(i);
jksoft 0:76dfa9657d9d 260 }
jksoft 0:76dfa9657d9d 261 wait_ms(50);
jksoft 0:76dfa9657d9d 262 }
jksoft 0:76dfa9657d9d 263 for(int i=0;i<4;i++)
jksoft 0:76dfa9657d9d 264 {
jksoft 0:76dfa9657d9d 265 base_fsen[i] = base_fsen[i] / 10;
jksoft 0:76dfa9657d9d 266 }
sibu2 1:a86c84ead24a 267 #if DBG
jksoft 0:76dfa9657d9d 268 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 269 #endif
jksoft 0:76dfa9657d9d 270 }
jksoft 0:76dfa9657d9d 271
jksoft 0:76dfa9657d9d 272 int get_line(int num)
jksoft 0:76dfa9657d9d 273 {
jksoft 0:76dfa9657d9d 274 int in = get_fsen(num);
jksoft 0:76dfa9657d9d 275 int ret = 0;
jksoft 0:76dfa9657d9d 276
jksoft 0:76dfa9657d9d 277 #if 1
jksoft 0:76dfa9657d9d 278 if(in > 700)
jksoft 0:76dfa9657d9d 279 #else
jksoft 0:76dfa9657d9d 280 if( (in > (base_fsen[num] + 200))||(in < (base_fsen[num] - 200)))
jksoft 0:76dfa9657d9d 281 #endif
jksoft 0:76dfa9657d9d 282 {
jksoft 0:76dfa9657d9d 283 ret = 1;
jksoft 0:76dfa9657d9d 284 }
jksoft 0:76dfa9657d9d 285 return(ret);
jksoft 0:76dfa9657d9d 286 }
jksoft 0:76dfa9657d9d 287
Keisuke_Yoshida 4:951fabc31ba6 288 //ライントレース関数
jksoft 0:76dfa9657d9d 289 void line(void)
jksoft 0:76dfa9657d9d 290 {
jksoft 0:76dfa9657d9d 291 ModeLed = 0;
jksoft 0:76dfa9657d9d 292 wait(1);
jksoft 0:76dfa9657d9d 293 while(sw1 != 0)
jksoft 0:76dfa9657d9d 294 {
jksoft 0:76dfa9657d9d 295 #if 0
jksoft 0:76dfa9657d9d 296 int line = get_line(0) ? 0 : 1;
jksoft 0:76dfa9657d9d 297 line |= get_line(1) ? 0 : 2;
jksoft 0:76dfa9657d9d 298 line |= get_line(2) ? 0 : 4;
jksoft 0:76dfa9657d9d 299 line |= get_line(3) ? 0 : 8;
jksoft 0:76dfa9657d9d 300 #else
jksoft 0:76dfa9657d9d 301 int line = get_line(0) ? 1 : 0;
jksoft 0:76dfa9657d9d 302 line |= get_line(1) ? 2 : 0;
jksoft 0:76dfa9657d9d 303 line |= get_line(2) ? 4 : 0;
jksoft 0:76dfa9657d9d 304 line |= get_line(3) ? 8 : 0;
jksoft 0:76dfa9657d9d 305 #endif
sibu2 1:a86c84ead24a 306
sibu2 1:a86c84ead24a 307 #if DBG
sibu2 1:a86c84ead24a 308 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 309 #endif
Keisuke_Yoshida 4:951fabc31ba6 310
sibu2 1:a86c84ead24a 311 #if 1
jksoft 0:76dfa9657d9d 312 switch(line)
jksoft 0:76dfa9657d9d 313 {
jksoft 0:76dfa9657d9d 314 case 1: // ○○○●
jksoft 0:76dfa9657d9d 315 left = 1.0;
jksoft 0:76dfa9657d9d 316 right = -1.0;
jksoft 0:76dfa9657d9d 317 break;
jksoft 0:76dfa9657d9d 318 case 3: // ○○●●
jksoft 0:76dfa9657d9d 319 left = 1.0;
jksoft 0:76dfa9657d9d 320 right = -0.5;
jksoft 0:76dfa9657d9d 321 break;
jksoft 0:76dfa9657d9d 322 case 2: // ○○●○
jksoft 0:76dfa9657d9d 323 left = 1.0;
jksoft 0:76dfa9657d9d 324 right = 0.5;
jksoft 0:76dfa9657d9d 325 break;
jksoft 0:76dfa9657d9d 326 case 6: // ○●●○
jksoft 0:76dfa9657d9d 327 left = 1.0;
jksoft 0:76dfa9657d9d 328 right = 1.0;
jksoft 0:76dfa9657d9d 329 break;
jksoft 0:76dfa9657d9d 330 case 4: // ○●○○
jksoft 0:76dfa9657d9d 331 left = 0.5;
jksoft 0:76dfa9657d9d 332 right = 1.0;
jksoft 0:76dfa9657d9d 333 break;
jksoft 0:76dfa9657d9d 334 case 12: // ●●○○
jksoft 0:76dfa9657d9d 335 left = -0.5;
jksoft 0:76dfa9657d9d 336 right = 1.0;
jksoft 0:76dfa9657d9d 337 break;
jksoft 0:76dfa9657d9d 338 case 8: // ●○○○
jksoft 0:76dfa9657d9d 339 left = -1.0;
jksoft 0:76dfa9657d9d 340 right = 1.0;
jksoft 0:76dfa9657d9d 341 break;
jksoft 0:76dfa9657d9d 342 default:
jksoft 0:76dfa9657d9d 343 left = 1.0;
jksoft 0:76dfa9657d9d 344 right = 1.0;
jksoft 0:76dfa9657d9d 345 break;
jksoft 0:76dfa9657d9d 346 }
sibu2 1:a86c84ead24a 347 #endif
Keisuke_Yoshida 4:951fabc31ba6 348
Keisuke_Yoshida 4:951fabc31ba6 349 }
Keisuke_Yoshida 4:951fabc31ba6 350 ModeLed = 1;
Keisuke_Yoshida 4:951fabc31ba6 351 left = 0.0;
Keisuke_Yoshida 4:951fabc31ba6 352 right = 0.0;
Keisuke_Yoshida 4:951fabc31ba6 353 wait(1);
Keisuke_Yoshida 4:951fabc31ba6 354 }
Keisuke_Yoshida 4:951fabc31ba6 355
Keisuke_Yoshida 4:951fabc31ba6 356 //動作パターン関数（工事中）
Keisuke_Yoshida 4:951fabc31ba6 357 void wb_control(void)
Keisuke_Yoshida 4:951fabc31ba6 358 {
Keisuke_Yoshida 4:951fabc31ba6 359 ModeLed = 0;
Dyotty 6:213041054f97 360 wait(1);
Dyotty 5:29f7f535d4b2 361
Dyotty 6:213041054f97 362 // Wallbot State Initialize
Dyotty 6:213041054f97 363 // Up Straight : 1
Dyotty 6:213041054f97 364 // Up Back : 2
Dyotty 6:213041054f97 365 // Up Rotate : 3
Dyotty 6:213041054f97 366 // Down Straight : 4
Dyotty 6:213041054f97 367 // Down Back : 5
Dyotty 6:213041054f97 368 // Down Rotate : 6
Dyotty 6:213041054f97 369 stt_Mode = 1;
Dyotty 6:213041054f97 370
Dyotty 6:213041054f97 371 // 値を格納する用の配列、変数
Dyotty 6:213041054f97 372 float acData[3];
Dyotty 6:213041054f97 373 float gyData[3];
Dyotty 6:213041054f97 374 float ax = 0;
Dyotty 6:213041054f97 375 float ay = 0;
mmmmmmmmma 12:252acb9c1201 376 float az = 0;
mmmmmmmmma 12:252acb9c1201 377 float a = 0;
Dyotty 7:897b9ded86e2 378 float buf_ax[10];
Dyotty 9:965bfb378463 379 float mean_ax = 0;
Dyotty 9:965bfb378463 380 float sum_ax = 0;
mmmmmmmmma 11:fcb6d8929c88 381 float log_ax[100];
Dyotty 5:29f7f535d4b2 382 int cnt_loop = 0;
Dyotty 7:897b9ded86e2 383 int cnt_back = 0;
Dyotty 9:965bfb378463 384 int cnt_straight = 0;
mmmmmmmmma 12:252acb9c1201 385 float thre_bump = 4.0;
Dyotty 9:965bfb378463 386 bool enable_ChangeMode = false;
Dyotty 9:965bfb378463 387
Dyotty 9:965bfb378463 388 for(int i = 0; i < 10; i++)
Dyotty 9:965bfb378463 389 {
Dyotty 9:965bfb378463 390 buf_ax[i] = 0;
Dyotty 9:965bfb378463 391 }
Dyotty 5:29f7f535d4b2 392
mmmmmmmmma 11:fcb6d8929c88 393
mmmmmmmmma 11:fcb6d8929c88 394 for(int i = 0; i < 100; i++)
mmmmmmmmma 11:fcb6d8929c88 395 {
mmmmmmmmma 11:fcb6d8929c88 396 log_ax[i] = 0;
mmmmmmmmma 11:fcb6d8929c88 397 }
mmmmmmmmma 11:fcb6d8929c88 398
Keisuke_Yoshida 4:951fabc31ba6 399 while(sw1 != 0)
Keisuke_Yoshida 4:951fabc31ba6 400 {
Dyotty 6:213041054f97 401 // Wait(0.1);
Dyotty 5:29f7f535d4b2 402
Dyotty 6:213041054f97 403 // Get value
Dyotty 6:213041054f97 404 //加速度を取得
Dyotty 6:213041054f97 405 Timer acTimer;
Dyotty 6:213041054f97 406 acTimer.start();
Dyotty 6:213041054f97 407 mpu.getAccelero(acData); //加速度を取得 acDataに格納
Dyotty 6:213041054f97 408 acTimer.stop();
Dyotty 6:213041054f97 409 // at = acTimer.read_ms();
Dyotty 6:213041054f97 410 acTimer.reset();
Dyotty 6:213041054f97 411
Dyotty 6:213041054f97 412 //ジャイロを取得
Dyotty 6:213041054f97 413 Timer gyTimer;
Dyotty 6:213041054f97 414 gyTimer.start();
Dyotty 6:213041054f97 415 mpu.getGyro(gyData); //ジャイロの値（角加速度）を取得 gyDataに格納
Dyotty 6:213041054f97 416 gyTimer.stop();
Dyotty 6:213041054f97 417 // gt = gyTimer.read_ms();
Dyotty 6:213041054f97 418 gyTimer.reset();
Dyotty 6:213041054f97 419
Dyotty 6:213041054f97 420 //floatの値を合計5桁、小数点以下3桁の形でPCへ送信
mmmmmmmmma 12:252acb9c1201 421 // 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 12:252acb9c1201 422 // pc.printf("%5.3f, ", acData[2]);
Dyotty 6:213041054f97 423
Dyotty 6:213041054f97 424 ax = acData[0];
Dyotty 6:213041054f97 425 ay = acData[1];
mmmmmmmmma 12:252acb9c1201 426 az = acData[2];
mmmmmmmmma 12:252acb9c1201 427 // a = sqrt(ax * ax + ay * ay + az * az);
mmmmmmmmma 12:252acb9c1201 428 // pc.printf("%5.3f, ", ax);
Dyotty 5:29f7f535d4b2 429
mmmmmmmmma 12:252acb9c1201 430 // log_ax[cnt_loop] = ax;
mmmmmmmmma 11:fcb6d8929c88 431
Dyotty 9:965bfb378463 432 // if(!enble_ChangeMode)
Dyotty 9:965bfb378463 433 // {
Dyotty 9:965bfb378463 434 // if(cnt_loop > 20)
Dyotty 9:965bfb378463 435 // {
Dyotty 9:965bfb378463 436 // enble_ChangeMode = true;
Dyotty 9:965bfb378463 437 // }
Dyotty 9:965bfb378463 438 // }
Dyotty 5:29f7f535d4b2 439
Dyotty 7:897b9ded86e2 440 // ax Buffer Store
Dyotty 7:897b9ded86e2 441 for(int i = 9; i > 0; i--)
Dyotty 7:897b9ded86e2 442 {
Dyotty 7:897b9ded86e2 443 buf_ax[i] = buf_ax[i - 1];
Dyotty 7:897b9ded86e2 444 }
Dyotty 7:897b9ded86e2 445 buf_ax[0] = ax;
Dyotty 9:965bfb378463 446 sum_ax += buf_ax[1];
Dyotty 9:965bfb378463 447 sum_ax -= buf_ax[9];
Dyotty 9:965bfb378463 448 mean_ax = sum_ax / 9;
mmmmmmmmma 12:252acb9c1201 449 pc.printf("%5.3f, ", ax);
mmmmmmmmma 12:252acb9c1201 450 //
Dyotty 9:965bfb378463 451 // for(int i = 29; i > 0; i--)
Dyotty 9:965bfb378463 452 // {
Dyotty 9:965bfb378463 453 // log_ax[i] = log_ax[i - 1];
Dyotty 9:965bfb378463 454 // }
Dyotty 9:965bfb378463 455 // log_ax[0] = ax;
Dyotty 9:965bfb378463 456
mmmmmmmmma 11:fcb6d8929c88 457 if(cnt_loop % 100 == 0)
mmmmmmmmma 11:fcb6d8929c88 458 {
mmmmmmmmma 12:252acb9c1201 459 // for(int i = 0; i < 100; i++)
mmmmmmmmma 12:252acb9c1201 460 // {
mmmmmmmmma 12:252acb9c1201 461 // pc.printf("%5.3f, ", log_ax[i]);
mmmmmmmmma 12:252acb9c1201 462 // }
mmmmmmmmma 11:fcb6d8929c88 463 // pc.printf("loop : %d\n", log_ax);
Dyotty 9:965bfb378463 464 // pc.printf("loop : %d\n", cnt_loop);
Dyotty 9:965bfb378463 465 // pc.printf("loop : %d\n", cnt_loop);
Dyotty 9:965bfb378463 466 // pc.printf("loop : %d\n", cnt_loop);
Dyotty 9:965bfb378463 467 // pc.printf("loop : %d\n", cnt_loop);
mmmmmmmmma 11:fcb6d8929c88 468 }
Dyotty 5:29f7f535d4b2 469
Keisuke_Yoshida 4:951fabc31ba6 470 #if 1
Dyotty 5:29f7f535d4b2 471 switch(stt_Mode)
Keisuke_Yoshida 4:951fabc31ba6 472 {
Dyotty 6:213041054f97 473 case 1: // Up Straight
Dyotty 10:d443aea353a2 474 // pc.printf("Mode 1 \n");
Dyotty 5:29f7f535d4b2 475
Dyotty 9:965bfb378463 476 // for(int i = 0; i < 30; i++)
Dyotty 9:965bfb378463 477 // {
Dyotty 9:965bfb378463 478 // pc.printf("%5.3f, ", log_ax[i]);
Dyotty 9:965bfb378463 479 // }
Dyotty 9:965bfb378463 480 // pc.printf("\n");
Dyotty 9:965bfb378463 481 // pc.printf("\n");
Dyotty 9:965bfb378463 482 // pc.printf("\n");
Dyotty 9:965bfb378463 483
Dyotty 9:965bfb378463 484 if(ay > 0.5){
Dyotty 9:965bfb378463 485 left = 0.9;
Dyotty 5:29f7f535d4b2 486 right = 1.0;
Dyotty 9:965bfb378463 487 // pc.printf("Lean Right");
Keisuke_Yoshida 4:951fabc31ba6 488 }
Dyotty 9:965bfb378463 489 else if(ay < -0.5){
Dyotty 5:29f7f535d4b2 490 left = 1.0;
Dyotty 9:965bfb378463 491 right = 0.9;
Dyotty 9:965bfb378463 492 // pc.printf("Lean Left");
Dyotty 5:29f7f535d4b2 493 }
Dyotty 5:29f7f535d4b2 494 else
Dyotty 5:29f7f535d4b2 495 {
Keisuke_Yoshida 4:951fabc31ba6 496 left = 1.0;
Keisuke_Yoshida 4:951fabc31ba6 497 right = 1.0;
Keisuke_Yoshida 4:951fabc31ba6 498 }
Dyotty 5:29f7f535d4b2 499
Dyotty 5:29f7f535d4b2 500 // Judge Bump
Dyotty 9:965bfb378463 501 if(enable_ChangeMode)
Dyotty 9:965bfb378463 502 {
mmmmmmmmma 12:252acb9c1201 503 if(ax - buf_ax[5] < -thre_bump || ax - buf_ax[5] > thre_bump)
Dyotty 7:897b9ded86e2 504 {
Dyotty 7:897b9ded86e2 505 stt_Mode = 2;
Dyotty 7:897b9ded86e2 506 pc.printf("Mode 1 -> 2 \n");
Dyotty 9:965bfb378463 507
Dyotty 9:965bfb378463 508 enable_ChangeMode = false;
Dyotty 7:897b9ded86e2 509 }
Dyotty 9:965bfb378463 510 }else
Dyotty 9:965bfb378463 511 {
Dyotty 9:965bfb378463 512 cnt_straight++;
Dyotty 9:965bfb378463 513 if(cnt_straight > 100)
Dyotty 9:965bfb378463 514 {
Dyotty 9:965bfb378463 515 enable_ChangeMode = true;
Dyotty 9:965bfb378463 516 cnt_straight = 0;
Dyotty 9:965bfb378463 517 }
Dyotty 9:965bfb378463 518 }
Dyotty 5:29f7f535d4b2 519
Dyotty 5:29f7f535d4b2 520 break;
Dyotty 6:213041054f97 521 case 2: // Up Back
mmmmmmmmma 12:252acb9c1201 522 // pc.printf("Mode 2 \n");
Dyotty 7:897b9ded86e2 523 if(cnt_back < 30)
Dyotty 5:29f7f535d4b2 524 {
Dyotty 9:965bfb378463 525 left = 0;//-1.0;
Dyotty 9:965bfb378463 526 right = 0;//-1.0;
Dyotty 9:965bfb378463 527 //cnt_back++;
Dyotty 5:29f7f535d4b2 528 }
Dyotty 5:29f7f535d4b2 529 else{
Dyotty 7:897b9ded86e2 530 cnt_back = 0;
Dyotty 5:29f7f535d4b2 531 stt_Mode = 3;
Dyotty 7:897b9ded86e2 532 pc.printf("Mode 2 -> 3 \n");
Dyotty 5:29f7f535d4b2 533 }
Keisuke_Yoshida 4:951fabc31ba6 534
Keisuke_Yoshida 4:951fabc31ba6 535 break;
Dyotty 6:213041054f97 536 case 3: // Up Rotate
Dyotty 7:897b9ded86e2 537 pc.printf("Mode 3 \n");
Dyotty 9:965bfb378463 538 if(!(ax < -9.7/* && ay < 0.5 && ay >= -0.3*/)) // Change Using Gyro??
Dyotty 5:29f7f535d4b2 539 {
Dyotty 5:29f7f535d4b2 540 left = 1.0;
Dyotty 5:29f7f535d4b2 541 right = 0;
Dyotty 5:29f7f535d4b2 542 }
Dyotty 5:29f7f535d4b2 543 else
Dyotty 5:29f7f535d4b2 544 {
Dyotty 5:29f7f535d4b2 545 stt_Mode = 4;
Dyotty 7:897b9ded86e2 546 pc.printf("Mode 3 - > 4 \n");
Dyotty 7:897b9ded86e2 547
Dyotty 5:29f7f535d4b2 548 }
Dyotty 5:29f7f535d4b2 549
Keisuke_Yoshida 4:951fabc31ba6 550 break;
Dyotty 5:29f7f535d4b2 551
Dyotty 6:213041054f97 552 case 4: // Down Straight
Dyotty 7:897b9ded86e2 553 pc.printf("Mode 4 \n");
Dyotty 9:965bfb378463 554 if(ay > 0.5){
Dyotty 5:29f7f535d4b2 555 left = 1.0;
Dyotty 9:965bfb378463 556 right = 0.9;
Dyotty 5:29f7f535d4b2 557 }
Dyotty 9:965bfb378463 558 else if(ay < -0.5){
Dyotty 9:965bfb378463 559 left = 0.9;
Dyotty 5:29f7f535d4b2 560 right = 1.0;
Dyotty 5:29f7f535d4b2 561 }
Dyotty 5:29f7f535d4b2 562 else
Dyotty 5:29f7f535d4b2 563 {
Dyotty 5:29f7f535d4b2 564 left = 1.0;
Dyotty 5:29f7f535d4b2 565 right = 1.0;
Dyotty 5:29f7f535d4b2 566 }
Dyotty 5:29f7f535d4b2 567
Dyotty 5:29f7f535d4b2 568 // Judge Bump
Dyotty 9:965bfb378463 569 if(enable_ChangeMode)
Dyotty 5:29f7f535d4b2 570 {
Dyotty 9:965bfb378463 571 if(ax - buf_ax[1] > thre_bump /*&& (ax - buf_ax[2] > thre_bump)*/)
Dyotty 9:965bfb378463 572 {
Dyotty 9:965bfb378463 573 stt_Mode = 5;
Dyotty 9:965bfb378463 574 pc.printf("Mode 4 -> 5 \n");
Dyotty 9:965bfb378463 575
Dyotty 9:965bfb378463 576 enable_ChangeMode = false;
Dyotty 9:965bfb378463 577 }
Dyotty 9:965bfb378463 578 }else
Dyotty 9:965bfb378463 579 {
Dyotty 9:965bfb378463 580 cnt_straight++;
Dyotty 9:965bfb378463 581 if(cnt_straight > 10)
Dyotty 9:965bfb378463 582 {
Dyotty 9:965bfb378463 583 enable_ChangeMode = true;
Dyotty 9:965bfb378463 584 cnt_straight = 0;
Dyotty 9:965bfb378463 585 }
Dyotty 5:29f7f535d4b2 586 }
Dyotty 5:29f7f535d4b2 587
Keisuke_Yoshida 4:951fabc31ba6 588 break;
Dyotty 5:29f7f535d4b2 589 case 5:
Dyotty 7:897b9ded86e2 590 pc.printf("Mode 5 \n");
Dyotty 7:897b9ded86e2 591 if(cnt_back < 30)
Dyotty 5:29f7f535d4b2 592 {
Dyotty 5:29f7f535d4b2 593 left = -1.0;
Dyotty 5:29f7f535d4b2 594 right = -1.0;
Dyotty 7:897b9ded86e2 595 cnt_back++;
Dyotty 5:29f7f535d4b2 596 }
Dyotty 5:29f7f535d4b2 597 else{
Dyotty 7:897b9ded86e2 598 cnt_back = 0;
Dyotty 5:29f7f535d4b2 599 stt_Mode = 6;
Dyotty 5:29f7f535d4b2 600 pc.printf(" 5 -> 6 \n");
Dyotty 5:29f7f535d4b2 601 }
Dyotty 5:29f7f535d4b2 602
Keisuke_Yoshida 4:951fabc31ba6 603 break;
Dyotty 5:29f7f535d4b2 604 case 6: //
Dyotty 7:897b9ded86e2 605 pc.printf("Mode 6 \n");
Dyotty 9:965bfb378463 606 if(!(ax > 9.7 /*&& ay < 0.5 && ay >= 0*/)) // Change Using Gyro??
Dyotty 5:29f7f535d4b2 607 {
Dyotty 5:29f7f535d4b2 608 left = 0;
Dyotty 5:29f7f535d4b2 609 right = 1.0;
Dyotty 5:29f7f535d4b2 610 }
Dyotty 5:29f7f535d4b2 611 else
Dyotty 5:29f7f535d4b2 612 {
Dyotty 5:29f7f535d4b2 613 stt_Mode = 1;
Dyotty 5:29f7f535d4b2 614 pc.printf(" 6 -> 1 \n");
Dyotty 9:965bfb378463 615 //
Dyotty 9:965bfb378463 616 // for(int i = 0; i < 3; i++)
Dyotty 9:965bfb378463 617 // {
Dyotty 9:965bfb378463 618 // buf_ax[i] = ax;
Dyotty 9:965bfb378463 619 // }
Dyotty 9:965bfb378463 620 }
Dyotty 8:102c19510dce 621
Dyotty 5:29f7f535d4b2 622
Keisuke_Yoshida 4:951fabc31ba6 623 break;
Keisuke_Yoshida 4:951fabc31ba6 624 default:
Keisuke_Yoshida 4:951fabc31ba6 625 break;
Keisuke_Yoshida 4:951fabc31ba6 626 }
Keisuke_Yoshida 4:951fabc31ba6 627 #endif
Keisuke_Yoshida 4:951fabc31ba6 628
Dyotty 7:897b9ded86e2 629 cnt_loop++;
Dyotty 7:897b9ded86e2 630
Dyotty 7:897b9ded86e2 631 if(cnt_loop > 2000)
Dyotty 7:897b9ded86e2 632 {
Dyotty 7:897b9ded86e2 633 cnt_loop = 0;
Dyotty 7:897b9ded86e2 634 }
jksoft 0:76dfa9657d9d 635 }
jksoft 0:76dfa9657d9d 636 ModeLed = 1;
jksoft 0:76dfa9657d9d 637 left = 0.0;
jksoft 0:76dfa9657d9d 638 right = 0.0;
jksoft 0:76dfa9657d9d 639 wait(1);
jksoft 0:76dfa9657d9d 640 }
sibu2 1:a86c84ead24a 641
sibu2 1:a86c84ead24a 642 #if 0
jksoft 0:76dfa9657d9d 643 int counter1 = 0;
jksoft 0:76dfa9657d9d 644 void p1_rise()
jksoft 0:76dfa9657d9d 645 {
jksoft 0:76dfa9657d9d 646 if( pin2 == 1 )
jksoft 0:76dfa9657d9d 647 {
jksoft 0:76dfa9657d9d 648 counter1++;
jksoft 0:76dfa9657d9d 649 }
jksoft 0:76dfa9657d9d 650 else
jksoft 0:76dfa9657d9d 651 {
jksoft 0:76dfa9657d9d 652 counter1--;
jksoft 0:76dfa9657d9d 653 }
jksoft 0:76dfa9657d9d 654 }
sibu2 1:a86c84ead24a 655 #endif
sibu2 1:a86c84ead24a 656
jksoft 0:76dfa9657d9d 657 /**************************************************************************/
jksoft 0:76dfa9657d9d 658 /*!
jksoft 0:76dfa9657d9d 659 @brief Program entry point
jksoft 0:76dfa9657d9d 660 */
jksoft 0:76dfa9657d9d 661 /**************************************************************************/
jksoft 0:76dfa9657d9d 662 int main(void)
jksoft 0:76dfa9657d9d 663 {
jksoft 0:76dfa9657d9d 664 sw1.mode(PullUp);
jksoft 0:76dfa9657d9d 665 sw2.mode(PullUp);
sibu2 1:a86c84ead24a 666
sibu2 1:a86c84ead24a 667 encl1.mode(PullNone);
sibu2 1:a86c84ead24a 668 encl2.mode(PullNone);
sibu2 1:a86c84ead24a 669 encr1.mode(PullNone);
sibu2 1:a86c84ead24a 670 encr2.mode(PullNone);
sibu2 1:a86c84ead24a 671
jksoft 0:76dfa9657d9d 672 ModeLed = 1;
jksoft 0:76dfa9657d9d 673 ConnectStateLed = 1;
jksoft 0:76dfa9657d9d 674 #if DBG
jksoft 0:76dfa9657d9d 675 //pc.baud(921600);
jksoft 0:76dfa9657d9d 676 pc.baud(9600);
jksoft 0:76dfa9657d9d 677 pc.printf("Start\n\r");
jksoft 0:76dfa9657d9d 678 #endif
jksoft 0:76dfa9657d9d 679 outlow = 0;
jksoft 0:76dfa9657d9d 680
jksoft 0:76dfa9657d9d 681 if(sw2 == 0)
jksoft 0:76dfa9657d9d 682 {
sibu2 1:a86c84ead24a 683 // pin1.mode(PullDown);
jksoft 0:76dfa9657d9d 684 // pin1.rise(&p1_rise);
jksoft 0:76dfa9657d9d 685 while(1)
jksoft 0:76dfa9657d9d 686 {
jksoft 0:76dfa9657d9d 687 //int in1 = pin1;
jksoft 0:76dfa9657d9d 688 //int in2 = pin2;
jksoft 0:76dfa9657d9d 689 //ModeLed = pin1;
jksoft 0:76dfa9657d9d 690 //pc.printf("dat = %d %d\r\n",in1,in2);
jksoft 0:76dfa9657d9d 691 base();
jksoft 0:76dfa9657d9d 692 #if 0
jksoft 0:76dfa9657d9d 693 left = 1.0;
jksoft 0:76dfa9657d9d 694 right = 1.0;
jksoft 0:76dfa9657d9d 695 wait(5);
jksoft 0:76dfa9657d9d 696 left = -1.0;
jksoft 0:76dfa9657d9d 697 right = -1.0;
jksoft 0:76dfa9657d9d 698 wait(5);
jksoft 0:76dfa9657d9d 699 #endif
jksoft 0:76dfa9657d9d 700 }
jksoft 0:76dfa9657d9d 701 }
jksoft 0:76dfa9657d9d 702
Dyotty 6:213041054f97 703 // // MPU6050 Initialize
Dyotty 6:213041054f97 704 // mpu.initialize();
Dyotty 6:213041054f97 705 // mpu.setAcceleroRange(MPU6050_ACCELERO_RANGE_8G);
Dyotty 6:213041054f97 706 // mpu.setGyroRange(MPU6050_GYRO_RANGE_1000);
Dyotty 6:213041054f97 707
Dyotty 6:213041054f97 708 ///180601 MPU6050センサの初期化処理
Dyotty 3:f707552ec50a 709 mpu.initialize();
Dyotty 6:213041054f97 710 // mpu.setAcceleroRange(MPU6050_ACCELERO_RANGE_8G); //加速度センサ 計測レンジ設定（今回は2Gか4Gがよさそう）
Dyotty 6:213041054f97 711 mpu.setAcceleroRange(MPU6050_ACCELERO_RANGE_2G);
Dyotty 6:213041054f97 712 // mpu.setGyroRange(MPU6050_GYRO_RANGE_1000); //ジャイロセンサ 計測レンジ設定（ここも250か500がよさそう（そんなに早く回転しないので））
Dyotty 6:213041054f97 713 mpu.setGyroRange(MPU6050_GYRO_RANGE_250);
Dyotty 6:213041054f97 714 ///180601
Dyotty 3:f707552ec50a 715
jksoft 0:76dfa9657d9d 716 ble.init();
jksoft 0:76dfa9657d9d 717 ble.onConnection(onConnected);
jksoft 0:76dfa9657d9d 718 ble.onDisconnection(onDisconnected);
jksoft 0:76dfa9657d9d 719 ble.onDataWritten(onDataWritten);
jksoft 0:76dfa9657d9d 720
jksoft 0:76dfa9657d9d 721 /* setup advertising */
jksoft 0:76dfa9657d9d 722 ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
jksoft 0:76dfa9657d9d 723 ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
jksoft 0:76dfa9657d9d 724 ble.accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME,
jksoft 0:76dfa9657d9d 725 (const uint8_t *)"mbed WallbotBLE", sizeof("mbed WallbotBLE") - 1);
jksoft 0:76dfa9657d9d 726 ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS,
jksoft 0:76dfa9657d9d 727 (const uint8_t *)RCBController_service_uuid, sizeof(RCBController_service_uuid));
jksoft 0:76dfa9657d9d 728
jksoft 0:76dfa9657d9d 729 ble.setAdvertisingInterval(160); /* 100ms; in multiples of 0.625ms. */
jksoft 0:76dfa9657d9d 730 ble.startAdvertising();
jksoft 0:76dfa9657d9d 731
jksoft 0:76dfa9657d9d 732 ble.addService(RCBControllerService);
jksoft 0:76dfa9657d9d 733
Dyotty 5:29f7f535d4b2 734
jksoft 0:76dfa9657d9d 735 while (true) {
jksoft 0:76dfa9657d9d 736 if(sw1 == 0)
jksoft 0:76dfa9657d9d 737 {
jksoft 0:76dfa9657d9d 738 bValue = 1;
jksoft 0:76dfa9657d9d 739 line_mode = 1;
Keisuke_Yoshida 4:951fabc31ba6 740 //line();
Keisuke_Yoshida 4:951fabc31ba6 741 wb_control(); //動作モード関数
jksoft 0:76dfa9657d9d 742 line_mode = 0;
jksoft 0:76dfa9657d9d 743 bValue = 0;
jksoft 0:76dfa9657d9d 744 }
jksoft 0:76dfa9657d9d 745 ble.waitForEvent();
jksoft 0:76dfa9657d9d 746 }
jksoft 0:76dfa9657d9d 747 }
jksoft 0:76dfa9657d9d 748