Diff: main.cpp

Revision:

0:cbbc3528eb59

Child:

1:a003b900b810

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Jun 22 12:16:52 2018 +0000
@@ -0,0 +1,259 @@
+#include "mbed.h"
+
+Serial bc(PA_9,PA_10);              // USART1 Bluetooth (Tx, Rx)
+Serial pc(PA_2,PA_3);               // USART2 pc (Tx, Rx)
+
+DigitalOut led(LED2);               // LED 
+DigitalOut enable(PB_12);           // Motor enable
+DigitalOut p1(PB_13);               // Motor dir1 up
+DigitalOut p2(PB_14);               // Motor dir2 down
+
+DigitalIn up(PC_8);                 // Burton Motor control
+DigitalIn down(PC_9);
+
+AnalogIn disSensor(PA_7);           // Distance Sensor
+
+Ticker timer1;                      // 
+Ticker timer2;                      // 
+
+/*------------------ ReadData Variables -------------------*/
+
+uint8_t             rx_buf[8];              // < _ _ _ _ _ _ > 8Byte
+char                RX_command[4] = {0,};   //   _ _ _ 
+char                RX_data[4] =  {0,};     //         _ _ _ 
+char                flag_RX = 0; 
+event_callback_t    Test;
+
+int                 targetDis;
+
+/*------------------ Callback Functions -------------------*/
+
+void ReadData(int events)
+{
+    bc.printf("Read The Data!!\n");
+    
+    
+    if (rx_buf[0] == '<' && rx_buf[7] == '>')
+    {
+        flag_RX = 1;
+        
+        RX_command[0] = rx_buf[1];
+        RX_command[1] = rx_buf[2];
+        RX_command[2] = rx_buf[3];
+        RX_data[0] = rx_buf[4];
+        RX_data[1] = rx_buf[5];
+        RX_data[2] = rx_buf[6];
+    }
+        
+      
+}
+
+void ControlLED(int mode)
+{
+    bc.printf("mode = %d\n",mode);
+    
+    switch (mode)
+    {
+        case 0 :
+        
+        led = 0;
+        
+        break;
+        
+        case 1 :
+        led = 1;
+        
+        break;
+        
+        case 2 :
+        bc.printf("ANYBARO");
+        
+        break;      
+        
+    }
+    
+}
+
+
+void MotorTest(int mode)
+{
+    
+    bc.printf("mode = %d\n",mode);
+    
+    switch (mode)
+    
+    {
+        case 0 : //stop
+    
+            enable = 0;
+
+            break;
+    
+        case 1 : // up
+    
+            enable = 1;
+            p1 = 0;
+            p2 = 1;
+    
+            break;
+    
+        case 2 : // down
+        
+            enable = 1;
+            p1 = 1;
+            p2 = 0;
+        
+            break;       
+            
+    }
+              
+}
+
+void ControlAng()
+{
+    double d1 = disSensor.read();
+    double d2 = 161.89*d1*d1*d1*d1-437.31*d1*d1*d1+440.89*d1*d1-207.4*d1+42.245;
+    
+    bc.printf("%1.3f\n", d1);
+    
+//    int ref_d = targetDis-d2;
+//    
+//    if(ref_d > 0.01)
+//    {
+//            enable = 1;
+//            p1 = 1;
+//            p2 = 0;
+//            
+//    }
+//    
+//    else if(ref_d < -0.01)
+//    {
+//                
+//            enable = 1;
+//            p1 = 0;
+//            p2 = 1;
+//                 
+//    }
+//    
+//    else
+//    {
+//        
+//            enable = 0;
+//            timer1.detach();
+//              
+//    }
+}
+
+void MotorBurton()
+{
+       
+        int a = up.read();
+        int b = down.read();
+        int c = a*2 + b;
+        
+        bc.printf("%d %d %d\n ",a,b,c); 
+        
+        switch(c)
+        {
+            case 0 : //stop
+        
+                enable = 0;
+        
+                break;
+        
+            case 1 : // down
+        
+                enable = 1;
+                p1 = 0;
+                p2 = 1;
+        
+                break;
+        
+            case 2 : // up
+            
+                enable = 1;
+                p1 = 1;
+                p2 = 0;
+            
+                break;
+            
+            case 3 : // stop
+            
+                enable = 1;
+                p1 = 0;
+                p2 = 0;
+            
+                break;
+                
+        }
+       
+            
+    
+}
+
+
+int main()
+{
+    bc.baud(9600);
+    bc.printf("START 180622 Ver. \n");
+    
+    up.mode(PullNone);
+    down.mode(PullNone);
+    
+    int modeLED;
+    int modeMotor;
+    
+    modeLED = 0;
+    modeMotor = 0;
+    
+    timer2.attach(&MotorBurton,0.1);
+    
+    while(1)
+    {
+         
+        bc.read(rx_buf, 8, ReadData , SERIAL_EVENT_RX_COMPLETE, '\n');
+        
+        // Readdata callback // int event = SERIAL_EVENT_RX_COMPLETE //       
+            
+        // analyze the recieved packets
+        
+        if (1 == flag_RX)
+        {
+            flag_RX = 0;
+            
+            if (0 == strcmp(RX_command,"LED"))
+            {
+                bc.printf("LED CONTROL MODE!!");
+                modeLED = atoi(RX_data);
+                ControlLED(modeLED);
+            }
+            
+            else if (0 == strcmp(RX_command,"MOT"))
+            {               
+                
+                bc.printf("MOTOR CONTROL MODE!!");
+                modeMotor = atoi(RX_data);
+                MotorTest(modeMotor);
+                timer1.attach(&ControlAng,0.1);
+                                
+            }
+            
+
+            
+//            else if (0 == strcmp(RX_command,"DIS"))
+//            {               
+//            
+//                bc.printf("MOTOR DISTANCE CONTROL MODE!!");
+//                targetDis = atoi(RX_data);
+//                timer1.attach(&ControlAng,0.1);
+//                            
+//            }
+                   
+                    
+        }    
+        
+        wait_ms(100);     
+        
+    }
+    
+}
\ No newline at end of file