Yutaka Yoshida / Mbed 2 deprecated BLE_WallbotBLE_Challenge_byYUTAKA

Gセンサ制御　実装版

Dependencies:   mbed

Fork of BLE_WallbotBLE_Challenge_byYUTAKA by keisuke Yoshida

main.cpp@4:951fabc31ba6, 2018-06-11 (annotated)

Committer:
Keisuke_Yoshida
Date:
Mon Jun 11 06:51:49 2018 +0000
Revision:
4:951fabc31ba6
Parent:
3:f707552ec50a
Child:
5:29f7f535d4b2
firstcmit

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
Keisuke_Yoshida 4:951fabc31ba6 285 //ライントレース関数
jksoft 0:76dfa9657d9d 286 void line(void)
jksoft 0:76dfa9657d9d 287 {
jksoft 0:76dfa9657d9d 288 ModeLed = 0;
jksoft 0:76dfa9657d9d 289 wait(1);
jksoft 0:76dfa9657d9d 290 while(sw1 != 0)
jksoft 0:76dfa9657d9d 291 {
jksoft 0:76dfa9657d9d 292 #if 0
jksoft 0:76dfa9657d9d 293 int line = get_line(0) ? 0 : 1;
jksoft 0:76dfa9657d9d 294 line |= get_line(1) ? 0 : 2;
jksoft 0:76dfa9657d9d 295 line |= get_line(2) ? 0 : 4;
jksoft 0:76dfa9657d9d 296 line |= get_line(3) ? 0 : 8;
jksoft 0:76dfa9657d9d 297 #else
jksoft 0:76dfa9657d9d 298 int line = get_line(0) ? 1 : 0;
jksoft 0:76dfa9657d9d 299 line |= get_line(1) ? 2 : 0;
jksoft 0:76dfa9657d9d 300 line |= get_line(2) ? 4 : 0;
jksoft 0:76dfa9657d9d 301 line |= get_line(3) ? 8 : 0;
jksoft 0:76dfa9657d9d 302 #endif
sibu2 1:a86c84ead24a 303
sibu2 1:a86c84ead24a 304 #if DBG
sibu2 1:a86c84ead24a 305 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 306 #endif
Keisuke_Yoshida 4:951fabc31ba6 307
sibu2 1:a86c84ead24a 308 #if 1
jksoft 0:76dfa9657d9d 309 switch(line)
jksoft 0:76dfa9657d9d 310 {
jksoft 0:76dfa9657d9d 311 case 1: // ○○○●
jksoft 0:76dfa9657d9d 312 left = 1.0;
jksoft 0:76dfa9657d9d 313 right = -1.0;
jksoft 0:76dfa9657d9d 314 break;
jksoft 0:76dfa9657d9d 315 case 3: // ○○●●
jksoft 0:76dfa9657d9d 316 left = 1.0;
jksoft 0:76dfa9657d9d 317 right = -0.5;
jksoft 0:76dfa9657d9d 318 break;
jksoft 0:76dfa9657d9d 319 case 2: // ○○●○
jksoft 0:76dfa9657d9d 320 left = 1.0;
jksoft 0:76dfa9657d9d 321 right = 0.5;
jksoft 0:76dfa9657d9d 322 break;
jksoft 0:76dfa9657d9d 323 case 6: // ○●●○
jksoft 0:76dfa9657d9d 324 left = 1.0;
jksoft 0:76dfa9657d9d 325 right = 1.0;
jksoft 0:76dfa9657d9d 326 break;
jksoft 0:76dfa9657d9d 327 case 4: // ○●○○
jksoft 0:76dfa9657d9d 328 left = 0.5;
jksoft 0:76dfa9657d9d 329 right = 1.0;
jksoft 0:76dfa9657d9d 330 break;
jksoft 0:76dfa9657d9d 331 case 12: // ●●○○
jksoft 0:76dfa9657d9d 332 left = -0.5;
jksoft 0:76dfa9657d9d 333 right = 1.0;
jksoft 0:76dfa9657d9d 334 break;
jksoft 0:76dfa9657d9d 335 case 8: // ●○○○
jksoft 0:76dfa9657d9d 336 left = -1.0;
jksoft 0:76dfa9657d9d 337 right = 1.0;
jksoft 0:76dfa9657d9d 338 break;
jksoft 0:76dfa9657d9d 339 default:
jksoft 0:76dfa9657d9d 340 left = 1.0;
jksoft 0:76dfa9657d9d 341 right = 1.0;
jksoft 0:76dfa9657d9d 342 break;
jksoft 0:76dfa9657d9d 343 }
sibu2 1:a86c84ead24a 344 #endif
Keisuke_Yoshida 4:951fabc31ba6 345
Keisuke_Yoshida 4:951fabc31ba6 346 }
Keisuke_Yoshida 4:951fabc31ba6 347 ModeLed = 1;
Keisuke_Yoshida 4:951fabc31ba6 348 left = 0.0;
Keisuke_Yoshida 4:951fabc31ba6 349 right = 0.0;
Keisuke_Yoshida 4:951fabc31ba6 350 wait(1);
Keisuke_Yoshida 4:951fabc31ba6 351 }
Keisuke_Yoshida 4:951fabc31ba6 352
Keisuke_Yoshida 4:951fabc31ba6 353 //動作パターン関数（工事中）
Keisuke_Yoshida 4:951fabc31ba6 354 void wb_control(void)
Keisuke_Yoshida 4:951fabc31ba6 355 {
Keisuke_Yoshida 4:951fabc31ba6 356 ModeLed = 0;
Keisuke_Yoshida 4:951fabc31ba6 357 wait(1);
Keisuke_Yoshida 4:951fabc31ba6 358 while(sw1 != 0)
Keisuke_Yoshida 4:951fabc31ba6 359 {
Keisuke_Yoshida 4:951fabc31ba6 360 #if 0
Keisuke_Yoshida 4:951fabc31ba6 361 int line = get_line(0) ? 0 : 1;
Keisuke_Yoshida 4:951fabc31ba6 362 line |= get_line(1) ? 0 : 2;
Keisuke_Yoshida 4:951fabc31ba6 363 line |= get_line(2) ? 0 : 4;
Keisuke_Yoshida 4:951fabc31ba6 364 line |= get_line(3) ? 0 : 8;
Keisuke_Yoshida 4:951fabc31ba6 365 #else
Keisuke_Yoshida 4:951fabc31ba6 366 int line = get_line(0) ? 1 : 0;
Keisuke_Yoshida 4:951fabc31ba6 367 line |= get_line(1) ? 2 : 0;
Keisuke_Yoshida 4:951fabc31ba6 368 line |= get_line(2) ? 4 : 0;
Keisuke_Yoshida 4:951fabc31ba6 369 line |= get_line(3) ? 8 : 0;
Keisuke_Yoshida 4:951fabc31ba6 370 #endif
Keisuke_Yoshida 4:951fabc31ba6 371
Keisuke_Yoshida 4:951fabc31ba6 372 #if DBG
Keisuke_Yoshida 4:951fabc31ba6 373 pc.printf("line=%02x %04x %04x %04x %04x\n\r",line,base_fsen[0],base_fsen[1],base_fsen[2],base_fsen[3]);
Keisuke_Yoshida 4:951fabc31ba6 374 #endif
Keisuke_Yoshida 4:951fabc31ba6 375
Keisuke_Yoshida 4:951fabc31ba6 376 //改造中のソース
Keisuke_Yoshida 4:951fabc31ba6 377
Keisuke_Yoshida 4:951fabc31ba6 378 #if 1
Keisuke_Yoshida 4:951fabc31ba6 379 switch(line)
Keisuke_Yoshida 4:951fabc31ba6 380 {
Keisuke_Yoshida 4:951fabc31ba6 381 case 1: //直進モード上から下へ ○○○● とりあえず書いてみたパターン
Keisuke_Yoshida 4:951fabc31ba6 382 if(ay<0){
Keisuke_Yoshida 4:951fabc31ba6 383 left = 1.0;
Keisuke_Yoshida 4:951fabc31ba6 384 right = 1.0;
Keisuke_Yoshida 4:951fabc31ba6 385 }
Keisuke_Yoshida 4:951fabc31ba6 386 else if(ay>0){
Keisuke_Yoshida 4:951fabc31ba6 387 left = 1.0;
Keisuke_Yoshida 4:951fabc31ba6 388 right = 1.0;
Keisuke_Yoshida 4:951fabc31ba6 389 }
Keisuke_Yoshida 4:951fabc31ba6 390
Keisuke_Yoshida 4:951fabc31ba6 391 break;
Keisuke_Yoshida 4:951fabc31ba6 392 case 3: //直進モード下から上へ ○○●●
Keisuke_Yoshida 4:951fabc31ba6 393 left = 1.0;
Keisuke_Yoshida 4:951fabc31ba6 394 right = -0.5;
Keisuke_Yoshida 4:951fabc31ba6 395 break;
Keisuke_Yoshida 4:951fabc31ba6 396 case 2: //回転モード上から下へ○○●○
Keisuke_Yoshida 4:951fabc31ba6 397 left = 1.0;
Keisuke_Yoshida 4:951fabc31ba6 398 right = 0.5;
Keisuke_Yoshida 4:951fabc31ba6 399 break;
Keisuke_Yoshida 4:951fabc31ba6 400 case 6: //回転モード下から上へ○●●○
Keisuke_Yoshida 4:951fabc31ba6 401 left = 1.0;
Keisuke_Yoshida 4:951fabc31ba6 402 right = 1.0;
Keisuke_Yoshida 4:951fabc31ba6 403 break;
Keisuke_Yoshida 4:951fabc31ba6 404 case 4: //後進モード上から下へ○●○○
Keisuke_Yoshida 4:951fabc31ba6 405 left = 0.5;
Keisuke_Yoshida 4:951fabc31ba6 406 right = 1.0;
Keisuke_Yoshida 4:951fabc31ba6 407 break;
Keisuke_Yoshida 4:951fabc31ba6 408 case 12: //後進モード下から上へ●●○○
Keisuke_Yoshida 4:951fabc31ba6 409 left = -0.5;
Keisuke_Yoshida 4:951fabc31ba6 410 right = 1.0;
Keisuke_Yoshida 4:951fabc31ba6 411 break;
Keisuke_Yoshida 4:951fabc31ba6 412 case 8: // ●○○○
Keisuke_Yoshida 4:951fabc31ba6 413 left = -1.0;
Keisuke_Yoshida 4:951fabc31ba6 414 right = 1.0;
Keisuke_Yoshida 4:951fabc31ba6 415 break;
Keisuke_Yoshida 4:951fabc31ba6 416 default:
Keisuke_Yoshida 4:951fabc31ba6 417 left = 1.0;
Keisuke_Yoshida 4:951fabc31ba6 418 right = 1.0;
Keisuke_Yoshida 4:951fabc31ba6 419 break;
Keisuke_Yoshida 4:951fabc31ba6 420 }
Keisuke_Yoshida 4:951fabc31ba6 421 #endif
Keisuke_Yoshida 4:951fabc31ba6 422
jksoft 0:76dfa9657d9d 423 }
jksoft 0:76dfa9657d9d 424 ModeLed = 1;
jksoft 0:76dfa9657d9d 425 left = 0.0;
jksoft 0:76dfa9657d9d 426 right = 0.0;
jksoft 0:76dfa9657d9d 427 wait(1);
jksoft 0:76dfa9657d9d 428 }
sibu2 1:a86c84ead24a 429
sibu2 1:a86c84ead24a 430 #if 0
jksoft 0:76dfa9657d9d 431 int counter1 = 0;
jksoft 0:76dfa9657d9d 432 void p1_rise()
jksoft 0:76dfa9657d9d 433 {
jksoft 0:76dfa9657d9d 434 if( pin2 == 1 )
jksoft 0:76dfa9657d9d 435 {
jksoft 0:76dfa9657d9d 436 counter1++;
jksoft 0:76dfa9657d9d 437 }
jksoft 0:76dfa9657d9d 438 else
jksoft 0:76dfa9657d9d 439 {
jksoft 0:76dfa9657d9d 440 counter1--;
jksoft 0:76dfa9657d9d 441 }
jksoft 0:76dfa9657d9d 442 }
sibu2 1:a86c84ead24a 443 #endif
sibu2 1:a86c84ead24a 444
jksoft 0:76dfa9657d9d 445 /**************************************************************************/
jksoft 0:76dfa9657d9d 446 /*!
jksoft 0:76dfa9657d9d 447 @brief Program entry point
jksoft 0:76dfa9657d9d 448 */
jksoft 0:76dfa9657d9d 449 /**************************************************************************/
jksoft 0:76dfa9657d9d 450 int main(void)
jksoft 0:76dfa9657d9d 451 {
jksoft 0:76dfa9657d9d 452 sw1.mode(PullUp);
jksoft 0:76dfa9657d9d 453 sw2.mode(PullUp);
sibu2 1:a86c84ead24a 454
sibu2 1:a86c84ead24a 455 encl1.mode(PullNone);
sibu2 1:a86c84ead24a 456 encl2.mode(PullNone);
sibu2 1:a86c84ead24a 457 encr1.mode(PullNone);
sibu2 1:a86c84ead24a 458 encr2.mode(PullNone);
sibu2 1:a86c84ead24a 459
jksoft 0:76dfa9657d9d 460 ModeLed = 1;
jksoft 0:76dfa9657d9d 461 ConnectStateLed = 1;
jksoft 0:76dfa9657d9d 462 #if DBG
jksoft 0:76dfa9657d9d 463 //pc.baud(921600);
jksoft 0:76dfa9657d9d 464 pc.baud(9600);
jksoft 0:76dfa9657d9d 465 pc.printf("Start\n\r");
jksoft 0:76dfa9657d9d 466 #endif
jksoft 0:76dfa9657d9d 467 outlow = 0;
jksoft 0:76dfa9657d9d 468
jksoft 0:76dfa9657d9d 469 if(sw2 == 0)
jksoft 0:76dfa9657d9d 470 {
sibu2 1:a86c84ead24a 471 // pin1.mode(PullDown);
jksoft 0:76dfa9657d9d 472 // pin1.rise(&p1_rise);
jksoft 0:76dfa9657d9d 473 while(1)
jksoft 0:76dfa9657d9d 474 {
jksoft 0:76dfa9657d9d 475 //int in1 = pin1;
jksoft 0:76dfa9657d9d 476 //int in2 = pin2;
jksoft 0:76dfa9657d9d 477 //ModeLed = pin1;
jksoft 0:76dfa9657d9d 478 //pc.printf("dat = %d %d\r\n",in1,in2);
jksoft 0:76dfa9657d9d 479 base();
jksoft 0:76dfa9657d9d 480 #if 0
jksoft 0:76dfa9657d9d 481 left = 1.0;
jksoft 0:76dfa9657d9d 482 right = 1.0;
jksoft 0:76dfa9657d9d 483 wait(5);
jksoft 0:76dfa9657d9d 484 left = -1.0;
jksoft 0:76dfa9657d9d 485 right = -1.0;
jksoft 0:76dfa9657d9d 486 wait(5);
jksoft 0:76dfa9657d9d 487 #endif
jksoft 0:76dfa9657d9d 488 }
jksoft 0:76dfa9657d9d 489 }
jksoft 0:76dfa9657d9d 490
Dyotty 3:f707552ec50a 491 // MPU6050 Initialize
Dyotty 3:f707552ec50a 492 mpu.initialize();
Dyotty 3:f707552ec50a 493 mpu.setAcceleroRange(MPU6050_ACCELERO_RANGE_8G);
Dyotty 3:f707552ec50a 494 mpu.setGyroRange(MPU6050_GYRO_RANGE_1000);
Dyotty 3:f707552ec50a 495
jksoft 0:76dfa9657d9d 496 ble.init();
jksoft 0:76dfa9657d9d 497 ble.onConnection(onConnected);
jksoft 0:76dfa9657d9d 498 ble.onDisconnection(onDisconnected);
jksoft 0:76dfa9657d9d 499 ble.onDataWritten(onDataWritten);
jksoft 0:76dfa9657d9d 500
jksoft 0:76dfa9657d9d 501 /* setup advertising */
jksoft 0:76dfa9657d9d 502 ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
jksoft 0:76dfa9657d9d 503 ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
jksoft 0:76dfa9657d9d 504 ble.accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME,
jksoft 0:76dfa9657d9d 505 (const uint8_t *)"mbed WallbotBLE", sizeof("mbed WallbotBLE") - 1);
jksoft 0:76dfa9657d9d 506 ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS,
jksoft 0:76dfa9657d9d 507 (const uint8_t *)RCBController_service_uuid, sizeof(RCBController_service_uuid));
jksoft 0:76dfa9657d9d 508
jksoft 0:76dfa9657d9d 509 ble.setAdvertisingInterval(160); /* 100ms; in multiples of 0.625ms. */
jksoft 0:76dfa9657d9d 510 ble.startAdvertising();
jksoft 0:76dfa9657d9d 511
jksoft 0:76dfa9657d9d 512 ble.addService(RCBControllerService);
jksoft 0:76dfa9657d9d 513
jksoft 0:76dfa9657d9d 514
jksoft 0:76dfa9657d9d 515 while (true) {
jksoft 0:76dfa9657d9d 516 if(sw1 == 0)
jksoft 0:76dfa9657d9d 517 {
jksoft 0:76dfa9657d9d 518 bValue = 1;
jksoft 0:76dfa9657d9d 519 line_mode = 1;
Keisuke_Yoshida 4:951fabc31ba6 520 //line();
Keisuke_Yoshida 4:951fabc31ba6 521 wb_control(); //動作モード関数
jksoft 0:76dfa9657d9d 522 line_mode = 0;
jksoft 0:76dfa9657d9d 523 bValue = 0;
jksoft 0:76dfa9657d9d 524 }
jksoft 0:76dfa9657d9d 525 ble.waitForEvent();
jksoft 0:76dfa9657d9d 526 }
jksoft 0:76dfa9657d9d 527 }
jksoft 0:76dfa9657d9d 528