Diff: main.cpp

Revision:

0:36e2fa5ac56d

diff -r 000000000000 -r 36e2fa5ac56d main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Dec 06 08:51:15 2017 +0000
@@ -0,0 +1,196 @@
+#include "mbed.h"
+#include "HCSR04.h"
+
+struct UART_buf
+{ 
+    uint8_t STA;
+    uint8_t MODE; 
+    uint8_t CMD;
+    uint8_t LEN;
+    uint8_t DATA[32];
+    uint8_t END; 
+     
+};
+ 
+// INA, INB
+PinName pin_DC[2] = {PA_6, PA_7};
+PinName pin_ECHO = PB_7;
+PinName pin_TRIG = PB_10;
+
+PwmOut INA(pin_DC[0]);
+PwmOut INB(pin_DC[1]);
+HCSR04 Ultrasonic(pin_ECHO, pin_TRIG);
+
+Serial SerialUART(PA_2, PA_3);
+
+enum{STOP, CW, CCW};
+
+uint8_t Buffer[37];
+
+UART_buf RX_BUF;
+
+Timer Sensor_Timer;
+
+uint32_t T_period = 1000;
+uint32_t timer_check = 0;
+
+void SerialUARTRX_ISR(void);
+void DC_Ctrl(uint8_t dir, float Speed);
+void Timer_setting(uint8_t cmd, uint8_t value);
+void Sensor_Read(void);
+float Ultrasonic_cm(void);
+
+int main() {
+    
+    SerialUART.baud(115200);
+    
+    SerialUART.attach(&SerialUARTRX_ISR);
+    
+    Timer_setting(0x06, 1);
+    
+    while(1)
+    {
+        //SerialUART.printf("%d", Sensor_Timer.read_ms());
+        if((timer_check + T_period) < Sensor_Timer.read_ms())
+        {
+          //  SerialUART.printf("%d",T_period);
+            Sensor_Read();
+            timer_check += T_period;
+        }
+        //wait(0.5);
+    }
+}
+
+void SerialUARTRX_ISR(void)
+{
+    static uint8_t RX_count = 0, RX_Len = 32, RX_Status = 0;
+    uint8_t rx_da = SerialUART.getc();
+    switch(RX_Status)
+    {
+        case 0:
+            if(rx_da == 0x76)
+            {
+                RX_BUF.STA = rx_da;
+                RX_Status++;
+            }
+            break;
+        case 1:
+            RX_BUF.MODE = rx_da;
+            RX_Status++;
+            break;
+        case 2:
+            RX_BUF.CMD = rx_da;
+            RX_Status++;
+            break;
+        case 3:
+            RX_BUF.LEN = rx_da;
+            RX_Len = RX_BUF.LEN;
+            RX_Status++;
+            if(RX_Len == 0)
+                RX_Status++;
+            break;
+        case 4:
+            RX_BUF.DATA[RX_count] = rx_da;
+            RX_count++;
+            if(RX_count == RX_Len)
+            {
+                RX_Status++;
+                RX_count = 0;
+                RX_Len = 32;
+            }
+            break;
+        case 5:
+            if(rx_da == 0x3E)
+            {
+                RX_BUF.END = rx_da;
+                RX_Status = 0;
+                switch(RX_BUF.MODE)
+                {
+                    case 0x01:
+                        if(RX_BUF.CMD == 0x12)
+                        {
+                            DC_Ctrl(RX_BUF.DATA[0], 50);
+                        }
+                        break;
+                    case 0x04:
+                        Timer_setting(RX_BUF.CMD, RX_BUF.DATA[0]);
+                        break;
+                }
+            }
+            break;
+    }
+}
+
+void DC_Ctrl(uint8_t dir, float Speed)
+{
+    switch(dir)
+    {
+        case STOP:
+            INA = 0;
+            INB = 0;
+            break;
+        case CW:
+            INA = Speed/100;
+            INB = 0;
+            break;
+        case CCW:
+            INA = 0;
+            INB = Speed/100;
+            break;
+    }
+}
+
+void Timer_setting(uint8_t cmd, uint8_t value)
+{
+    double Time_value = 0;
+    Sensor_Timer.stop();
+    Sensor_Timer.reset();
+    switch(cmd)
+    {
+        case 0x01:
+            Time_value = 30;
+            break;
+        case 0x02:
+            Time_value = 60;
+            break;
+        case 0x03:
+            Time_value = 120;
+            break;
+        case 0x04:
+            Time_value = 300;
+            break;
+        case 0x05:
+            Time_value = 600;
+            break;
+        case 0x06:
+            Time_value = value;
+            Time_value = 1.0/Time_value;
+            break;
+    }
+    Sensor_Timer.start();
+    timer_check = Sensor_Timer.read_ms();
+    T_period = Time_value * 1000;
+}
+
+void Sensor_Read(void)
+{
+    uint16_t dist = Ultrasonic_cm();
+    Buffer[0] = 0x76;
+    Buffer[1] = 0x01;
+    Buffer[2] = 0x07;
+    Buffer[3] = 0x02;
+    Buffer[4] = dist >> 8;
+    Buffer[5] = dist & 0xFF;
+    Buffer[6] = 0x3E;
+    for(int i=0; i<7; i++)
+        SerialUART.putc(Buffer[i]);
+}
+
+float Ultrasonic_cm(void)
+{
+    Ultrasonic.startMeasurement();
+    
+    while(!Ultrasonic.isNewDataReady());
+    
+    return Ultrasonic.getDistance_cm();
+}
\ No newline at end of file