Yutaka Yoshida
/
BLE_WallbotBLE_Challenge_byYUTAKA3
6/22 上下往復完成版
Fork of BLE_WallbotBLE_Challenge_byYUTAKA3 by
main.cpp@10:d443aea353a2, 2018-06-18 (annotated)
- Committer:
- Dyotty
- Date:
- Mon Jun 18 06:18:04 2018 +0000
- Revision:
- 10:d443aea353a2
- Parent:
- 9:965bfb378463
- Child:
- 11:fcb6d8929c88
6/18
Who changed what in which revision?
User | Revision | Line number | New 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; |
Dyotty | 7:897b9ded86e2 | 376 | float buf_ax[10]; |
Dyotty | 9:965bfb378463 | 377 | float mean_ax = 0; |
Dyotty | 9:965bfb378463 | 378 | float sum_ax = 0; |
Dyotty | 9:965bfb378463 | 379 | // float log_ax[30]; |
Dyotty | 5:29f7f535d4b2 | 380 | int cnt_loop = 0; |
Dyotty | 7:897b9ded86e2 | 381 | int cnt_back = 0; |
Dyotty | 9:965bfb378463 | 382 | int cnt_straight = 0; |
Dyotty | 7:897b9ded86e2 | 383 | float thre_bump = 2.0; |
Dyotty | 9:965bfb378463 | 384 | bool enable_ChangeMode = false; |
Dyotty | 9:965bfb378463 | 385 | |
Dyotty | 9:965bfb378463 | 386 | for(int i = 0; i < 10; i++) |
Dyotty | 9:965bfb378463 | 387 | { |
Dyotty | 9:965bfb378463 | 388 | buf_ax[i] = 0; |
Dyotty | 9:965bfb378463 | 389 | } |
Dyotty | 5:29f7f535d4b2 | 390 | |
Keisuke_Yoshida | 4:951fabc31ba6 | 391 | while(sw1 != 0) |
Keisuke_Yoshida | 4:951fabc31ba6 | 392 | { |
Dyotty | 6:213041054f97 | 393 | // Wait(0.1); |
Dyotty | 5:29f7f535d4b2 | 394 | |
Dyotty | 6:213041054f97 | 395 | // Get value |
Dyotty | 6:213041054f97 | 396 | //加速度を取得 |
Dyotty | 6:213041054f97 | 397 | Timer acTimer; |
Dyotty | 6:213041054f97 | 398 | acTimer.start(); |
Dyotty | 6:213041054f97 | 399 | mpu.getAccelero(acData); //加速度を取得 acDataに格納 |
Dyotty | 6:213041054f97 | 400 | acTimer.stop(); |
Dyotty | 6:213041054f97 | 401 | // at = acTimer.read_ms(); |
Dyotty | 6:213041054f97 | 402 | acTimer.reset(); |
Dyotty | 6:213041054f97 | 403 | |
Dyotty | 6:213041054f97 | 404 | //ジャイロを取得 |
Dyotty | 6:213041054f97 | 405 | Timer gyTimer; |
Dyotty | 6:213041054f97 | 406 | gyTimer.start(); |
Dyotty | 6:213041054f97 | 407 | mpu.getGyro(gyData); //ジャイロの値(角加速度)を取得 gyDataに格納 |
Dyotty | 6:213041054f97 | 408 | gyTimer.stop(); |
Dyotty | 6:213041054f97 | 409 | // gt = gyTimer.read_ms(); |
Dyotty | 6:213041054f97 | 410 | gyTimer.reset(); |
Dyotty | 6:213041054f97 | 411 | |
Dyotty | 6:213041054f97 | 412 | //floatの値を合計5桁、小数点以下3桁の形でPCへ送信 |
Dyotty | 10:d443aea353a2 | 413 | 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]); |
Dyotty | 6:213041054f97 | 414 | |
Dyotty | 6:213041054f97 | 415 | ax = acData[0]; |
Dyotty | 6:213041054f97 | 416 | ay = acData[1]; |
Dyotty | 5:29f7f535d4b2 | 417 | |
Dyotty | 9:965bfb378463 | 418 | // if(!enble_ChangeMode) |
Dyotty | 9:965bfb378463 | 419 | // { |
Dyotty | 9:965bfb378463 | 420 | // if(cnt_loop > 20) |
Dyotty | 9:965bfb378463 | 421 | // { |
Dyotty | 9:965bfb378463 | 422 | // enble_ChangeMode = true; |
Dyotty | 9:965bfb378463 | 423 | // } |
Dyotty | 9:965bfb378463 | 424 | // } |
Dyotty | 5:29f7f535d4b2 | 425 | |
Dyotty | 7:897b9ded86e2 | 426 | // ax Buffer Store |
Dyotty | 7:897b9ded86e2 | 427 | for(int i = 9; i > 0; i--) |
Dyotty | 7:897b9ded86e2 | 428 | { |
Dyotty | 7:897b9ded86e2 | 429 | buf_ax[i] = buf_ax[i - 1]; |
Dyotty | 7:897b9ded86e2 | 430 | } |
Dyotty | 7:897b9ded86e2 | 431 | buf_ax[0] = ax; |
Dyotty | 9:965bfb378463 | 432 | sum_ax += buf_ax[1]; |
Dyotty | 9:965bfb378463 | 433 | sum_ax -= buf_ax[9]; |
Dyotty | 9:965bfb378463 | 434 | mean_ax = sum_ax / 9; |
Dyotty | 7:897b9ded86e2 | 435 | |
Dyotty | 9:965bfb378463 | 436 | // for(int i = 29; i > 0; i--) |
Dyotty | 9:965bfb378463 | 437 | // { |
Dyotty | 9:965bfb378463 | 438 | // log_ax[i] = log_ax[i - 1]; |
Dyotty | 9:965bfb378463 | 439 | // } |
Dyotty | 9:965bfb378463 | 440 | // log_ax[0] = ax; |
Dyotty | 9:965bfb378463 | 441 | |
Dyotty | 9:965bfb378463 | 442 | // if(cnt_loop % 100 == 0) |
Dyotty | 9:965bfb378463 | 443 | // { |
Dyotty | 9:965bfb378463 | 444 | // pc.printf("loop : %d\n", cnt_loop); |
Dyotty | 9:965bfb378463 | 445 | // pc.printf("loop : %d\n", cnt_loop); |
Dyotty | 9:965bfb378463 | 446 | // pc.printf("loop : %d\n", cnt_loop); |
Dyotty | 9:965bfb378463 | 447 | // pc.printf("loop : %d\n", cnt_loop); |
Dyotty | 9:965bfb378463 | 448 | // pc.printf("loop : %d\n", cnt_loop); |
Dyotty | 9:965bfb378463 | 449 | // } |
Dyotty | 5:29f7f535d4b2 | 450 | |
Keisuke_Yoshida | 4:951fabc31ba6 | 451 | #if 1 |
Dyotty | 5:29f7f535d4b2 | 452 | switch(stt_Mode) |
Keisuke_Yoshida | 4:951fabc31ba6 | 453 | { |
Dyotty | 6:213041054f97 | 454 | case 1: // Up Straight |
Dyotty | 10:d443aea353a2 | 455 | // pc.printf("Mode 1 \n"); |
Dyotty | 5:29f7f535d4b2 | 456 | |
Dyotty | 9:965bfb378463 | 457 | // for(int i = 0; i < 30; i++) |
Dyotty | 9:965bfb378463 | 458 | // { |
Dyotty | 9:965bfb378463 | 459 | // pc.printf("%5.3f, ", log_ax[i]); |
Dyotty | 9:965bfb378463 | 460 | // } |
Dyotty | 9:965bfb378463 | 461 | // pc.printf("\n"); |
Dyotty | 9:965bfb378463 | 462 | // pc.printf("\n"); |
Dyotty | 9:965bfb378463 | 463 | // pc.printf("\n"); |
Dyotty | 9:965bfb378463 | 464 | |
Dyotty | 9:965bfb378463 | 465 | if(ay > 0.5){ |
Dyotty | 9:965bfb378463 | 466 | left = 0.9; |
Dyotty | 5:29f7f535d4b2 | 467 | right = 1.0; |
Dyotty | 9:965bfb378463 | 468 | // pc.printf("Lean Right"); |
Keisuke_Yoshida | 4:951fabc31ba6 | 469 | } |
Dyotty | 9:965bfb378463 | 470 | else if(ay < -0.5){ |
Dyotty | 5:29f7f535d4b2 | 471 | left = 1.0; |
Dyotty | 9:965bfb378463 | 472 | right = 0.9; |
Dyotty | 9:965bfb378463 | 473 | // pc.printf("Lean Left"); |
Dyotty | 5:29f7f535d4b2 | 474 | } |
Dyotty | 5:29f7f535d4b2 | 475 | else |
Dyotty | 5:29f7f535d4b2 | 476 | { |
Keisuke_Yoshida | 4:951fabc31ba6 | 477 | left = 1.0; |
Keisuke_Yoshida | 4:951fabc31ba6 | 478 | right = 1.0; |
Keisuke_Yoshida | 4:951fabc31ba6 | 479 | } |
Dyotty | 5:29f7f535d4b2 | 480 | |
Dyotty | 5:29f7f535d4b2 | 481 | // Judge Bump |
Dyotty | 9:965bfb378463 | 482 | if(enable_ChangeMode) |
Dyotty | 9:965bfb378463 | 483 | { |
Dyotty | 9:965bfb378463 | 484 | if(ax - mean_ax > thre_bump) |
Dyotty | 7:897b9ded86e2 | 485 | { |
Dyotty | 7:897b9ded86e2 | 486 | stt_Mode = 2; |
Dyotty | 7:897b9ded86e2 | 487 | pc.printf("Mode 1 -> 2 \n"); |
Dyotty | 9:965bfb378463 | 488 | |
Dyotty | 9:965bfb378463 | 489 | enable_ChangeMode = false; |
Dyotty | 7:897b9ded86e2 | 490 | } |
Dyotty | 9:965bfb378463 | 491 | }else |
Dyotty | 9:965bfb378463 | 492 | { |
Dyotty | 9:965bfb378463 | 493 | cnt_straight++; |
Dyotty | 9:965bfb378463 | 494 | if(cnt_straight > 100) |
Dyotty | 9:965bfb378463 | 495 | { |
Dyotty | 9:965bfb378463 | 496 | enable_ChangeMode = true; |
Dyotty | 9:965bfb378463 | 497 | cnt_straight = 0; |
Dyotty | 9:965bfb378463 | 498 | } |
Dyotty | 9:965bfb378463 | 499 | } |
Dyotty | 5:29f7f535d4b2 | 500 | |
Dyotty | 5:29f7f535d4b2 | 501 | break; |
Dyotty | 6:213041054f97 | 502 | case 2: // Up Back |
Dyotty | 7:897b9ded86e2 | 503 | pc.printf("Mode 2 \n"); |
Dyotty | 7:897b9ded86e2 | 504 | if(cnt_back < 30) |
Dyotty | 5:29f7f535d4b2 | 505 | { |
Dyotty | 9:965bfb378463 | 506 | left = 0;//-1.0; |
Dyotty | 9:965bfb378463 | 507 | right = 0;//-1.0; |
Dyotty | 9:965bfb378463 | 508 | //cnt_back++; |
Dyotty | 5:29f7f535d4b2 | 509 | } |
Dyotty | 5:29f7f535d4b2 | 510 | else{ |
Dyotty | 7:897b9ded86e2 | 511 | cnt_back = 0; |
Dyotty | 5:29f7f535d4b2 | 512 | stt_Mode = 3; |
Dyotty | 7:897b9ded86e2 | 513 | pc.printf("Mode 2 -> 3 \n"); |
Dyotty | 5:29f7f535d4b2 | 514 | } |
Keisuke_Yoshida | 4:951fabc31ba6 | 515 | |
Keisuke_Yoshida | 4:951fabc31ba6 | 516 | break; |
Dyotty | 6:213041054f97 | 517 | case 3: // Up Rotate |
Dyotty | 7:897b9ded86e2 | 518 | pc.printf("Mode 3 \n"); |
Dyotty | 9:965bfb378463 | 519 | if(!(ax < -9.7/* && ay < 0.5 && ay >= -0.3*/)) // Change Using Gyro?? |
Dyotty | 5:29f7f535d4b2 | 520 | { |
Dyotty | 5:29f7f535d4b2 | 521 | left = 1.0; |
Dyotty | 5:29f7f535d4b2 | 522 | right = 0; |
Dyotty | 5:29f7f535d4b2 | 523 | } |
Dyotty | 5:29f7f535d4b2 | 524 | else |
Dyotty | 5:29f7f535d4b2 | 525 | { |
Dyotty | 5:29f7f535d4b2 | 526 | stt_Mode = 4; |
Dyotty | 7:897b9ded86e2 | 527 | pc.printf("Mode 3 - > 4 \n"); |
Dyotty | 7:897b9ded86e2 | 528 | |
Dyotty | 5:29f7f535d4b2 | 529 | } |
Dyotty | 5:29f7f535d4b2 | 530 | |
Keisuke_Yoshida | 4:951fabc31ba6 | 531 | break; |
Dyotty | 5:29f7f535d4b2 | 532 | |
Dyotty | 6:213041054f97 | 533 | case 4: // Down Straight |
Dyotty | 7:897b9ded86e2 | 534 | pc.printf("Mode 4 \n"); |
Dyotty | 9:965bfb378463 | 535 | if(ay > 0.5){ |
Dyotty | 5:29f7f535d4b2 | 536 | left = 1.0; |
Dyotty | 9:965bfb378463 | 537 | right = 0.9; |
Dyotty | 5:29f7f535d4b2 | 538 | } |
Dyotty | 9:965bfb378463 | 539 | else if(ay < -0.5){ |
Dyotty | 9:965bfb378463 | 540 | left = 0.9; |
Dyotty | 5:29f7f535d4b2 | 541 | right = 1.0; |
Dyotty | 5:29f7f535d4b2 | 542 | } |
Dyotty | 5:29f7f535d4b2 | 543 | else |
Dyotty | 5:29f7f535d4b2 | 544 | { |
Dyotty | 5:29f7f535d4b2 | 545 | left = 1.0; |
Dyotty | 5:29f7f535d4b2 | 546 | right = 1.0; |
Dyotty | 5:29f7f535d4b2 | 547 | } |
Dyotty | 5:29f7f535d4b2 | 548 | |
Dyotty | 5:29f7f535d4b2 | 549 | // Judge Bump |
Dyotty | 9:965bfb378463 | 550 | if(enable_ChangeMode) |
Dyotty | 5:29f7f535d4b2 | 551 | { |
Dyotty | 9:965bfb378463 | 552 | if(ax - buf_ax[1] > thre_bump /*&& (ax - buf_ax[2] > thre_bump)*/) |
Dyotty | 9:965bfb378463 | 553 | { |
Dyotty | 9:965bfb378463 | 554 | stt_Mode = 5; |
Dyotty | 9:965bfb378463 | 555 | pc.printf("Mode 4 -> 5 \n"); |
Dyotty | 9:965bfb378463 | 556 | |
Dyotty | 9:965bfb378463 | 557 | enable_ChangeMode = false; |
Dyotty | 9:965bfb378463 | 558 | } |
Dyotty | 9:965bfb378463 | 559 | }else |
Dyotty | 9:965bfb378463 | 560 | { |
Dyotty | 9:965bfb378463 | 561 | cnt_straight++; |
Dyotty | 9:965bfb378463 | 562 | if(cnt_straight > 10) |
Dyotty | 9:965bfb378463 | 563 | { |
Dyotty | 9:965bfb378463 | 564 | enable_ChangeMode = true; |
Dyotty | 9:965bfb378463 | 565 | cnt_straight = 0; |
Dyotty | 9:965bfb378463 | 566 | } |
Dyotty | 5:29f7f535d4b2 | 567 | } |
Dyotty | 5:29f7f535d4b2 | 568 | |
Keisuke_Yoshida | 4:951fabc31ba6 | 569 | break; |
Dyotty | 5:29f7f535d4b2 | 570 | case 5: |
Dyotty | 7:897b9ded86e2 | 571 | pc.printf("Mode 5 \n"); |
Dyotty | 7:897b9ded86e2 | 572 | if(cnt_back < 30) |
Dyotty | 5:29f7f535d4b2 | 573 | { |
Dyotty | 5:29f7f535d4b2 | 574 | left = -1.0; |
Dyotty | 5:29f7f535d4b2 | 575 | right = -1.0; |
Dyotty | 7:897b9ded86e2 | 576 | cnt_back++; |
Dyotty | 5:29f7f535d4b2 | 577 | } |
Dyotty | 5:29f7f535d4b2 | 578 | else{ |
Dyotty | 7:897b9ded86e2 | 579 | cnt_back = 0; |
Dyotty | 5:29f7f535d4b2 | 580 | stt_Mode = 6; |
Dyotty | 5:29f7f535d4b2 | 581 | pc.printf(" 5 -> 6 \n"); |
Dyotty | 5:29f7f535d4b2 | 582 | } |
Dyotty | 5:29f7f535d4b2 | 583 | |
Keisuke_Yoshida | 4:951fabc31ba6 | 584 | break; |
Dyotty | 5:29f7f535d4b2 | 585 | case 6: // |
Dyotty | 7:897b9ded86e2 | 586 | pc.printf("Mode 6 \n"); |
Dyotty | 9:965bfb378463 | 587 | if(!(ax > 9.7 /*&& ay < 0.5 && ay >= 0*/)) // Change Using Gyro?? |
Dyotty | 5:29f7f535d4b2 | 588 | { |
Dyotty | 5:29f7f535d4b2 | 589 | left = 0; |
Dyotty | 5:29f7f535d4b2 | 590 | right = 1.0; |
Dyotty | 5:29f7f535d4b2 | 591 | } |
Dyotty | 5:29f7f535d4b2 | 592 | else |
Dyotty | 5:29f7f535d4b2 | 593 | { |
Dyotty | 5:29f7f535d4b2 | 594 | stt_Mode = 1; |
Dyotty | 5:29f7f535d4b2 | 595 | pc.printf(" 6 -> 1 \n"); |
Dyotty | 9:965bfb378463 | 596 | // |
Dyotty | 9:965bfb378463 | 597 | // for(int i = 0; i < 3; i++) |
Dyotty | 9:965bfb378463 | 598 | // { |
Dyotty | 9:965bfb378463 | 599 | // buf_ax[i] = ax; |
Dyotty | 9:965bfb378463 | 600 | // } |
Dyotty | 9:965bfb378463 | 601 | } |
Dyotty | 8:102c19510dce | 602 | |
Dyotty | 5:29f7f535d4b2 | 603 | |
Keisuke_Yoshida | 4:951fabc31ba6 | 604 | break; |
Keisuke_Yoshida | 4:951fabc31ba6 | 605 | default: |
Keisuke_Yoshida | 4:951fabc31ba6 | 606 | break; |
Keisuke_Yoshida | 4:951fabc31ba6 | 607 | } |
Keisuke_Yoshida | 4:951fabc31ba6 | 608 | #endif |
Keisuke_Yoshida | 4:951fabc31ba6 | 609 | |
Dyotty | 7:897b9ded86e2 | 610 | cnt_loop++; |
Dyotty | 7:897b9ded86e2 | 611 | |
Dyotty | 7:897b9ded86e2 | 612 | if(cnt_loop > 2000) |
Dyotty | 7:897b9ded86e2 | 613 | { |
Dyotty | 7:897b9ded86e2 | 614 | cnt_loop = 0; |
Dyotty | 7:897b9ded86e2 | 615 | } |
jksoft | 0:76dfa9657d9d | 616 | } |
jksoft | 0:76dfa9657d9d | 617 | ModeLed = 1; |
jksoft | 0:76dfa9657d9d | 618 | left = 0.0; |
jksoft | 0:76dfa9657d9d | 619 | right = 0.0; |
jksoft | 0:76dfa9657d9d | 620 | wait(1); |
jksoft | 0:76dfa9657d9d | 621 | } |
sibu2 | 1:a86c84ead24a | 622 | |
sibu2 | 1:a86c84ead24a | 623 | #if 0 |
jksoft | 0:76dfa9657d9d | 624 | int counter1 = 0; |
jksoft | 0:76dfa9657d9d | 625 | void p1_rise() |
jksoft | 0:76dfa9657d9d | 626 | { |
jksoft | 0:76dfa9657d9d | 627 | if( pin2 == 1 ) |
jksoft | 0:76dfa9657d9d | 628 | { |
jksoft | 0:76dfa9657d9d | 629 | counter1++; |
jksoft | 0:76dfa9657d9d | 630 | } |
jksoft | 0:76dfa9657d9d | 631 | else |
jksoft | 0:76dfa9657d9d | 632 | { |
jksoft | 0:76dfa9657d9d | 633 | counter1--; |
jksoft | 0:76dfa9657d9d | 634 | } |
jksoft | 0:76dfa9657d9d | 635 | } |
sibu2 | 1:a86c84ead24a | 636 | #endif |
sibu2 | 1:a86c84ead24a | 637 | |
jksoft | 0:76dfa9657d9d | 638 | /**************************************************************************/ |
jksoft | 0:76dfa9657d9d | 639 | /*! |
jksoft | 0:76dfa9657d9d | 640 | @brief Program entry point |
jksoft | 0:76dfa9657d9d | 641 | */ |
jksoft | 0:76dfa9657d9d | 642 | /**************************************************************************/ |
jksoft | 0:76dfa9657d9d | 643 | int main(void) |
jksoft | 0:76dfa9657d9d | 644 | { |
jksoft | 0:76dfa9657d9d | 645 | sw1.mode(PullUp); |
jksoft | 0:76dfa9657d9d | 646 | sw2.mode(PullUp); |
sibu2 | 1:a86c84ead24a | 647 | |
sibu2 | 1:a86c84ead24a | 648 | encl1.mode(PullNone); |
sibu2 | 1:a86c84ead24a | 649 | encl2.mode(PullNone); |
sibu2 | 1:a86c84ead24a | 650 | encr1.mode(PullNone); |
sibu2 | 1:a86c84ead24a | 651 | encr2.mode(PullNone); |
sibu2 | 1:a86c84ead24a | 652 | |
jksoft | 0:76dfa9657d9d | 653 | ModeLed = 1; |
jksoft | 0:76dfa9657d9d | 654 | ConnectStateLed = 1; |
jksoft | 0:76dfa9657d9d | 655 | #if DBG |
jksoft | 0:76dfa9657d9d | 656 | //pc.baud(921600); |
jksoft | 0:76dfa9657d9d | 657 | pc.baud(9600); |
jksoft | 0:76dfa9657d9d | 658 | pc.printf("Start\n\r"); |
jksoft | 0:76dfa9657d9d | 659 | #endif |
jksoft | 0:76dfa9657d9d | 660 | outlow = 0; |
jksoft | 0:76dfa9657d9d | 661 | |
jksoft | 0:76dfa9657d9d | 662 | if(sw2 == 0) |
jksoft | 0:76dfa9657d9d | 663 | { |
sibu2 | 1:a86c84ead24a | 664 | // pin1.mode(PullDown); |
jksoft | 0:76dfa9657d9d | 665 | // pin1.rise(&p1_rise); |
jksoft | 0:76dfa9657d9d | 666 | while(1) |
jksoft | 0:76dfa9657d9d | 667 | { |
jksoft | 0:76dfa9657d9d | 668 | //int in1 = pin1; |
jksoft | 0:76dfa9657d9d | 669 | //int in2 = pin2; |
jksoft | 0:76dfa9657d9d | 670 | //ModeLed = pin1; |
jksoft | 0:76dfa9657d9d | 671 | //pc.printf("dat = %d %d\r\n",in1,in2); |
jksoft | 0:76dfa9657d9d | 672 | base(); |
jksoft | 0:76dfa9657d9d | 673 | #if 0 |
jksoft | 0:76dfa9657d9d | 674 | left = 1.0; |
jksoft | 0:76dfa9657d9d | 675 | right = 1.0; |
jksoft | 0:76dfa9657d9d | 676 | wait(5); |
jksoft | 0:76dfa9657d9d | 677 | left = -1.0; |
jksoft | 0:76dfa9657d9d | 678 | right = -1.0; |
jksoft | 0:76dfa9657d9d | 679 | wait(5); |
jksoft | 0:76dfa9657d9d | 680 | #endif |
jksoft | 0:76dfa9657d9d | 681 | } |
jksoft | 0:76dfa9657d9d | 682 | } |
jksoft | 0:76dfa9657d9d | 683 | |
Dyotty | 6:213041054f97 | 684 | // // MPU6050 Initialize |
Dyotty | 6:213041054f97 | 685 | // mpu.initialize(); |
Dyotty | 6:213041054f97 | 686 | // mpu.setAcceleroRange(MPU6050_ACCELERO_RANGE_8G); |
Dyotty | 6:213041054f97 | 687 | // mpu.setGyroRange(MPU6050_GYRO_RANGE_1000); |
Dyotty | 6:213041054f97 | 688 | |
Dyotty | 6:213041054f97 | 689 | ///180601 MPU6050センサの初期化処理 |
Dyotty | 3:f707552ec50a | 690 | mpu.initialize(); |
Dyotty | 6:213041054f97 | 691 | // mpu.setAcceleroRange(MPU6050_ACCELERO_RANGE_8G); //加速度センサ 計測レンジ設定(今回は2Gか4Gがよさそう) |
Dyotty | 6:213041054f97 | 692 | mpu.setAcceleroRange(MPU6050_ACCELERO_RANGE_2G); |
Dyotty | 6:213041054f97 | 693 | // mpu.setGyroRange(MPU6050_GYRO_RANGE_1000); //ジャイロセンサ 計測レンジ設定(ここも250か500がよさそう(そんなに早く回転しないので)) |
Dyotty | 6:213041054f97 | 694 | mpu.setGyroRange(MPU6050_GYRO_RANGE_250); |
Dyotty | 6:213041054f97 | 695 | ///180601 |
Dyotty | 3:f707552ec50a | 696 | |
jksoft | 0:76dfa9657d9d | 697 | ble.init(); |
jksoft | 0:76dfa9657d9d | 698 | ble.onConnection(onConnected); |
jksoft | 0:76dfa9657d9d | 699 | ble.onDisconnection(onDisconnected); |
jksoft | 0:76dfa9657d9d | 700 | ble.onDataWritten(onDataWritten); |
jksoft | 0:76dfa9657d9d | 701 | |
jksoft | 0:76dfa9657d9d | 702 | /* setup advertising */ |
jksoft | 0:76dfa9657d9d | 703 | ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED); |
jksoft | 0:76dfa9657d9d | 704 | ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED); |
jksoft | 0:76dfa9657d9d | 705 | ble.accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME, |
jksoft | 0:76dfa9657d9d | 706 | (const uint8_t *)"mbed WallbotBLE", sizeof("mbed WallbotBLE") - 1); |
jksoft | 0:76dfa9657d9d | 707 | ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, |
jksoft | 0:76dfa9657d9d | 708 | (const uint8_t *)RCBController_service_uuid, sizeof(RCBController_service_uuid)); |
jksoft | 0:76dfa9657d9d | 709 | |
jksoft | 0:76dfa9657d9d | 710 | ble.setAdvertisingInterval(160); /* 100ms; in multiples of 0.625ms. */ |
jksoft | 0:76dfa9657d9d | 711 | ble.startAdvertising(); |
jksoft | 0:76dfa9657d9d | 712 | |
jksoft | 0:76dfa9657d9d | 713 | ble.addService(RCBControllerService); |
jksoft | 0:76dfa9657d9d | 714 | |
Dyotty | 5:29f7f535d4b2 | 715 | |
jksoft | 0:76dfa9657d9d | 716 | while (true) { |
jksoft | 0:76dfa9657d9d | 717 | if(sw1 == 0) |
jksoft | 0:76dfa9657d9d | 718 | { |
jksoft | 0:76dfa9657d9d | 719 | bValue = 1; |
jksoft | 0:76dfa9657d9d | 720 | line_mode = 1; |
Keisuke_Yoshida | 4:951fabc31ba6 | 721 | //line(); |
Keisuke_Yoshida | 4:951fabc31ba6 | 722 | wb_control(); //動作モード関数 |
jksoft | 0:76dfa9657d9d | 723 | line_mode = 0; |
jksoft | 0:76dfa9657d9d | 724 | bValue = 0; |
jksoft | 0:76dfa9657d9d | 725 | } |
jksoft | 0:76dfa9657d9d | 726 | ble.waitForEvent(); |
jksoft | 0:76dfa9657d9d | 727 | } |
jksoft | 0:76dfa9657d9d | 728 | } |
jksoft | 0:76dfa9657d9d | 729 |