byeongjin kim / Mbed 2 deprecated Hug2Go_ver_2

joystick_bjk

Dependencies:   mbed VL53L1X

main.cpp@7:1f4ef899e43e, 2019-09-20 (annotated)

Committer:
byeongjinkim
Date:
Fri Sep 20 03:53:31 2019 +0000
Revision:
7:1f4ef899e43e
Parent:
6:beddcb25ad4e 
joystick

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Bhoney 0:6f0f41537e2f 1 #include "mbed.h"
Bhoney 1:fd1e7e2d0780 2 #include "VL53L1X.h"
Bhoney 1:fd1e7e2d0780 3 #include "CONFIG.h"
Bhoney 3:cfb7cc54a3ba 4
Bhoney 1:fd1e7e2d0780 5 #define DEBUG
Bhoney 2:284491e0f6bf 6 #define BT_MODE
Bhoney 3:cfb7cc54a3ba 7
Haeun 6:beddcb25ad4e 8 //#define BUTTON_MODE
Haeun 6:beddcb25ad4e 9 #define JOYSTICK_MODE
Bhoney 3:cfb7cc54a3ba 10
Bhoney 3:cfb7cc54a3ba 11
Haeun 6:beddcb25ad4e 12 #define ToF_MODE
Haeun 6:beddcb25ad4e 13 #define ToF
Haeun 6:beddcb25ad4e 14
Bhoney 1:fd1e7e2d0780 15 #ifdef DEBUG
Haeun 4:97a11deb0ab7 16 //DigitalOut led(LED1);
Bhoney 1:fd1e7e2d0780 17 #endif
Bhoney 0:6f0f41537e2f 18
Haeun 6:beddcb25ad4e 19 //reserved pin
Haeun 6:beddcb25ad4e 20 //AnalogIn t1(SPI2_SCK);
Haeun 6:beddcb25ad4e 21 //AnalogIn t2(SPI2_NSS);
Haeun 6:beddcb25ad4e 22 //AnalogIn t3(SPI2_MISO);
Haeun 6:beddcb25ad4e 23 //AnalogIn t4(SPI2_MOSI);
Haeun 6:beddcb25ad4e 24 //AnalogIn t5(SPI3_SCK);
Haeun 6:beddcb25ad4e 25 //AnalogIn t6(SPI3_NSS);
Haeun 6:beddcb25ad4e 26 //AnalogIn t7(SPI3_MISO);
Haeun 6:beddcb25ad4e 27 //AnalogIn t8(SPI3_MOSI);
Haeun 6:beddcb25ad4e 28 //AnalogIn t9(USART3_TX);
Haeun 6:beddcb25ad4e 29 //AnalogIn t10(USART3_RX);
Haeun 6:beddcb25ad4e 30 //AnalogIn t11(ADC2_t);
Haeun 6:beddcb25ad4e 31 //AnalogIn t12(ADC3_1_t);
Haeun 6:beddcb25ad4e 32 //AnalogIn t13(ADC3_2_t);
Haeun 6:beddcb25ad4e 33 //AnalogIn t14(TIM1_t);
Haeun 6:beddcb25ad4e 34 //AnalogIn t15(TIM4_t);
Haeun 6:beddcb25ad4e 35 //AnalogIn t16(TIM9_t);
Haeun 6:beddcb25ad4e 36 //AnalogIn t17(TIM11_t);
Haeun 6:beddcb25ad4e 37
Haeun 6:beddcb25ad4e 38
Haeun 6:beddcb25ad4e 39
Haeun 6:beddcb25ad4e 40
Haeun 6:beddcb25ad4e 41
Haeun 6:beddcb25ad4e 42
Bhoney 3:cfb7cc54a3ba 43 Serial bt1(BLUETOOTH1_TX, BLUETOOTH1_RX); //input (motor control)
Haeun 5:fb5a46e37787 44
Haeun 5:fb5a46e37787 45 #ifdef ToF
Bhoney 3:cfb7cc54a3ba 46 Serial bt2(BLUETOOTH2_TX, BLUETOOTH2_RX); //output (ToF result)
Bhoney 0:6f0f41537e2f 47
Haeun 6:beddcb25ad4e 48 VL53L1X tof1(ToF1_I2C_SDA, ToF1_I2C_SCL); //F303K8(PB_7, PB_6), F072RB(PB_9, PB_8)
Haeun 6:beddcb25ad4e 49 VL53L1X tof2(ToF2_I2C_SDA, ToF2_I2C_SCL);
Haeun 6:beddcb25ad4e 50 VL53L1X tof3(ToF3_I2C_SDA, ToF3_I2C_SCL);
Bhoney 0:6f0f41537e2f 51
Haeun 6:beddcb25ad4e 52 //DigitalOut Xshut1(ToF_XShut_1);
Haeun 6:beddcb25ad4e 53 //DigitalOut Xshut2(ToF_XShut_2);
Haeun 6:beddcb25ad4e 54 //DigitalOut Xshut3(ToF_XShut_3);
Bhoney 1:fd1e7e2d0780 55 #endif
Bhoney 0:6f0f41537e2f 56
Bhoney 1:fd1e7e2d0780 57 DigitalOut reverseR(MOTOR_REVERSE_R);
Bhoney 1:fd1e7e2d0780 58 DigitalOut reverseL(MOTOR_REVERSE_L);
Bhoney 0:6f0f41537e2f 59
Bhoney 1:fd1e7e2d0780 60 AnalogOut throttleR(MOTOR_THROTTLE_R);
Bhoney 1:fd1e7e2d0780 61 AnalogOut throttleL(MOTOR_THROTTLE_L);
Haeun 4:97a11deb0ab7 62
Bhoney 2:284491e0f6bf 63 Timer timer;
Bhoney 2:284491e0f6bf 64 #ifdef BT_MODE
Bhoney 1:fd1e7e2d0780 65 char rxChar = 0;
Bhoney 1:fd1e7e2d0780 66 bool rxFlag = false;
Haeun 6:beddcb25ad4e 67 #endif
Haeun 6:beddcb25ad4e 68
Haeun 6:beddcb25ad4e 69 #ifdef BUTTON_MODE
Bhoney 1:fd1e7e2d0780 70 void rxData(){
Haeun 6:beddcb25ad4e 71 char temp = bt1.getc();
Bhoney 3:cfb7cc54a3ba 72 bt1.putc(0); //convention
Haeun 6:beddcb25ad4e 73 if(temp== '1'||temp== '2'||temp== '3'||temp== '4'||temp== '5'||temp== '6'||temp=='7'){
Haeun 6:beddcb25ad4e 74 rxChar = temp;
Haeun 6:beddcb25ad4e 75 rxFlag = 1;
Haeun 6:beddcb25ad4e 76 }
Bhoney 1:fd1e7e2d0780 77 }
Haeun 6:beddcb25ad4e 78 #endif
Haeun 6:beddcb25ad4e 79
Haeun 6:beddcb25ad4e 80 #ifdef JOYSTICK_MODE
Haeun 6:beddcb25ad4e 81 int X_position = 0;
Haeun 6:beddcb25ad4e 82 int Y_position = 0;
Haeun 6:beddcb25ad4e 83
Haeun 6:beddcb25ad4e 84 void rxData(){
byeongjinkim 7:1f4ef899e43e 85 char temp[2] = {32,32};
byeongjinkim 7:1f4ef899e43e 86 if(bt1.readable()){
byeongjinkim 7:1f4ef899e43e 87 for(int i=0; i<2; i++){
byeongjinkim 7:1f4ef899e43e 88 temp[i] = bt1.getc();
byeongjinkim 7:1f4ef899e43e 89 }
byeongjinkim 7:1f4ef899e43e 90 X_position = temp[1];
byeongjinkim 7:1f4ef899e43e 91 Y_position = temp[0];
byeongjinkim 7:1f4ef899e43e 92 }
Haeun 6:beddcb25ad4e 93 }
Bhoney 2:284491e0f6bf 94 #endif
byeongjinkim 7:1f4ef899e43e 95 float map(int in, float inMin, float inMax, float outMin, float outMax) {
byeongjinkim 7:1f4ef899e43e 96 // check it's within the range
byeongjinkim 7:1f4ef899e43e 97 if (inMin<inMax) {
byeongjinkim 7:1f4ef899e43e 98 if (in <= inMin)
byeongjinkim 7:1f4ef899e43e 99 return outMin;
byeongjinkim 7:1f4ef899e43e 100 if (in >= inMax)
byeongjinkim 7:1f4ef899e43e 101 return outMax;
byeongjinkim 7:1f4ef899e43e 102 } else { // cope with input range being backwards.
byeongjinkim 7:1f4ef899e43e 103 if (in >= inMin)
byeongjinkim 7:1f4ef899e43e 104 return outMin;
byeongjinkim 7:1f4ef899e43e 105 if (in <= inMax)
byeongjinkim 7:1f4ef899e43e 106 return outMax;
byeongjinkim 7:1f4ef899e43e 107 }
byeongjinkim 7:1f4ef899e43e 108 // calculate how far into the range we are
byeongjinkim 7:1f4ef899e43e 109 float scale = (in-inMin)/(inMax-inMin);
byeongjinkim 7:1f4ef899e43e 110 // calculate the output.
byeongjinkim 7:1f4ef899e43e 111 return outMin + scale*(outMax-outMin);
byeongjinkim 7:1f4ef899e43e 112 }
Bhoney 0:6f0f41537e2f 113 int main()
Bhoney 0:6f0f41537e2f 114 {
Haeun 4:97a11deb0ab7 115 throttleL = 0;
Haeun 4:97a11deb0ab7 116 throttleR = 0;
Bhoney 2:284491e0f6bf 117 #ifdef BT_MODE
Bhoney 3:cfb7cc54a3ba 118 bt1.baud(UART_BAUD);
Haeun 6:beddcb25ad4e 119 bt1.attach(&rxData, Serial::RxIrq);
Haeun 5:fb5a46e37787 120 #endif
Haeun 6:beddcb25ad4e 121 // #ifdef ToF_MODE
Haeun 4:97a11deb0ab7 122 bt2.baud(UART_BAUD);
Haeun 6:beddcb25ad4e 123 // bt2.attach(&rxData2, Serial::RxIrq);
Haeun 6:beddcb25ad4e 124 // #endif
Bhoney 2:284491e0f6bf 125 int dis1, dis2, dis3;
Bhoney 2:284491e0f6bf 126
Bhoney 2:284491e0f6bf 127 reverseR = 0;
Bhoney 2:284491e0f6bf 128 reverseL = 0;
Bhoney 3:cfb7cc54a3ba 129
Haeun 6:beddcb25ad4e 130 // bool wheelDirection = true;
Haeun 6:beddcb25ad4e 131
Haeun 4:97a11deb0ab7 132
Haeun 6:beddcb25ad4e 133 #ifdef ToF_MODE
Haeun 6:beddcb25ad4e 134 ///// first ToF
Haeun 6:beddcb25ad4e 135 //
Haeun 6:beddcb25ad4e 136 tof1.begin();
Haeun 6:beddcb25ad4e 137 tof1.setDistanceMode(2);
Haeun 6:beddcb25ad4e 138 while(!tof1.newDataReady());
Haeun 6:beddcb25ad4e 139
Haeun 6:beddcb25ad4e 140
Haeun 6:beddcb25ad4e 141 ///// second ToF
Haeun 6:beddcb25ad4e 142
Haeun 6:beddcb25ad4e 143 tof2.begin();
Haeun 6:beddcb25ad4e 144 tof2.setDistanceMode(2);
Haeun 6:beddcb25ad4e 145 while(!tof2.newDataReady());
Haeun 6:beddcb25ad4e 146 //
Haeun 6:beddcb25ad4e 147 // ///// third ToF
Haeun 6:beddcb25ad4e 148 tof3.begin();
Haeun 6:beddcb25ad4e 149 tof3.setDistanceMode(2);
Haeun 6:beddcb25ad4e 150 while(!tof3.newDataReady());
Haeun 6:beddcb25ad4e 151
Haeun 6:beddcb25ad4e 152 #endif
Haeun 6:beddcb25ad4e 153 //return INTEGER
Haeun 6:beddcb25ad4e 154 float speed = MIN_SPEED;
Bhoney 2:284491e0f6bf 155 while(1) {
Bhoney 2:284491e0f6bf 156
Bhoney 3:cfb7cc54a3ba 157 #ifdef ToF_MODE
Bhoney 3:cfb7cc54a3ba 158
Bhoney 3:cfb7cc54a3ba 159 ///// first ToF
Haeun 6:beddcb25ad4e 160 // tof1.startMeasurement();
Haeun 6:beddcb25ad4e 161 while(!tof1.newDataReady());
Haeun 6:beddcb25ad4e 162 dis1 = tof1.getDistance();
Haeun 6:beddcb25ad4e 163 //
Haeun 6:beddcb25ad4e 164 // tof2.startMeasurement();
Bhoney 3:cfb7cc54a3ba 165 ///// second ToF
Haeun 6:beddcb25ad4e 166 while(!tof2.newDataReady());
Haeun 6:beddcb25ad4e 167 dis2 = tof2.getDistance();
Bhoney 2:284491e0f6bf 168
Haeun 6:beddcb25ad4e 169 //
Haeun 6:beddcb25ad4e 170 // ///// third ToF
Haeun 6:beddcb25ad4e 171 // tof3.startMeasurement();
Haeun 6:beddcb25ad4e 172 while(!tof3.newDataReady());
Haeun 6:beddcb25ad4e 173 dis3 = tof3.getDistance(); //return INTEGER
Haeun 6:beddcb25ad4e 174 // //end of ToF com.
Bhoney 2:284491e0f6bf 175
Bhoney 2:284491e0f6bf 176
Haeun 4:97a11deb0ab7 177
Haeun 4:97a11deb0ab7 178 //bt1.putc(0);
Haeun 4:97a11deb0ab7 179
Bhoney 2:284491e0f6bf 180 #endif
Haeun 6:beddcb25ad4e 181 bt2.printf("%d|%d|%d\n", dis1, dis2, dis3);
Haeun 6:beddcb25ad4e 182
Haeun 6:beddcb25ad4e 183 #ifdef JOYSTICK_MODE
byeongjinkim 7:1f4ef899e43e 184
byeongjinkim 7:1f4ef899e43e 185 if (Y_position < 32) {
byeongjinkim 7:1f4ef899e43e 186 // Convert the declining Y-axis readings for going backward from 470 to 0 into 0 to 255 value for the PWM signal for increasing the motor speed
byeongjinkim 7:1f4ef899e43e 187 reverseR = 1;
byeongjinkim 7:1f4ef899e43e 188 reverseL = 1;
byeongjinkim 7:1f4ef899e43e 189 throttleR = map(Y_position, 32, 0, 0, 1);
byeongjinkim 7:1f4ef899e43e 190 throttleL = map(Y_position, 32, 0, 0, 1);
byeongjinkim 7:1f4ef899e43e 191 }
byeongjinkim 7:1f4ef899e43e 192 else if (Y_position > 32) {
byeongjinkim 7:1f4ef899e43e 193 // Convert the increasing Y-axis readings for going forward from 550 to 1023 into 0 to 255 value for the PWM signal for increasing the motor speed
byeongjinkim 7:1f4ef899e43e 194 reverseR = 0;
byeongjinkim 7:1f4ef899e43e 195 reverseL = 0;
byeongjinkim 7:1f4ef899e43e 196 throttleR = map(Y_position, 32, 64, 0, 1);
byeongjinkim 7:1f4ef899e43e 197 throttleL = map(Y_position, 32, 64, 0, 1);
byeongjinkim 7:1f4ef899e43e 198 }
byeongjinkim 7:1f4ef899e43e 199 // If joystick stays in middle the motors are not moving
byeongjinkim 7:1f4ef899e43e 200 else {
byeongjinkim 7:1f4ef899e43e 201 throttleR = 0;
byeongjinkim 7:1f4ef899e43e 202 throttleL = 0;
byeongjinkim 7:1f4ef899e43e 203 }
Haeun 6:beddcb25ad4e 204
byeongjinkim 7:1f4ef899e43e 205 // X-axis used for left and right control
byeongjinkim 7:1f4ef899e43e 206 if (X_position < 32) {
byeongjinkim 7:1f4ef899e43e 207 // Convert the declining X-axis readings from 470 to 0 into increasing 0 to 255 value
byeongjinkim 7:1f4ef899e43e 208 int xMapped = map(X_position, 32, 0, 0, 0.3);
byeongjinkim 7:1f4ef899e43e 209 // Move to left - decrease left motor speed, increase right motor speed
byeongjinkim 7:1f4ef899e43e 210 throttleL = throttleL - xMapped;
byeongjinkim 7:1f4ef899e43e 211 throttleR = throttleR + xMapped;
byeongjinkim 7:1f4ef899e43e 212 // Confine the range from 0 to 255
byeongjinkim 7:1f4ef899e43e 213 if (throttleL < 0) {
byeongjinkim 7:1f4ef899e43e 214 throttleL = 0;
byeongjinkim 7:1f4ef899e43e 215 }
byeongjinkim 7:1f4ef899e43e 216 if (throttleR > 255) {
byeongjinkim 7:1f4ef899e43e 217 throttleR = 255;
byeongjinkim 7:1f4ef899e43e 218 }
byeongjinkim 7:1f4ef899e43e 219 }
byeongjinkim 7:1f4ef899e43e 220 if (X_position > 32) {
byeongjinkim 7:1f4ef899e43e 221 // Convert the increasing X-axis readings from 550 to 1023 into 0 to 255 value
byeongjinkim 7:1f4ef899e43e 222 int xMapped = map(X_position, 32, 64, 0, 0.3);
byeongjinkim 7:1f4ef899e43e 223 // Move right - decrease right motor speed, increase left motor speed
byeongjinkim 7:1f4ef899e43e 224 throttleL = throttleL + xMapped;
byeongjinkim 7:1f4ef899e43e 225 throttleR = throttleR - xMapped;
byeongjinkim 7:1f4ef899e43e 226 // Confine the range from 0 to 255
byeongjinkim 7:1f4ef899e43e 227 if (throttleL > 255) {
byeongjinkim 7:1f4ef899e43e 228 throttleL = 255;
byeongjinkim 7:1f4ef899e43e 229 }
byeongjinkim 7:1f4ef899e43e 230 if (throttleR < 0) {
byeongjinkim 7:1f4ef899e43e 231 throttleR = 0;
byeongjinkim 7:1f4ef899e43e 232 }
byeongjinkim 7:1f4ef899e43e 233 }
Haeun 6:beddcb25ad4e 234
byeongjinkim 7:1f4ef899e43e 235 if (throttleR < 0.4) {
byeongjinkim 7:1f4ef899e43e 236 throttleR = 0;
byeongjinkim 7:1f4ef899e43e 237 }
byeongjinkim 7:1f4ef899e43e 238 if (throttleL < 0.4) {
byeongjinkim 7:1f4ef899e43e 239 throttleL = 0;
byeongjinkim 7:1f4ef899e43e 240 }
Haeun 6:beddcb25ad4e 241 #endif
Haeun 6:beddcb25ad4e 242
Haeun 6:beddcb25ad4e 243 #ifdef BUTTON_MODE
Haeun 6:beddcb25ad4e 244
Bhoney 1:fd1e7e2d0780 245 if(rxFlag)
Bhoney 1:fd1e7e2d0780 246 {
Haeun 6:beddcb25ad4e 247 rxFlag=0;
Haeun 6:beddcb25ad4e 248
Bhoney 1:fd1e7e2d0780 249 switch(rxChar){
Bhoney 3:cfb7cc54a3ba 250 case '1': // 전진
Haeun 4:97a11deb0ab7 251 if(reverseR==1&&reverseL==1){
Haeun 6:beddcb25ad4e 252 speed=MIN_SPEED;
Haeun 6:beddcb25ad4e 253 throttleR=speed;
Haeun 6:beddcb25ad4e 254 throttleL=speed+SPEED_CORRECTION;
Haeun 6:beddcb25ad4e 255 wait(TIME_DELAY_FOR_REVERSE);
Haeun 6:beddcb25ad4e 256 reverseR = 0;
Haeun 6:beddcb25ad4e 257 reverseL = 0;
Haeun 4:97a11deb0ab7 258 }
Haeun 6:beddcb25ad4e 259
Bhoney 3:cfb7cc54a3ba 260 //need to be delayed
Haeun 6:beddcb25ad4e 261 if(speed <= MAX_SPEED-DAC_ONE_STEP)
Haeun 6:beddcb25ad4e 262 speed = speed + DAC_ONE_STEP;
Haeun 6:beddcb25ad4e 263 else
Haeun 6:beddcb25ad4e 264 speed = MAX_SPEED;
Haeun 6:beddcb25ad4e 265 throttleR = speed;
Haeun 6:beddcb25ad4e 266 throttleL = speed+SPEED_CORRECTION;
Haeun 6:beddcb25ad4e 267 // bt1.printf("Speed : %f\n",throttleR);
Haeun 4:97a11deb0ab7 268 // throttleR = 1.0f;
Haeun 4:97a11deb0ab7 269 // throttleL = 1.0f;
Bhoney 1:fd1e7e2d0780 270 break;
Bhoney 1:fd1e7e2d0780 271
Bhoney 3:cfb7cc54a3ba 272 case '2': // 후진
Haeun 4:97a11deb0ab7 273 if(reverseR==0&&reverseL==0){
Haeun 6:beddcb25ad4e 274 speed = MIN_SPEED;
Haeun 4:97a11deb0ab7 275 throttleR=0;
Haeun 4:97a11deb0ab7 276 throttleL=0;
Haeun 6:beddcb25ad4e 277 wait(TIME_DELAY_FOR_REVERSE);
Haeun 6:beddcb25ad4e 278 reverseR = 1;
Haeun 6:beddcb25ad4e 279 reverseL = 1;
Haeun 4:97a11deb0ab7 280 }
Haeun 6:beddcb25ad4e 281
Bhoney 3:cfb7cc54a3ba 282 //need to be delayed
Haeun 6:beddcb25ad4e 283 if(speed <= MAX_SPEED-DAC_ONE_STEP)
Haeun 6:beddcb25ad4e 284 speed = speed + DAC_ONE_STEP;
Haeun 6:beddcb25ad4e 285 else
Haeun 6:beddcb25ad4e 286 speed = MAX_SPEED;
Haeun 6:beddcb25ad4e 287 throttleR = speed;
Haeun 6:beddcb25ad4e 288 throttleL = speed+SPEED_CORRECTION;
Haeun 6:beddcb25ad4e 289 // throttleR = throttleR + DAC_ONE_STEP;
Haeun 6:beddcb25ad4e 290 // throttleL = throttleL + DAC_ONE_STEP;
Bhoney 3:cfb7cc54a3ba 291 break;
Bhoney 3:cfb7cc54a3ba 292
Bhoney 3:cfb7cc54a3ba 293 case '3': // 좌회전
Haeun 4:97a11deb0ab7 294 if(reverseR==1&&reverseL==1){
Haeun 6:beddcb25ad4e 295 speed = MIN_SPEED;
Haeun 4:97a11deb0ab7 296 throttleR=0;
Haeun 4:97a11deb0ab7 297 throttleL=0;
Haeun 6:beddcb25ad4e 298 wait(TIME_DELAY_FOR_REVERSE);
Haeun 4:97a11deb0ab7 299 reverseR = 0;
Haeun 4:97a11deb0ab7 300 reverseL = 0;
Haeun 4:97a11deb0ab7 301 }
Haeun 6:beddcb25ad4e 302 if(speed > MIN_SPEED+DAC_ONE_STEP)
Haeun 6:beddcb25ad4e 303 speed = speed - DAC_ONE_STEP;
Haeun 6:beddcb25ad4e 304 else
Haeun 6:beddcb25ad4e 305 speed = MIN_SPEED;
Haeun 6:beddcb25ad4e 306 throttleL=speed+SPEED_CORRECTION;
Haeun 6:beddcb25ad4e 307 // throttleR = speed+SPEED_CORRECTION;
Bhoney 3:cfb7cc54a3ba 308 break;
Bhoney 3:cfb7cc54a3ba 309
Haeun 4:97a11deb0ab7 310 case '4': // 우회전
Haeun 4:97a11deb0ab7 311 if(reverseR==1&&reverseL==1){
Haeun 6:beddcb25ad4e 312 speed = MIN_SPEED;
Haeun 4:97a11deb0ab7 313 throttleR=0;
Haeun 4:97a11deb0ab7 314 throttleL=0;
Haeun 6:beddcb25ad4e 315 wait(TIME_DELAY_FOR_REVERSE);
Haeun 4:97a11deb0ab7 316 reverseR = 0;
Haeun 4:97a11deb0ab7 317 reverseL = 0;
Haeun 4:97a11deb0ab7 318 }
Haeun 6:beddcb25ad4e 319 if(speed > MIN_SPEED+DAC_ONE_STEP)
Haeun 6:beddcb25ad4e 320 speed = speed - DAC_ONE_STEP;
Haeun 6:beddcb25ad4e 321 else
Haeun 6:beddcb25ad4e 322 speed = MIN_SPEED;
Haeun 6:beddcb25ad4e 323 throttleR=speed;
Haeun 6:beddcb25ad4e 324 // throttleR=0;
Haeun 6:beddcb25ad4e 325 // throttleL = speed;
Bhoney 1:fd1e7e2d0780 326 break;
Bhoney 1:fd1e7e2d0780 327
Bhoney 3:cfb7cc54a3ba 328 case '5': // 정지
Haeun 6:beddcb25ad4e 329 speed = MIN_SPEED;
Haeun 4:97a11deb0ab7 330 throttleR=0;
Haeun 4:97a11deb0ab7 331 throttleL=0;
Haeun 4:97a11deb0ab7 332 break;
Bhoney 1:fd1e7e2d0780 333
Bhoney 1:fd1e7e2d0780 334 case '6':
Bhoney 3:cfb7cc54a3ba 335 // reverseL = !reverseL;
Haeun 6:beddcb25ad4e 336 if(reverseR==1&&reverseL==1){
Haeun 6:beddcb25ad4e 337 speed = MIN_SPEED;
Haeun 6:beddcb25ad4e 338 throttleR=0;
Haeun 6:beddcb25ad4e 339 throttleL=0;
Haeun 6:beddcb25ad4e 340 wait(TIME_DELAY_FOR_REVERSE);
Haeun 6:beddcb25ad4e 341 reverseR = 0;
Haeun 6:beddcb25ad4e 342 reverseL = 0;
Haeun 6:beddcb25ad4e 343 }
Haeun 6:beddcb25ad4e 344 if(speed <= MAX_SPEED-DAC_ONE_STEP)
Haeun 6:beddcb25ad4e 345 speed = speed + DAC_ONE_STEP;
Haeun 6:beddcb25ad4e 346 else
Haeun 6:beddcb25ad4e 347 speed = MAX_SPEED;
Haeun 6:beddcb25ad4e 348 // throttleL=speed+SPEED_CORRECTION;
Haeun 6:beddcb25ad4e 349 throttleR = speed;
Haeun 4:97a11deb0ab7 350 break;
Haeun 6:beddcb25ad4e 351 case '7':
Haeun 6:beddcb25ad4e 352 if(reverseR==1&&reverseL==1){
Haeun 6:beddcb25ad4e 353 speed = MIN_SPEED;
Haeun 6:beddcb25ad4e 354 throttleR=0;
Haeun 6:beddcb25ad4e 355 throttleL=0;
Haeun 6:beddcb25ad4e 356 wait(TIME_DELAY_FOR_REVERSE);
Haeun 6:beddcb25ad4e 357 reverseR = 0;
Haeun 6:beddcb25ad4e 358 reverseL = 0;
Haeun 6:beddcb25ad4e 359 }
Haeun 6:beddcb25ad4e 360 if(speed <= MAX_SPEED-DAC_ONE_STEP)
Haeun 6:beddcb25ad4e 361 speed = speed + DAC_ONE_STEP;
Haeun 6:beddcb25ad4e 362 else
Haeun 6:beddcb25ad4e 363 speed = MAX_SPEED;
Haeun 6:beddcb25ad4e 364 // throttleR=speed;
Haeun 6:beddcb25ad4e 365 // throttleR=0;
Haeun 6:beddcb25ad4e 366 throttleL = speed;
Haeun 6:beddcb25ad4e 367 break;
Bhoney 1:fd1e7e2d0780 368 default:
Haeun 4:97a11deb0ab7 369 // reverseR = 0;
Haeun 4:97a11deb0ab7 370 // reverseL = 0;
Haeun 4:97a11deb0ab7 371 // throttleL = 0;
Haeun 4:97a11deb0ab7 372 // throttleR = 0;
Haeun 4:97a11deb0ab7 373
Bhoney 2:284491e0f6bf 374 break;
Bhoney 1:fd1e7e2d0780 375
Bhoney 1:fd1e7e2d0780 376 }
Haeun 6:beddcb25ad4e 377
Bhoney 1:fd1e7e2d0780 378 }
Bhoney 2:284491e0f6bf 379 #endif
Bhoney 0:6f0f41537e2f 380 }
Haeun 4:97a11deb0ab7 381 //#endif
Bhoney 0:6f0f41537e2f 382 }