byeongjin kim / Mbed 2 deprecated Hug2Go_ver_2

joystick_bjk

Dependencies:   mbed VL53L1X

main.cpp@5:fb5a46e37787, 2019-09-09 (annotated)

Committer:
Haeun
Date:
Mon Sep 09 07:53:29 2019 +0000
Revision:
5:fb5a46e37787
Parent:
4:97a11deb0ab7
Child:
6:beddcb25ad4e
bt1 only

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