han back / Mbed OS CLEO_UART_ULTRASONIC_DC

SMART CLEO Ultrasonic DC

main.cpp@0:36e2fa5ac56d, 2017-12-06 (annotated)

Committer:
SMART_CLEO
Date:
Wed Dec 06 08:51:15 2017 +0000
Revision:
0:36e2fa5ac56d
SMART_CLEO

Who changed what in which revision?

UserRevisionLine numberNew contents of line
SMART_CLEO 0:36e2fa5ac56d 1 #include "mbed.h"
SMART_CLEO 0:36e2fa5ac56d 2 #include "HCSR04.h"
SMART_CLEO 0:36e2fa5ac56d 3
SMART_CLEO 0:36e2fa5ac56d 4 struct UART_buf
SMART_CLEO 0:36e2fa5ac56d 5 {
SMART_CLEO 0:36e2fa5ac56d 6 uint8_t STA;
SMART_CLEO 0:36e2fa5ac56d 7 uint8_t MODE;
SMART_CLEO 0:36e2fa5ac56d 8 uint8_t CMD;
SMART_CLEO 0:36e2fa5ac56d 9 uint8_t LEN;
SMART_CLEO 0:36e2fa5ac56d 10 uint8_t DATA[32];
SMART_CLEO 0:36e2fa5ac56d 11 uint8_t END;
SMART_CLEO 0:36e2fa5ac56d 12
SMART_CLEO 0:36e2fa5ac56d 13 };
SMART_CLEO 0:36e2fa5ac56d 14
SMART_CLEO 0:36e2fa5ac56d 15 // INA, INB
SMART_CLEO 0:36e2fa5ac56d 16 PinName pin_DC[2] = {PA_6, PA_7};
SMART_CLEO 0:36e2fa5ac56d 17 PinName pin_ECHO = PB_7;
SMART_CLEO 0:36e2fa5ac56d 18 PinName pin_TRIG = PB_10;
SMART_CLEO 0:36e2fa5ac56d 19
SMART_CLEO 0:36e2fa5ac56d 20 PwmOut INA(pin_DC[0]);
SMART_CLEO 0:36e2fa5ac56d 21 PwmOut INB(pin_DC[1]);
SMART_CLEO 0:36e2fa5ac56d 22 HCSR04 Ultrasonic(pin_ECHO, pin_TRIG);
SMART_CLEO 0:36e2fa5ac56d 23
SMART_CLEO 0:36e2fa5ac56d 24 Serial SerialUART(PA_2, PA_3);
SMART_CLEO 0:36e2fa5ac56d 25
SMART_CLEO 0:36e2fa5ac56d 26 enum{STOP, CW, CCW};
SMART_CLEO 0:36e2fa5ac56d 27
SMART_CLEO 0:36e2fa5ac56d 28 uint8_t Buffer[37];
SMART_CLEO 0:36e2fa5ac56d 29
SMART_CLEO 0:36e2fa5ac56d 30 UART_buf RX_BUF;
SMART_CLEO 0:36e2fa5ac56d 31
SMART_CLEO 0:36e2fa5ac56d 32 Timer Sensor_Timer;
SMART_CLEO 0:36e2fa5ac56d 33
SMART_CLEO 0:36e2fa5ac56d 34 uint32_t T_period = 1000;
SMART_CLEO 0:36e2fa5ac56d 35 uint32_t timer_check = 0;
SMART_CLEO 0:36e2fa5ac56d 36
SMART_CLEO 0:36e2fa5ac56d 37 void SerialUARTRX_ISR(void);
SMART_CLEO 0:36e2fa5ac56d 38 void DC_Ctrl(uint8_t dir, float Speed);
SMART_CLEO 0:36e2fa5ac56d 39 void Timer_setting(uint8_t cmd, uint8_t value);
SMART_CLEO 0:36e2fa5ac56d 40 void Sensor_Read(void);
SMART_CLEO 0:36e2fa5ac56d 41 float Ultrasonic_cm(void);
SMART_CLEO 0:36e2fa5ac56d 42
SMART_CLEO 0:36e2fa5ac56d 43 int main() {
SMART_CLEO 0:36e2fa5ac56d 44
SMART_CLEO 0:36e2fa5ac56d 45 SerialUART.baud(115200);
SMART_CLEO 0:36e2fa5ac56d 46
SMART_CLEO 0:36e2fa5ac56d 47 SerialUART.attach(&SerialUARTRX_ISR);
SMART_CLEO 0:36e2fa5ac56d 48
SMART_CLEO 0:36e2fa5ac56d 49 Timer_setting(0x06, 1);
SMART_CLEO 0:36e2fa5ac56d 50
SMART_CLEO 0:36e2fa5ac56d 51 while(1)
SMART_CLEO 0:36e2fa5ac56d 52 {
SMART_CLEO 0:36e2fa5ac56d 53 //SerialUART.printf("%d", Sensor_Timer.read_ms());
SMART_CLEO 0:36e2fa5ac56d 54 if((timer_check + T_period) < Sensor_Timer.read_ms())
SMART_CLEO 0:36e2fa5ac56d 55 {
SMART_CLEO 0:36e2fa5ac56d 56 // SerialUART.printf("%d",T_period);
SMART_CLEO 0:36e2fa5ac56d 57 Sensor_Read();
SMART_CLEO 0:36e2fa5ac56d 58 timer_check += T_period;
SMART_CLEO 0:36e2fa5ac56d 59 }
SMART_CLEO 0:36e2fa5ac56d 60 //wait(0.5);
SMART_CLEO 0:36e2fa5ac56d 61 }
SMART_CLEO 0:36e2fa5ac56d 62 }
SMART_CLEO 0:36e2fa5ac56d 63
SMART_CLEO 0:36e2fa5ac56d 64 void SerialUARTRX_ISR(void)
SMART_CLEO 0:36e2fa5ac56d 65 {
SMART_CLEO 0:36e2fa5ac56d 66 static uint8_t RX_count = 0, RX_Len = 32, RX_Status = 0;
SMART_CLEO 0:36e2fa5ac56d 67 uint8_t rx_da = SerialUART.getc();
SMART_CLEO 0:36e2fa5ac56d 68 switch(RX_Status)
SMART_CLEO 0:36e2fa5ac56d 69 {
SMART_CLEO 0:36e2fa5ac56d 70 case 0:
SMART_CLEO 0:36e2fa5ac56d 71 if(rx_da == 0x76)
SMART_CLEO 0:36e2fa5ac56d 72 {
SMART_CLEO 0:36e2fa5ac56d 73 RX_BUF.STA = rx_da;
SMART_CLEO 0:36e2fa5ac56d 74 RX_Status++;
SMART_CLEO 0:36e2fa5ac56d 75 }
SMART_CLEO 0:36e2fa5ac56d 76 break;
SMART_CLEO 0:36e2fa5ac56d 77 case 1:
SMART_CLEO 0:36e2fa5ac56d 78 RX_BUF.MODE = rx_da;
SMART_CLEO 0:36e2fa5ac56d 79 RX_Status++;
SMART_CLEO 0:36e2fa5ac56d 80 break;
SMART_CLEO 0:36e2fa5ac56d 81 case 2:
SMART_CLEO 0:36e2fa5ac56d 82 RX_BUF.CMD = rx_da;
SMART_CLEO 0:36e2fa5ac56d 83 RX_Status++;
SMART_CLEO 0:36e2fa5ac56d 84 break;
SMART_CLEO 0:36e2fa5ac56d 85 case 3:
SMART_CLEO 0:36e2fa5ac56d 86 RX_BUF.LEN = rx_da;
SMART_CLEO 0:36e2fa5ac56d 87 RX_Len = RX_BUF.LEN;
SMART_CLEO 0:36e2fa5ac56d 88 RX_Status++;
SMART_CLEO 0:36e2fa5ac56d 89 if(RX_Len == 0)
SMART_CLEO 0:36e2fa5ac56d 90 RX_Status++;
SMART_CLEO 0:36e2fa5ac56d 91 break;
SMART_CLEO 0:36e2fa5ac56d 92 case 4:
SMART_CLEO 0:36e2fa5ac56d 93 RX_BUF.DATA[RX_count] = rx_da;
SMART_CLEO 0:36e2fa5ac56d 94 RX_count++;
SMART_CLEO 0:36e2fa5ac56d 95 if(RX_count == RX_Len)
SMART_CLEO 0:36e2fa5ac56d 96 {
SMART_CLEO 0:36e2fa5ac56d 97 RX_Status++;
SMART_CLEO 0:36e2fa5ac56d 98 RX_count = 0;
SMART_CLEO 0:36e2fa5ac56d 99 RX_Len = 32;
SMART_CLEO 0:36e2fa5ac56d 100 }
SMART_CLEO 0:36e2fa5ac56d 101 break;
SMART_CLEO 0:36e2fa5ac56d 102 case 5:
SMART_CLEO 0:36e2fa5ac56d 103 if(rx_da == 0x3E)
SMART_CLEO 0:36e2fa5ac56d 104 {
SMART_CLEO 0:36e2fa5ac56d 105 RX_BUF.END = rx_da;
SMART_CLEO 0:36e2fa5ac56d 106 RX_Status = 0;
SMART_CLEO 0:36e2fa5ac56d 107 switch(RX_BUF.MODE)
SMART_CLEO 0:36e2fa5ac56d 108 {
SMART_CLEO 0:36e2fa5ac56d 109 case 0x01:
SMART_CLEO 0:36e2fa5ac56d 110 if(RX_BUF.CMD == 0x12)
SMART_CLEO 0:36e2fa5ac56d 111 {
SMART_CLEO 0:36e2fa5ac56d 112 DC_Ctrl(RX_BUF.DATA[0], 50);
SMART_CLEO 0:36e2fa5ac56d 113 }
SMART_CLEO 0:36e2fa5ac56d 114 break;
SMART_CLEO 0:36e2fa5ac56d 115 case 0x04:
SMART_CLEO 0:36e2fa5ac56d 116 Timer_setting(RX_BUF.CMD, RX_BUF.DATA[0]);
SMART_CLEO 0:36e2fa5ac56d 117 break;
SMART_CLEO 0:36e2fa5ac56d 118 }
SMART_CLEO 0:36e2fa5ac56d 119 }
SMART_CLEO 0:36e2fa5ac56d 120 break;
SMART_CLEO 0:36e2fa5ac56d 121 }
SMART_CLEO 0:36e2fa5ac56d 122 }
SMART_CLEO 0:36e2fa5ac56d 123
SMART_CLEO 0:36e2fa5ac56d 124 void DC_Ctrl(uint8_t dir, float Speed)
SMART_CLEO 0:36e2fa5ac56d 125 {
SMART_CLEO 0:36e2fa5ac56d 126 switch(dir)
SMART_CLEO 0:36e2fa5ac56d 127 {
SMART_CLEO 0:36e2fa5ac56d 128 case STOP:
SMART_CLEO 0:36e2fa5ac56d 129 INA = 0;
SMART_CLEO 0:36e2fa5ac56d 130 INB = 0;
SMART_CLEO 0:36e2fa5ac56d 131 break;
SMART_CLEO 0:36e2fa5ac56d 132 case CW:
SMART_CLEO 0:36e2fa5ac56d 133 INA = Speed/100;
SMART_CLEO 0:36e2fa5ac56d 134 INB = 0;
SMART_CLEO 0:36e2fa5ac56d 135 break;
SMART_CLEO 0:36e2fa5ac56d 136 case CCW:
SMART_CLEO 0:36e2fa5ac56d 137 INA = 0;
SMART_CLEO 0:36e2fa5ac56d 138 INB = Speed/100;
SMART_CLEO 0:36e2fa5ac56d 139 break;
SMART_CLEO 0:36e2fa5ac56d 140 }
SMART_CLEO 0:36e2fa5ac56d 141 }
SMART_CLEO 0:36e2fa5ac56d 142
SMART_CLEO 0:36e2fa5ac56d 143 void Timer_setting(uint8_t cmd, uint8_t value)
SMART_CLEO 0:36e2fa5ac56d 144 {
SMART_CLEO 0:36e2fa5ac56d 145 double Time_value = 0;
SMART_CLEO 0:36e2fa5ac56d 146 Sensor_Timer.stop();
SMART_CLEO 0:36e2fa5ac56d 147 Sensor_Timer.reset();
SMART_CLEO 0:36e2fa5ac56d 148 switch(cmd)
SMART_CLEO 0:36e2fa5ac56d 149 {
SMART_CLEO 0:36e2fa5ac56d 150 case 0x01:
SMART_CLEO 0:36e2fa5ac56d 151 Time_value = 30;
SMART_CLEO 0:36e2fa5ac56d 152 break;
SMART_CLEO 0:36e2fa5ac56d 153 case 0x02:
SMART_CLEO 0:36e2fa5ac56d 154 Time_value = 60;
SMART_CLEO 0:36e2fa5ac56d 155 break;
SMART_CLEO 0:36e2fa5ac56d 156 case 0x03:
SMART_CLEO 0:36e2fa5ac56d 157 Time_value = 120;
SMART_CLEO 0:36e2fa5ac56d 158 break;
SMART_CLEO 0:36e2fa5ac56d 159 case 0x04:
SMART_CLEO 0:36e2fa5ac56d 160 Time_value = 300;
SMART_CLEO 0:36e2fa5ac56d 161 break;
SMART_CLEO 0:36e2fa5ac56d 162 case 0x05:
SMART_CLEO 0:36e2fa5ac56d 163 Time_value = 600;
SMART_CLEO 0:36e2fa5ac56d 164 break;
SMART_CLEO 0:36e2fa5ac56d 165 case 0x06:
SMART_CLEO 0:36e2fa5ac56d 166 Time_value = value;
SMART_CLEO 0:36e2fa5ac56d 167 Time_value = 1.0/Time_value;
SMART_CLEO 0:36e2fa5ac56d 168 break;
SMART_CLEO 0:36e2fa5ac56d 169 }
SMART_CLEO 0:36e2fa5ac56d 170 Sensor_Timer.start();
SMART_CLEO 0:36e2fa5ac56d 171 timer_check = Sensor_Timer.read_ms();
SMART_CLEO 0:36e2fa5ac56d 172 T_period = Time_value * 1000;
SMART_CLEO 0:36e2fa5ac56d 173 }
SMART_CLEO 0:36e2fa5ac56d 174
SMART_CLEO 0:36e2fa5ac56d 175 void Sensor_Read(void)
SMART_CLEO 0:36e2fa5ac56d 176 {
SMART_CLEO 0:36e2fa5ac56d 177 uint16_t dist = Ultrasonic_cm();
SMART_CLEO 0:36e2fa5ac56d 178 Buffer[0] = 0x76;
SMART_CLEO 0:36e2fa5ac56d 179 Buffer[1] = 0x01;
SMART_CLEO 0:36e2fa5ac56d 180 Buffer[2] = 0x07;
SMART_CLEO 0:36e2fa5ac56d 181 Buffer[3] = 0x02;
SMART_CLEO 0:36e2fa5ac56d 182 Buffer[4] = dist >> 8;
SMART_CLEO 0:36e2fa5ac56d 183 Buffer[5] = dist & 0xFF;
SMART_CLEO 0:36e2fa5ac56d 184 Buffer[6] = 0x3E;
SMART_CLEO 0:36e2fa5ac56d 185 for(int i=0; i<7; i++)
SMART_CLEO 0:36e2fa5ac56d 186 SerialUART.putc(Buffer[i]);
SMART_CLEO 0:36e2fa5ac56d 187 }
SMART_CLEO 0:36e2fa5ac56d 188
SMART_CLEO 0:36e2fa5ac56d 189 float Ultrasonic_cm(void)
SMART_CLEO 0:36e2fa5ac56d 190 {
SMART_CLEO 0:36e2fa5ac56d 191 Ultrasonic.startMeasurement();
SMART_CLEO 0:36e2fa5ac56d 192
SMART_CLEO 0:36e2fa5ac56d 193 while(!Ultrasonic.isNewDataReady());
SMART_CLEO 0:36e2fa5ac56d 194
SMART_CLEO 0:36e2fa5ac56d 195 return Ultrasonic.getDistance_cm();
SMART_CLEO 0:36e2fa5ac56d 196 }