byeongjin kim / Mbed 2 deprecated Hug2Go_ver_2

joystick_bjk

Dependencies:   mbed VL53L1X

main.cpp@3:cfb7cc54a3ba, 2019-09-05 (annotated)

Committer:
Bhoney
Date:
Thu Sep 05 10:24:20 2019 +0000
Revision:
3:cfb7cc54a3ba
Parent:
2:284491e0f6bf
Child:
4:97a11deb0ab7
added ref, bt1 & 2

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
Bhoney 2:284491e0f6bf 8 //#define ToF_MODE
Bhoney 3:cfb7cc54a3ba 9
Bhoney 3:cfb7cc54a3ba 10
Bhoney 1:fd1e7e2d0780 11 #ifdef DEBUG
Bhoney 1:fd1e7e2d0780 12 DigitalOut led(LED1);
Bhoney 1:fd1e7e2d0780 13 #endif
Bhoney 0:6f0f41537e2f 14
Bhoney 3:cfb7cc54a3ba 15 Serial bt1(BLUETOOTH1_TX, BLUETOOTH1_RX); //input (motor control)
Bhoney 3:cfb7cc54a3ba 16 Serial bt2(BLUETOOTH2_TX, BLUETOOTH2_RX); //output (ToF result)
Bhoney 0:6f0f41537e2f 17
Bhoney 1:fd1e7e2d0780 18 #ifdef ToF
Bhoney 1:fd1e7e2d0780 19 VL53L1X tof(ToF_I2C_SDA, ToF_I2C_SCL); //F303K8(PB_7, PB_6), F072RB(PB_9, PB_8)
Bhoney 0:6f0f41537e2f 20
Bhoney 1:fd1e7e2d0780 21 DigitalOut Xshut1(ToF_XShut_1);
Bhoney 1:fd1e7e2d0780 22 DigitalOut Xshut2(ToF_XShut_2);
Bhoney 1:fd1e7e2d0780 23 DigitalOut Xshut3(ToF_XShut_3);
Bhoney 1:fd1e7e2d0780 24 #endif
Bhoney 0:6f0f41537e2f 25
Bhoney 1:fd1e7e2d0780 26 DigitalOut reverseR(MOTOR_REVERSE_R);
Bhoney 1:fd1e7e2d0780 27 DigitalOut reverseL(MOTOR_REVERSE_L);
Bhoney 0:6f0f41537e2f 28
Bhoney 1:fd1e7e2d0780 29 AnalogOut throttleR(MOTOR_THROTTLE_R);
Bhoney 1:fd1e7e2d0780 30 AnalogOut throttleL(MOTOR_THROTTLE_L);
Bhoney 1:fd1e7e2d0780 31
Bhoney 2:284491e0f6bf 32 Timer timer;
Bhoney 2:284491e0f6bf 33 #ifdef BT_MODE
Bhoney 1:fd1e7e2d0780 34 char rxChar = 0;
Bhoney 1:fd1e7e2d0780 35 bool rxFlag = false;
Bhoney 0:6f0f41537e2f 36
Bhoney 1:fd1e7e2d0780 37 void rxData(){
Bhoney 3:cfb7cc54a3ba 38 rxChar = bt1.getc();
Bhoney 3:cfb7cc54a3ba 39 bt1.putc(0); //convention
Bhoney 1:fd1e7e2d0780 40 rxFlag = true;
Bhoney 1:fd1e7e2d0780 41 }
Bhoney 2:284491e0f6bf 42 #endif
Bhoney 0:6f0f41537e2f 43 int main()
Bhoney 0:6f0f41537e2f 44 {
Bhoney 3:cfb7cc54a3ba 45
Bhoney 2:284491e0f6bf 46 #ifdef BT_MODE
Bhoney 3:cfb7cc54a3ba 47 bt1.baud(UART_BAUD);
Bhoney 3:cfb7cc54a3ba 48 bt1.attach(&rxData, Serial::RxIrq);
Bhoney 2:284491e0f6bf 49 #endif
Bhoney 2:284491e0f6bf 50 #ifdef ToF_MODE
Bhoney 2:284491e0f6bf 51 int dis1, dis2, dis3;
Bhoney 2:284491e0f6bf 52
Bhoney 2:284491e0f6bf 53 reverseR = 0;
Bhoney 2:284491e0f6bf 54 reverseL = 0;
Bhoney 3:cfb7cc54a3ba 55
Bhoney 2:284491e0f6bf 56 bool wheelDirection = true;
Bhoney 2:284491e0f6bf 57 #endif
Bhoney 2:284491e0f6bf 58 while(1) {
Bhoney 2:284491e0f6bf 59
Bhoney 3:cfb7cc54a3ba 60 #ifdef ToF_MODE
Bhoney 3:cfb7cc54a3ba 61
Bhoney 3:cfb7cc54a3ba 62 ///// first ToF
Bhoney 2:284491e0f6bf 63 Xshut1 = 1;
Bhoney 2:284491e0f6bf 64 Xshut2 = 0;
Bhoney 2:284491e0f6bf 65 Xshut3 = 0;
Bhoney 2:284491e0f6bf 66
Bhoney 2:284491e0f6bf 67 tof.begin();
Bhoney 2:284491e0f6bf 68 tof.setDistanceMode();
Bhoney 2:284491e0f6bf 69 wait(ToF_SETUP_TIME);
Bhoney 2:284491e0f6bf 70
Bhoney 2:284491e0f6bf 71 dis1 = tof.getDistance();
Bhoney 3:cfb7cc54a3ba 72
Bhoney 2:284491e0f6bf 73
Bhoney 3:cfb7cc54a3ba 74 ///// second ToF
Bhoney 2:284491e0f6bf 75 Xshut1 = 0;
Bhoney 2:284491e0f6bf 76 Xshut2 = 1;
Bhoney 2:284491e0f6bf 77 Xshut3 = 0;
Bhoney 2:284491e0f6bf 78
Bhoney 2:284491e0f6bf 79 tof.begin();
Bhoney 2:284491e0f6bf 80 tof.setDistanceMode();
Bhoney 2:284491e0f6bf 81 wait(ToF_SETUP_TIME);
Bhoney 2:284491e0f6bf 82 dis2 = tof.getDistance();
Bhoney 3:cfb7cc54a3ba 83
Bhoney 2:284491e0f6bf 84
Bhoney 3:cfb7cc54a3ba 85 ///// third ToF
Bhoney 2:284491e0f6bf 86 Xshut1 = 0;
Bhoney 2:284491e0f6bf 87 Xshut2 = 0;
Bhoney 2:284491e0f6bf 88 Xshut3 = 1;
Bhoney 2:284491e0f6bf 89
Bhoney 2:284491e0f6bf 90 tof.begin();
Bhoney 2:284491e0f6bf 91 tof.setDistanceMode();
Bhoney 2:284491e0f6bf 92 wait(ToF_SETUP_TIME);
Bhoney 0:6f0f41537e2f 93
Bhoney 3:cfb7cc54a3ba 94 dis3 = tof.getDistance(); //return INTEGER
Bhoney 3:cfb7cc54a3ba 95 //end of ToF com.
Bhoney 2:284491e0f6bf 96
Bhoney 3:cfb7cc54a3ba 97 bt2.printf("%d|%d|%d\n", dis1, dis2, dis3);
Bhoney 2:284491e0f6bf 98
Bhoney 3:cfb7cc54a3ba 99 // obstacle avoiding
Bhoney 3:cfb7cc54a3ba 100 // if( dis2 > HIGH_BOUND ){
Bhoney 3:cfb7cc54a3ba 101 // if(!wheelDirection){
Bhoney 3:cfb7cc54a3ba 102 // throttleL = 0;
Bhoney 3:cfb7cc54a3ba 103 // throttleR = 0;
Bhoney 3:cfb7cc54a3ba 104 // reverseR = 0;
Bhoney 3:cfb7cc54a3ba 105 // reverseL = 0;
Bhoney 3:cfb7cc54a3ba 106 // wait(1.2f);
Bhoney 3:cfb7cc54a3ba 107 // wheelDirection = true;
Bhoney 3:cfb7cc54a3ba 108 //
Bhoney 3:cfb7cc54a3ba 109 // }
Bhoney 3:cfb7cc54a3ba 110 //
Bhoney 3:cfb7cc54a3ba 111 // throttleL = 0.9f;
Bhoney 3:cfb7cc54a3ba 112 // throttleR = 0.9f;
Bhoney 3:cfb7cc54a3ba 113 // }
Bhoney 3:cfb7cc54a3ba 114 //
Bhoney 3:cfb7cc54a3ba 115 // else if ( dis2 < LOW_BOUND ){
Bhoney 3:cfb7cc54a3ba 116 // if(wheelDirection){
Bhoney 3:cfb7cc54a3ba 117 // throttleL = 0;
Bhoney 3:cfb7cc54a3ba 118 // throttleR = 0;
Bhoney 3:cfb7cc54a3ba 119 //
Bhoney 3:cfb7cc54a3ba 120 // reverseR = 1;
Bhoney 3:cfb7cc54a3ba 121 // reverseL = 1;
Bhoney 3:cfb7cc54a3ba 122 // wait(1.2f);
Bhoney 3:cfb7cc54a3ba 123 // wheelDirection = false;
Bhoney 3:cfb7cc54a3ba 124 // }
Bhoney 3:cfb7cc54a3ba 125 //
Bhoney 3:cfb7cc54a3ba 126 // wait(0.01f);
Bhoney 3:cfb7cc54a3ba 127 //
Bhoney 3:cfb7cc54a3ba 128 // throttleL = 0.9f;
Bhoney 3:cfb7cc54a3ba 129 // throttleR = 0.9f;
Bhoney 3:cfb7cc54a3ba 130 // }
Bhoney 3:cfb7cc54a3ba 131 // else{
Bhoney 3:cfb7cc54a3ba 132 // throttleL = 0;
Bhoney 3:cfb7cc54a3ba 133 // throttleR = 0;
Bhoney 3:cfb7cc54a3ba 134 // }
Bhoney 2:284491e0f6bf 135 #endif
Bhoney 1:fd1e7e2d0780 136
Bhoney 2:284491e0f6bf 137 #ifdef BT_MODE
Bhoney 1:fd1e7e2d0780 138 if(rxFlag)
Bhoney 1:fd1e7e2d0780 139 {
Bhoney 1:fd1e7e2d0780 140 #ifdef DEBUG
Bhoney 3:cfb7cc54a3ba 141 // bt1.puts("get\r\n");
Bhoney 1:fd1e7e2d0780 142 #endif
Bhoney 1:fd1e7e2d0780 143
Bhoney 1:fd1e7e2d0780 144 switch(rxChar){
Bhoney 3:cfb7cc54a3ba 145 case '1': // 전진
Bhoney 3:cfb7cc54a3ba 146 reverseR = 0;
Bhoney 3:cfb7cc54a3ba 147 reverseL = 0;
Bhoney 3:cfb7cc54a3ba 148
Bhoney 3:cfb7cc54a3ba 149 //need to be delayed
Bhoney 3:cfb7cc54a3ba 150
Bhoney 1:fd1e7e2d0780 151 throttleR = throttleR + DAC_ONE_STEP;
Bhoney 3:cfb7cc54a3ba 152 throttleL = throttleL + DAC_ONE_STEP;
Bhoney 1:fd1e7e2d0780 153 break;
Bhoney 1:fd1e7e2d0780 154
Bhoney 3:cfb7cc54a3ba 155 case '2': // 후진
Bhoney 3:cfb7cc54a3ba 156 reverseR = 1;
Bhoney 3:cfb7cc54a3ba 157 reverseL = 1;
Bhoney 3:cfb7cc54a3ba 158
Bhoney 3:cfb7cc54a3ba 159 //need to be delayed
Bhoney 3:cfb7cc54a3ba 160
Bhoney 3:cfb7cc54a3ba 161 throttleR = throttleR - DAC_ONE_STEP;
Bhoney 3:cfb7cc54a3ba 162 throttleL = throttleL + DAC_ONE_STEP;
Bhoney 3:cfb7cc54a3ba 163 break;
Bhoney 3:cfb7cc54a3ba 164
Bhoney 3:cfb7cc54a3ba 165 case '3': // 좌회전
Bhoney 3:cfb7cc54a3ba 166 throttleL = throttleL + DAC_ONE_STEP;
Bhoney 3:cfb7cc54a3ba 167 break;
Bhoney 3:cfb7cc54a3ba 168
Bhoney 3:cfb7cc54a3ba 169 case '4': // 우회진
Bhoney 1:fd1e7e2d0780 170 throttleR = throttleR - DAC_ONE_STEP;
Bhoney 1:fd1e7e2d0780 171 break;
Bhoney 1:fd1e7e2d0780 172
Bhoney 3:cfb7cc54a3ba 173 case '5': // 정지
Bhoney 3:cfb7cc54a3ba 174 // reverseR = !reverseR;
Bhoney 3:cfb7cc54a3ba 175 // break;
Bhoney 1:fd1e7e2d0780 176
Bhoney 1:fd1e7e2d0780 177 case '6':
Bhoney 3:cfb7cc54a3ba 178 // reverseL = !reverseL;
Bhoney 3:cfb7cc54a3ba 179 // break;
Bhoney 1:fd1e7e2d0780 180
Bhoney 1:fd1e7e2d0780 181 default:
Bhoney 1:fd1e7e2d0780 182 reverseR = 0;
Bhoney 1:fd1e7e2d0780 183 reverseL = 0;
Bhoney 1:fd1e7e2d0780 184 throttleL = 0;
Bhoney 2:284491e0f6bf 185 throttleR = 0;
Bhoney 2:284491e0f6bf 186 break;
Bhoney 1:fd1e7e2d0780 187
Bhoney 1:fd1e7e2d0780 188 }
Bhoney 1:fd1e7e2d0780 189 rxFlag = false;
Bhoney 1:fd1e7e2d0780 190 }
Bhoney 2:284491e0f6bf 191 #endif
Bhoney 0:6f0f41537e2f 192 }
Bhoney 2:284491e0f6bf 193 #endif
Bhoney 0:6f0f41537e2f 194 }