byeongjin kim / Mbed 2 deprecated Hug2Go_ver_2

joystick_bjk

Dependencies:   mbed VL53L1X

main.cpp@8:27d6afa7cb7c, 2019-09-19 (annotated)

Committer:
Haeun
Date:
Thu Sep 19 14:33:33 2019 +0000
Revision:
8:27d6afa7cb7c
Parent:
6:beddcb25ad4e 
0919

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 0:6f0f41537e2f 70
Bhoney 1:fd1e7e2d0780 71 void rxData(){
Haeun 6:beddcb25ad4e 72 char temp = bt1.getc();
Bhoney 3:cfb7cc54a3ba 73 bt1.putc(0); //convention
Haeun 6:beddcb25ad4e 74 if(temp== '1'||temp== '2'||temp== '3'||temp== '4'||temp== '5'||temp== '6'||temp=='7'){
Haeun 6:beddcb25ad4e 75 rxChar = temp;
Haeun 6:beddcb25ad4e 76 rxFlag = 1;
Haeun 6:beddcb25ad4e 77 }
Bhoney 1:fd1e7e2d0780 78 }
Haeun 6:beddcb25ad4e 79
Haeun 6:beddcb25ad4e 80 #endif
Haeun 6:beddcb25ad4e 81
Haeun 6:beddcb25ad4e 82 #ifdef JOYSTICK_MODE
Haeun 8:27d6afa7cb7c 83 int x_position = 0;
Haeun 8:27d6afa7cb7c 84 int y_position = 0;
Haeun 6:beddcb25ad4e 85
Haeun 6:beddcb25ad4e 86 void rxData(){
Haeun 8:27d6afa7cb7c 87 char temp[5];
Haeun 8:27d6afa7cb7c 88 // int x_temp=0,y_temp=0;
Haeun 8:27d6afa7cb7c 89 if('('==bt1.getc()){
Haeun 8:27d6afa7cb7c 90 for(int i=1;i<6&&temp[i-2]!=',';i++){
Haeun 8:27d6afa7cb7c 91 temp[i-1]=bt1.getc();
Haeun 8:27d6afa7cb7c 92 }
Haeun 8:27d6afa7cb7c 93 x_position=atoi(temp);
Haeun 8:27d6afa7cb7c 94 for(int i=1;i<6&&temp[i-2]!=')';i++){
Haeun 8:27d6afa7cb7c 95 temp[i-1]=bt1.getc();
Haeun 8:27d6afa7cb7c 96 }
Haeun 8:27d6afa7cb7c 97 y_position=atoi(temp);
Haeun 8:27d6afa7cb7c 98 // bt2.printf("X:%d,Y:%d\n",x_temp,y_temp);
Haeun 8:27d6afa7cb7c 99 }
Haeun 6:beddcb25ad4e 100 }
Bhoney 2:284491e0f6bf 101 #endif
Bhoney 0:6f0f41537e2f 102 int main()
Bhoney 0:6f0f41537e2f 103 {
Haeun 4:97a11deb0ab7 104 throttleL = 0;
Haeun 4:97a11deb0ab7 105 throttleR = 0;
Bhoney 2:284491e0f6bf 106 #ifdef BT_MODE
Bhoney 3:cfb7cc54a3ba 107 bt1.baud(UART_BAUD);
Haeun 6:beddcb25ad4e 108 bt1.attach(&rxData, Serial::RxIrq);
Haeun 5:fb5a46e37787 109 #endif
Haeun 6:beddcb25ad4e 110 // #ifdef ToF_MODE
Haeun 4:97a11deb0ab7 111 bt2.baud(UART_BAUD);
Haeun 6:beddcb25ad4e 112 // bt2.attach(&rxData2, Serial::RxIrq);
Haeun 6:beddcb25ad4e 113 // #endif
Bhoney 2:284491e0f6bf 114 int dis1, dis2, dis3;
Bhoney 2:284491e0f6bf 115
Bhoney 2:284491e0f6bf 116 reverseR = 0;
Bhoney 2:284491e0f6bf 117 reverseL = 0;
Bhoney 3:cfb7cc54a3ba 118
Haeun 6:beddcb25ad4e 119 // bool wheelDirection = true;
Haeun 6:beddcb25ad4e 120
Haeun 4:97a11deb0ab7 121
Haeun 6:beddcb25ad4e 122 #ifdef ToF_MODE
Haeun 6:beddcb25ad4e 123 ///// first ToF
Haeun 6:beddcb25ad4e 124 //
Haeun 6:beddcb25ad4e 125 tof1.begin();
Haeun 6:beddcb25ad4e 126 tof1.setDistanceMode(2);
Haeun 6:beddcb25ad4e 127 while(!tof1.newDataReady());
Haeun 6:beddcb25ad4e 128
Haeun 6:beddcb25ad4e 129
Haeun 6:beddcb25ad4e 130 ///// second ToF
Haeun 6:beddcb25ad4e 131
Haeun 6:beddcb25ad4e 132 tof2.begin();
Haeun 6:beddcb25ad4e 133 tof2.setDistanceMode(2);
Haeun 6:beddcb25ad4e 134 while(!tof2.newDataReady());
Haeun 6:beddcb25ad4e 135 //
Haeun 6:beddcb25ad4e 136 // ///// third ToF
Haeun 6:beddcb25ad4e 137 tof3.begin();
Haeun 6:beddcb25ad4e 138 tof3.setDistanceMode(2);
Haeun 6:beddcb25ad4e 139 while(!tof3.newDataReady());
Haeun 6:beddcb25ad4e 140
Haeun 6:beddcb25ad4e 141 #endif
Haeun 6:beddcb25ad4e 142 //return INTEGER
Haeun 6:beddcb25ad4e 143 float speed = MIN_SPEED;
Haeun 8:27d6afa7cb7c 144 bool forward=1;
Haeun 8:27d6afa7cb7c 145 float speed_joystick =0,direction=0;
Bhoney 2:284491e0f6bf 146 while(1) {
Bhoney 2:284491e0f6bf 147
Bhoney 3:cfb7cc54a3ba 148 #ifdef ToF_MODE
Bhoney 3:cfb7cc54a3ba 149
Bhoney 3:cfb7cc54a3ba 150 ///// first ToF
Haeun 6:beddcb25ad4e 151 // tof1.startMeasurement();
Haeun 6:beddcb25ad4e 152 while(!tof1.newDataReady());
Haeun 6:beddcb25ad4e 153 dis1 = tof1.getDistance();
Haeun 6:beddcb25ad4e 154 //
Haeun 6:beddcb25ad4e 155 // tof2.startMeasurement();
Bhoney 3:cfb7cc54a3ba 156 ///// second ToF
Haeun 6:beddcb25ad4e 157 while(!tof2.newDataReady());
Haeun 6:beddcb25ad4e 158 dis2 = tof2.getDistance();
Bhoney 2:284491e0f6bf 159
Haeun 6:beddcb25ad4e 160 //
Haeun 6:beddcb25ad4e 161 // ///// third ToF
Haeun 6:beddcb25ad4e 162 // tof3.startMeasurement();
Haeun 6:beddcb25ad4e 163 while(!tof3.newDataReady());
Haeun 6:beddcb25ad4e 164 dis3 = tof3.getDistance(); //return INTEGER
Haeun 6:beddcb25ad4e 165 // //end of ToF com.
Bhoney 2:284491e0f6bf 166
Bhoney 2:284491e0f6bf 167
Haeun 4:97a11deb0ab7 168
Haeun 4:97a11deb0ab7 169 //bt1.putc(0);
Haeun 4:97a11deb0ab7 170
Bhoney 2:284491e0f6bf 171 #endif
Haeun 6:beddcb25ad4e 172
Haeun 6:beddcb25ad4e 173
Haeun 8:27d6afa7cb7c 174 #ifdef JOYSTICK_MODE
Haeun 8:27d6afa7cb7c 175 bt2.printf("%d|%d|%d\n", dis1, dis2, dis3);
Haeun 8:27d6afa7cb7c 176
Haeun 8:27d6afa7cb7c 177
Haeun 8:27d6afa7cb7c 178 speed_joystick = (float)y_position/250;
Haeun 8:27d6afa7cb7c 179 direction = (float)x_position/2000;
Haeun 8:27d6afa7cb7c 180 //bt2.printf("X:%d,Y:%d\n",x_position,y_position);
Haeun 8:27d6afa7cb7c 181 //bt2.printf("speed:%1.3f,dir:%1.3f\n",speed,direction);
Haeun 8:27d6afa7cb7c 182 if(speed_joystick>0&&forward==0){
Haeun 8:27d6afa7cb7c 183 forward=1;
Haeun 8:27d6afa7cb7c 184 throttleR=0;
Haeun 8:27d6afa7cb7c 185 throttleL=0;
Haeun 8:27d6afa7cb7c 186 wait(TIME_DELAY_FOR_REVERSE);
Haeun 8:27d6afa7cb7c 187 reverseR = 0;
Haeun 8:27d6afa7cb7c 188 reverseL = 0;
Haeun 8:27d6afa7cb7c 189 wait(1.0);
Haeun 8:27d6afa7cb7c 190 }
Haeun 8:27d6afa7cb7c 191 else if(speed_joystick<0&&forward==1){
Haeun 8:27d6afa7cb7c 192 throttleR=0;
Haeun 8:27d6afa7cb7c 193 throttleL=0;
Haeun 8:27d6afa7cb7c 194 forward=0;
Haeun 8:27d6afa7cb7c 195 wait(TIME_DELAY_FOR_REVERSE);
Haeun 8:27d6afa7cb7c 196 reverseR = 1;
Haeun 8:27d6afa7cb7c 197 reverseL = 1;
Haeun 8:27d6afa7cb7c 198 wait(1.0);
Haeun 8:27d6afa7cb7c 199 }
Haeun 8:27d6afa7cb7c 200 if(direction<0){//left
Haeun 8:27d6afa7cb7c 201 throttleR = abs(speed_joystick)+SPEED_CORRECTION+0.01f;
Haeun 8:27d6afa7cb7c 202 throttleL = abs(speed_joystick)-SPEED_CORRECTION+direction;
Haeun 8:27d6afa7cb7c 203 }
Haeun 8:27d6afa7cb7c 204 else{//right
Haeun 8:27d6afa7cb7c 205 throttleR = speed_joystick-direction;
Haeun 8:27d6afa7cb7c 206 throttleL = speed_joystick+SPEED_CORRECTION;
Haeun 8:27d6afa7cb7c 207 }
Haeun 8:27d6afa7cb7c 208
Haeun 6:beddcb25ad4e 209
Haeun 6:beddcb25ad4e 210 #endif
Haeun 6:beddcb25ad4e 211
Haeun 6:beddcb25ad4e 212 #ifdef BUTTON_MODE
Haeun 8:27d6afa7cb7c 213 bt2.printf("%d|%d|%d\n", dis1, dis2, dis3);
Bhoney 1:fd1e7e2d0780 214 if(rxFlag)
Bhoney 1:fd1e7e2d0780 215 {
Haeun 6:beddcb25ad4e 216 rxFlag=0;
Haeun 6:beddcb25ad4e 217
Bhoney 1:fd1e7e2d0780 218 switch(rxChar){
Bhoney 3:cfb7cc54a3ba 219 case '1': // 전진
Haeun 4:97a11deb0ab7 220 if(reverseR==1&&reverseL==1){
Haeun 6:beddcb25ad4e 221 speed=MIN_SPEED;
Haeun 6:beddcb25ad4e 222 throttleR=speed;
Haeun 6:beddcb25ad4e 223 throttleL=speed+SPEED_CORRECTION;
Haeun 6:beddcb25ad4e 224 wait(TIME_DELAY_FOR_REVERSE);
Haeun 6:beddcb25ad4e 225 reverseR = 0;
Haeun 6:beddcb25ad4e 226 reverseL = 0;
Haeun 4:97a11deb0ab7 227 }
Haeun 6:beddcb25ad4e 228
Bhoney 3:cfb7cc54a3ba 229 //need to be delayed
Haeun 6:beddcb25ad4e 230 if(speed <= MAX_SPEED-DAC_ONE_STEP)
Haeun 6:beddcb25ad4e 231 speed = speed + DAC_ONE_STEP;
Haeun 6:beddcb25ad4e 232 else
Haeun 6:beddcb25ad4e 233 speed = MAX_SPEED;
Haeun 6:beddcb25ad4e 234 throttleR = speed;
Haeun 6:beddcb25ad4e 235 throttleL = speed+SPEED_CORRECTION;
Haeun 6:beddcb25ad4e 236 // bt1.printf("Speed : %f\n",throttleR);
Haeun 4:97a11deb0ab7 237 // throttleR = 1.0f;
Haeun 4:97a11deb0ab7 238 // throttleL = 1.0f;
Bhoney 1:fd1e7e2d0780 239 break;
Bhoney 1:fd1e7e2d0780 240
Bhoney 3:cfb7cc54a3ba 241 case '2': // 후진
Haeun 4:97a11deb0ab7 242 if(reverseR==0&&reverseL==0){
Haeun 6:beddcb25ad4e 243 speed = MIN_SPEED;
Haeun 4:97a11deb0ab7 244 throttleR=0;
Haeun 4:97a11deb0ab7 245 throttleL=0;
Haeun 6:beddcb25ad4e 246 wait(TIME_DELAY_FOR_REVERSE);
Haeun 6:beddcb25ad4e 247 reverseR = 1;
Haeun 6:beddcb25ad4e 248 reverseL = 1;
Haeun 4:97a11deb0ab7 249 }
Haeun 6:beddcb25ad4e 250
Bhoney 3:cfb7cc54a3ba 251 //need to be delayed
Haeun 6:beddcb25ad4e 252 if(speed <= MAX_SPEED-DAC_ONE_STEP)
Haeun 6:beddcb25ad4e 253 speed = speed + DAC_ONE_STEP;
Haeun 6:beddcb25ad4e 254 else
Haeun 6:beddcb25ad4e 255 speed = MAX_SPEED;
Haeun 6:beddcb25ad4e 256 throttleR = speed;
Haeun 6:beddcb25ad4e 257 throttleL = speed+SPEED_CORRECTION;
Haeun 6:beddcb25ad4e 258 // throttleR = throttleR + DAC_ONE_STEP;
Haeun 6:beddcb25ad4e 259 // throttleL = throttleL + DAC_ONE_STEP;
Bhoney 3:cfb7cc54a3ba 260 break;
Bhoney 3:cfb7cc54a3ba 261
Bhoney 3:cfb7cc54a3ba 262 case '3': // 좌회전
Haeun 4:97a11deb0ab7 263 if(reverseR==1&&reverseL==1){
Haeun 6:beddcb25ad4e 264 speed = MIN_SPEED;
Haeun 4:97a11deb0ab7 265 throttleR=0;
Haeun 4:97a11deb0ab7 266 throttleL=0;
Haeun 6:beddcb25ad4e 267 wait(TIME_DELAY_FOR_REVERSE);
Haeun 4:97a11deb0ab7 268 reverseR = 0;
Haeun 4:97a11deb0ab7 269 reverseL = 0;
Haeun 4:97a11deb0ab7 270 }
Haeun 6:beddcb25ad4e 271 if(speed > MIN_SPEED+DAC_ONE_STEP)
Haeun 6:beddcb25ad4e 272 speed = speed - DAC_ONE_STEP;
Haeun 6:beddcb25ad4e 273 else
Haeun 6:beddcb25ad4e 274 speed = MIN_SPEED;
Haeun 6:beddcb25ad4e 275 throttleL=speed+SPEED_CORRECTION;
Haeun 6:beddcb25ad4e 276 // throttleR = speed+SPEED_CORRECTION;
Bhoney 3:cfb7cc54a3ba 277 break;
Bhoney 3:cfb7cc54a3ba 278
Haeun 4:97a11deb0ab7 279 case '4': // 우회전
Haeun 4:97a11deb0ab7 280 if(reverseR==1&&reverseL==1){
Haeun 6:beddcb25ad4e 281 speed = MIN_SPEED;
Haeun 4:97a11deb0ab7 282 throttleR=0;
Haeun 4:97a11deb0ab7 283 throttleL=0;
Haeun 6:beddcb25ad4e 284 wait(TIME_DELAY_FOR_REVERSE);
Haeun 4:97a11deb0ab7 285 reverseR = 0;
Haeun 4:97a11deb0ab7 286 reverseL = 0;
Haeun 4:97a11deb0ab7 287 }
Haeun 6:beddcb25ad4e 288 if(speed > MIN_SPEED+DAC_ONE_STEP)
Haeun 6:beddcb25ad4e 289 speed = speed - DAC_ONE_STEP;
Haeun 6:beddcb25ad4e 290 else
Haeun 6:beddcb25ad4e 291 speed = MIN_SPEED;
Haeun 6:beddcb25ad4e 292 throttleR=speed;
Haeun 6:beddcb25ad4e 293 // throttleR=0;
Haeun 6:beddcb25ad4e 294 // throttleL = speed;
Bhoney 1:fd1e7e2d0780 295 break;
Bhoney 1:fd1e7e2d0780 296
Bhoney 3:cfb7cc54a3ba 297 case '5': // 정지
Haeun 6:beddcb25ad4e 298 speed = MIN_SPEED;
Haeun 4:97a11deb0ab7 299 throttleR=0;
Haeun 4:97a11deb0ab7 300 throttleL=0;
Haeun 4:97a11deb0ab7 301 break;
Bhoney 1:fd1e7e2d0780 302
Bhoney 1:fd1e7e2d0780 303 case '6':
Bhoney 3:cfb7cc54a3ba 304 // reverseL = !reverseL;
Haeun 6:beddcb25ad4e 305 if(reverseR==1&&reverseL==1){
Haeun 6:beddcb25ad4e 306 speed = MIN_SPEED;
Haeun 6:beddcb25ad4e 307 throttleR=0;
Haeun 6:beddcb25ad4e 308 throttleL=0;
Haeun 6:beddcb25ad4e 309 wait(TIME_DELAY_FOR_REVERSE);
Haeun 6:beddcb25ad4e 310 reverseR = 0;
Haeun 6:beddcb25ad4e 311 reverseL = 0;
Haeun 6:beddcb25ad4e 312 }
Haeun 6:beddcb25ad4e 313 if(speed <= MAX_SPEED-DAC_ONE_STEP)
Haeun 6:beddcb25ad4e 314 speed = speed + DAC_ONE_STEP;
Haeun 6:beddcb25ad4e 315 else
Haeun 6:beddcb25ad4e 316 speed = MAX_SPEED;
Haeun 6:beddcb25ad4e 317 // throttleL=speed+SPEED_CORRECTION;
Haeun 6:beddcb25ad4e 318 throttleR = speed;
Haeun 4:97a11deb0ab7 319 break;
Haeun 6:beddcb25ad4e 320 case '7':
Haeun 6:beddcb25ad4e 321 if(reverseR==1&&reverseL==1){
Haeun 6:beddcb25ad4e 322 speed = MIN_SPEED;
Haeun 6:beddcb25ad4e 323 throttleR=0;
Haeun 6:beddcb25ad4e 324 throttleL=0;
Haeun 6:beddcb25ad4e 325 wait(TIME_DELAY_FOR_REVERSE);
Haeun 6:beddcb25ad4e 326 reverseR = 0;
Haeun 6:beddcb25ad4e 327 reverseL = 0;
Haeun 6:beddcb25ad4e 328 }
Haeun 6:beddcb25ad4e 329 if(speed <= MAX_SPEED-DAC_ONE_STEP)
Haeun 6:beddcb25ad4e 330 speed = speed + DAC_ONE_STEP;
Haeun 6:beddcb25ad4e 331 else
Haeun 6:beddcb25ad4e 332 speed = MAX_SPEED;
Haeun 6:beddcb25ad4e 333 // throttleR=speed;
Haeun 6:beddcb25ad4e 334 // throttleR=0;
Haeun 6:beddcb25ad4e 335 throttleL = speed;
Haeun 6:beddcb25ad4e 336 break;
Bhoney 1:fd1e7e2d0780 337 default:
Haeun 4:97a11deb0ab7 338 // reverseR = 0;
Haeun 4:97a11deb0ab7 339 // reverseL = 0;
Haeun 4:97a11deb0ab7 340 // throttleL = 0;
Haeun 4:97a11deb0ab7 341 // throttleR = 0;
Haeun 4:97a11deb0ab7 342
Bhoney 2:284491e0f6bf 343 break;
Bhoney 1:fd1e7e2d0780 344
Bhoney 1:fd1e7e2d0780 345 }
Haeun 6:beddcb25ad4e 346
Bhoney 1:fd1e7e2d0780 347 }
Bhoney 2:284491e0f6bf 348 #endif
Bhoney 0:6f0f41537e2f 349 }
Haeun 4:97a11deb0ab7 350 //#endif
Bhoney 0:6f0f41537e2f 351 }