joystick_bjk

Dependencies:   mbed VL53L1X

Committer:
Haeun
Date:
Fri Sep 06 01:39:40 2019 +0000
Revision:
4:97a11deb0ab7
Parent:
3:cfb7cc54a3ba
Child:
5:fb5a46e37787
Haeun_revised

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