byeongjin kim / Mbed 2 deprecated Hug2Go_ver_2

joystick_bjk

Dependencies:   mbed VL53L1X

main.cpp@2:284491e0f6bf, 2019-09-05 (annotated)

Committer:
Bhoney
Date:
Thu Sep 05 10:02:40 2019 +0000
Revision:
2:284491e0f6bf
Parent:
1:fd1e7e2d0780
Child:
3:cfb7cc54a3ba
190905 (2channel bt)

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