byeongjin kim
/
Hug2Go_ver_2
joystick_bjk
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?
User | Revision | Line number | New 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 | } |