Diff: main.cpp

Revision:

4:bf4ad2079096

Parent:

3:b79aa7d836fb

Child:

5:6bb52b2a79bf

Child:

6:549177f76f8e

diff -r b79aa7d836fb -r bf4ad2079096 main.cpp
--- a/main.cpp	Fri Jul 20 02:50:24 2018 +0000
+++ b/main.cpp	Fri Dec 21 20:39:24 2018 +0000
@@ -1,295 +1,290 @@
 #include "mbed.h"
+#include "MotorControl.h"
+#include "ForceRead.h"
 
-Serial bc(PA_9,PA_10);              // USART1 Bluetooth (Tx, Rx)
-Serial pc(PA_2,PA_3);               // USART2 pc (Tx, Rx)
+#define DEBUG
 
-InterruptIn up(PC_8);         // Button Interrupt Motor control
-InterruptIn down(PC_9);
-
+Serial              bt(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
+InterruptIn         up(PC_8);                    // Button Interrupt Motor control
+InterruptIn         down(PC_9);
+InterruptIn         bt_ml(PC_10);                 // Millking action button       
+
+DigitalOut          led(LED2);                   // Operating LED signal 
+
+AnalogIn            disSensor(PA_7);             // Distance Sensor
 
-AnalogIn disSensor(PA_7);           // Distance Sensor
+Timeout             ResetTimer;                  // Reset angle 0 position
+
+Ticker              timer0;                      // Operating MCU LED on 
+Ticker              timer1;                      // Control Angle By Using Distance Sensor
+Ticker              timer2;                      // Force Sensor
+Ticker              timer3;                      // Millking Action Mode
 
-AnalogIn force_L1(PB_0);            // Force Sensor
-AnalogIn force_L2(PB_1);
-AnalogIn force_L3(PC_0);            // Force Sensor
-AnalogIn force_L4(PC_1);
-AnalogIn force_R1(PC_2);            // Force Sensor
-AnalogIn force_R2(PC_3);
-AnalogIn force_R3(PC_4);            // Force Sensor
-AnalogIn force_R4(PC_5);
+int                 targetDis;                   // Target Of Distance
+int                 milLoop;                     // Number of Millking Action
+int                 onewayNum=0;                 // Counting Turning Point
+int                 sysStatus=0;   
+bool                mkOn = 0;
 
+/*----------------------- ReadData Variables ------------------------*/
 
-Ticker timer0;                      // Operating MCU LED on 
-Ticker timer1;                      // Control Angle By Using Distance Sensor
-Ticker timer2;                      // Force Sensor
+char                rxData[7];
+bool                flagRx = 0;
 
-/*------------------ ReadData Variables -------------------*/
+#define parA 347.44f        // parmeter of equation (values of distance sensor)
+#define parB -1*481.16f
+#define parC 155.42f
 
-uint8_t             rx_buf[8];              // < _ _ _ _ _ _ > 8Byte
-char                RX_command[4] = {0,};   //   _ _ _ 
-char                RX_data[4] =  {0,};     //         _ _ _ 
-char                flag_RX = 0; 
-event_callback_t    Test;
+/*----------------------- Callback Functions ------------------------*/
 
-int                 targetDis;
-
-/*------------------ Callback Functions -------------------*/
-
-void ReadData(int events)
+void ReadData()
 {
-    bc.printf("Read The Data!!\n");
+    char inChar;
+    static char rxCount = 0;
+    static char rxBuf[7];
     
-    
-    if (rx_buf[0] == '<' && rx_buf[7] == '>')
+    while(1 == bt.readable())
     {
-        flag_RX = 1;
+        inChar = bt.getc();
+     
+        if('<' == inChar)
+        {
+            rxCount = 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];
-    }
+        else if (rxCount > 0 && rxCount <8)
+        {
+            rxBuf[rxCount-1] = inChar;
+            rxCount++;            
+        }
         
-      
+        else if ( 8 == rxCount && '>' == inChar)
+        {
+            rxCount = 0;
+            flagRx = 1;
+            memcpy(rxData, rxBuf, 7);
+        }
+    }  
 }
 
+
 void ControlLED(int mode)
 {
-    bc.printf("mode = %d\n",mode);
+
     
     switch (mode)
     {
         case 0 :
-        
+        timer0.detach();
         led = 0;
         
         break;
         
         case 1 :
+        timer0.detach();
         led = 1;
         
         break;
         
         case 2 :
-        bc.printf("ANYBARO");
+        bt.puts("ANYBARO");
         
         break;      
-        
     }
     
 }
 
-
-void MotorTest(int mode)
+void stop()
 {
-    
-    bc.printf("mode = %d\n",mode);
-    
-    switch (mode)
-    
-    {
-        case 0 : //stop
-    
-            enable = 0;
+    MotorTest(0);               // [ADD] Stop
+}
 
-            break;
-    
-        case 1 : // up
-    
-            enable = 1;
-            p1 = 0;
-            p2 = 1;
-    
-            break;
-    
-        case 2 : // down
-        
-            enable = 1;
-            p1 = 1;
-            p2 = 0;
-        
-            break;       
-            
-    }
-    
-              
+void ReadAng()
+{
+    float d1 = disSensor.read();
+    float d2 = parA*d1*d1+parB*d1+parC;
+    bt.printf("%1.2f",d2);
 }
 
-void ControlAng()
+void ModeSelect(int mode)       // [ADD] MODE
 {
+    
+    switch(mode) 
+    {
         
-        float d1 = disSensor.read();
-        float d2 = -840.53f*d1*d1*d1+1742.1f*d1*d1-1262.7f*d1+340.25f;
-//        float d2 = -840.53f*d1*d1*d1+1742.1f*d1*d1-1600.0f*d1+340.25f;
-        float ref_d = targetDis-d2;
-        
-        bc.printf("%1.3f %1.3f %1.3f\n", d1, d2, ref_d);
+        case 0 :                            //Standard Mode
+         
+        timer1.detach();
+        timer2.detach();
+        timer3.detach();
+
+        break;
+            
+        case 1 :                            //Reset Mode
         
-        if(ref_d > 0.9f)
-        {
-                enable = 1;
-                p1 = 0;
-                p2 = 1;
-                
-        }
+        timer3.detach();
+        timer2.detach();
+        timer1.detach();
+        onewayNum = 0;
+        milLoop = 0;
+
+        MotorTest(2);
+        ResetTimer.attach(&stop,10);
+        
+        break;         
+            
+        case 2 :                            //Read Force
         
-        else if(ref_d < -0.9f)
-        {
-                    
-                enable = 1;
-                p1 = 1;
-                p2 = 0;
-                     
-        }
+        timer2.attach(&CalForce,0.1);
+
+        break;
+
+        case 3 :                            //Read Angle
         
-        else
-        {
-            
-                enable = 0;
-                timer1.detach();
-                  
-        }
+        timer2.detach();        
+        ReadAng();
+
+        break;
+     
+        case 4 :                            //Pause Temporarily
+        
+        MotorTest(0);
+        
+        break;
+        
+
+    }
 }
 
-void MotorButton()
-{
-        
-        int a = up.read();
-        int b = down.read();
-        int c = a*2 + b;
-        
-        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;
-                
-        }
-       
-            
-    
-}
-
-void ReadForce()
-{
-    float force_L[4];
-    float force_R[4];
-    
-    force_L[0] = force_L1.read();
-    force_L[1] = force_L2.read();
-    force_L[2] = force_L3.read();
-    force_L[3] = force_L4.read();
-    force_R[0] = force_R1.read();
-    force_R[1] = force_R2.read();
-    force_R[2] = force_R3.read();
-    force_R[3] = force_R4.read();
-    
-//    bc.printf("%1.3f, %1.3f, %1.3f, %1.3f\n", force_L[0],force_L[1],force_L[2],force_L[3]);
-//    bc.printf("%1.3f, %1.3f, %1.3f, %1.3f\n", force_R[0],force_R[1],force_R[2],force_R[3]);
-}
 
 void OperateLED()
 {
     led =!led;
 }
 
+void ManualMk()
+{
+    
+    if( 1 == sysStatus | 2 == sysStatus)
+    {
+        bt.puts("Please Wait Moving Anybaro");
+    }
+    else
+    {
+        sysStatus = 6;
+        MkActionManual();
+    }
+
+
+}
+
+void ButtonSys()
+{
+    if( 1 == sysStatus | 2 == sysStatus)
+    {
+        bt.puts("Please Wait Moving Anybaro");
+    }
+    else
+    {
+        MotorButton();
+    }
+}
+
 int main()
 {
-    bc.baud(9600);
-    bc.printf("START 180622 Ver. \n");
-    led = 1;  
+    bt.baud(115200);
+    pc.baud(115200);
+    bt.puts("START 181203 Ver.6 \n");
+
+    int modeNum = 0;    
+    int modeLED = 0;
+    int modeMotor =0;
+
+    char tmpCommand[4]={0,};                     // [ADD] command
+    int rxVal;
     
     up.mode(PullNone);
     down.mode(PullNone);
-    
-    up.fall(&MotorButton);                  // 1 > 0 swtich on 
-    up.rise(&MotorButton);                  // 0 > 1 switch on
-    down.fall(&MotorButton);
-    down.rise(&MotorButton);
+    bt_ml.mode(PullNone);                   // [ADD] Setup Millking Action Manual Button
     
-    int modeLED;
-    int modeMotor;
+    up.fall(&ButtonSys);                  
+    up.rise(&ButtonSys);                 
+    down.fall(&ButtonSys);
+    down.rise(&ButtonSys);                
     
-    modeLED = 0;
-    modeMotor = 0;
+    bt_ml.fall(&ManualMk);            // [ADD] Interrupt Millking Action Manual Button    
     
-    timer0.attach(&OperateLED,0.5);
-    timer2.attach(&ReadForce,0.05);
+    timer0.attach(&OperateLED,0.2);
     
+    bt.attach(&ReadData, Serial::RxIrq);
     
     
     while(1)
     {
-    
-        bc.read(rx_buf, 8, ReadData , SERIAL_EVENT_RX_COMPLETE, '\n');
-        
-        // Readdata callback // int event = SERIAL_EVENT_RX_COMPLETE //       
+     
+        if (1 == flagRx)
+        {
+            flagRx = 0;
+            tmpCommand[0] = rxData[0];
+            tmpCommand[1] = rxData[1];
+            tmpCommand[2] = rxData[2];
+            rxVal = atoi(rxData+3);
             
-        // analyze the recieved packets
-        
-        if (1 == flag_RX)
-        {
-            flag_RX = 0;
-            
-            if (0 == strcmp(RX_command,"LED"))
+            if (0 == strcmp(tmpCommand,"LED"))
             {
-                bc.printf("LED CONTROL MODE!!");
-                modeLED = atoi(RX_data);
+                #ifdef DEBUG
+                bt.puts("\nLED CONTROL MODE!!\n");
+                #endif
+                modeLED = rxVal;
                 ControlLED(modeLED);
             }
+
+            else if (0 == strcmp(tmpCommand,"MOD")) 
+            {   
+                #ifdef DEBUG
+                bt.puts("\nMODE SELECT!!\n");
+                #endif
+                modeNum = rxVal;
+                sysStatus = rxVal;
+                ModeSelect(modeNum);
+            }
             
-            else if (0 == strcmp(RX_command,"MOT"))
-            {               
-                
-                bc.printf("MOTOR TEST CONTROL MODE!!");
-                modeMotor = atoi(RX_data);
-                MotorTest(modeMotor);              
-                                
+            else if (0 == strcmp(tmpCommand,"MOT"))
+            {   
+                #ifdef DEBUG            
+                bt.puts("\nMOTOR TEST CONTROL MODE!!\n");
+                #endif
+                modeMotor = rxVal;
+                MotorTest(modeMotor);                      
             }
             
-            else if (0 == strcmp(RX_command,"POS"))
+            else if (0 == strcmp(tmpCommand,"POS"))
+            {
+                #ifdef DEBUG
+                bt.puts("\nMOTOR DISTANCE CONTROL MODE!!\n");
+                #endif        
+                targetDis = rxVal;
+                timer1.attach(&ControlAng,0.8);
+            }
+            
+            if (0 == strcmp(tmpCommand,"MIL"))
             {
-                bc.printf("MOTOR DISTANCE CONTROL MODE!!");          
-                targetDis = atoi(RX_data);
-                timer1.attach(&ControlAng,0.1);
-                            
+                sysStatus = 5;
+                #ifdef DEBUG
+                bt.puts("\nMILLKING ACTION MODE!!\n");
+                #endif
+                targetDis = atoi(rxData+5);                                     // [ADD] return 2 characters in the back of 4 characters
+                milLoop = 0.01*(atoi(rxData+3)-atoi(rxData+5));                 // [ADD] return 2 cahracters in the front of 4 characters
+                timer3.attach(&MkAction,0.1);
+                
+                if ( 0 == mkOn) mkOn = 1;
+   
             }
-                   
-                    
-        }    
+            
+     
+       }
            
         
     }