Diff: main.cpp

Revision:

19:67584cb64b9c

Parent:

15:e5e34512a00e

--- a/main.cpp	Thu Jul 04 08:08:18 2019 +0000
+++ b/main.cpp	Thu Aug 22 08:36:15 2019 +0000
@@ -2,13 +2,16 @@
 #include "MotorControl.h"
 #include "ForceRead.h"
 
-Serial              bt(PA_9,PA_10);              // USART1 Bluetooth (Tx, Rx)
-//Serial              pc(PA_2,PA_3);               // USART2 pc (Tx, Rx)
-Serial              pc(USBTX,USBRX);               // USART2 pc (Tx, Rx)
+#define QUALIFYING_MODE
+
+Serial              bt(PA_2,PA_3);              // USART1 Bluetooth (Tx, Rx)
+// Serial              pc(PA_2,PA_3);               // USART2 pc (Tx, Rx)
+//Serial              pc(USBTX,USBRX);               // USART2 pc (Tx, Rx)
 
 InterruptIn         up(PC_8);                    // Button Interrupt Motor control
 InterruptIn         down(PC_9);
 InterruptIn         bt_ml(PC_10);                // Millking action button       
+InterruptIn         nFault(PB_15);               // Motor Driver DRV
 
 DigitalOut          led(PA_5);                   // Operating LED signal 
 
@@ -16,30 +19,159 @@
 
 Timeout             ResetTimer;                  // Reset angle 0 position
 
+#ifdef QUALIFYING_MODE
+Timer               Qtimer;                       // timer for qualifying test
+int                 Qtime;                        // Qtime stores result from Qtimer
+bool                Qflag;                        // Qflag rises when result is stored
+#endif
+DigitalOut          enable(PB_12);
+DigitalOut          nreset(PC_6);   
+PwmOut              p1(PB_13);               
+PwmOut              p2(PB_14);               
+
+
 Ticker              timer0;
 Ticker              timer1;                      // Control Angle By Using Distance Sensor
 Ticker              timer2;                      // Force Sensor
 Ticker              timer3;                      // Millking Action Mode
 //Ticker              timer4;
 
+
+/*----------------------- Global Variables ------------------------*/
+
+char                rxData[7];
+bool                flagRx = 0;
+
 int                 targetDis;                   // Target Of Distance
 int                 milLoop;                     // Number of Millking Action
-//float               errorPrevious;
 
 int                 onewayNum=0;                 // Counting Turning Point
 int                 sysStatus=0;   
 bool                mkOn = 0;
+bool                nFaultFlag = true;
 
-/*----------------------- ReadData Variables ------------------------*/
+float parA = 73.671f;
+float parB = -1*206.25f;
+float parC = 106.43f;
+/*----------------------- Callback Function Prototypes ------------------------*/
+void nFaultLow();
+void nFaultHigh();
+void ButtonSys();
+void ManualMk();
+void OperateLED();
+void ModeSelect(int mode);
+void ReadAng();
+void stop();
+void ReadData();
+void Test();
+/*----------------------- Main function ------------------------*/
+int main()
+{
+    bt.baud(115200);
+//    pc.baud(115200);
+
+
+    #ifdef QUALIFYING_MODE
+    bt.puts("START 190822 Q_MODE\r\n");
+    #else
+    bt.puts("START 190822 Ver.9\r\n");
+    #endif
+    
+//    pc.puts("START 190703 Ver.9\r\n");
+    int modeNum = 0;   
+    int modeMotor = 0;
+
+    char tmpCommand[4]={0,};                                // [ADD] command
+    int rxVal;
+    
+    nFault.mode(PullUp);
+    nFault.fall(&nFaultLow);
+    nFault.rise(&nFaultHigh);
+    up.mode(PullNone);
+    down.mode(PullNone);
+    bt_ml.mode(PullNone);                                   // [ADD] Setup Millking Action Manual Button
+    up.fall(&ButtonSys);                  
+    up.rise(&ButtonSys);                 
+    down.fall(&ButtonSys);
+    down.rise(&ButtonSys);                
+    
+    bt_ml.fall(&ManualMk);                                     // [ADD] Interrupt Millking Action Manual Button    
+    timer0.attach(&OperateLED,0.2);
+
+//  timer4.attach(&Test,0.1);
+
+    bt.attach(&ReadData, Serial::RxIrq);
+   
 
-char                rxData[7];
-bool                flagRx = 0;
+    while(1)
+    {
+        #ifdef QUALIFYING_MODE
+        if(Qflag){
+            Qflag = false;
+            bt.printf("%dus from btn to motor output\r\n",Qtime);
+        }
+        #endif
+     
+        if(!nFaultFlag)
+        {
+            enable = 0;
+            nreset = 0;
+            p1=0;
+            p2=0;
+            wait(0.01f);
+        }
+     
+        if (1 == flagRx)
+        {
+            flagRx = 0;
+            tmpCommand[0] = rxData[0];
+            tmpCommand[1] = rxData[1];
+            tmpCommand[2] = rxData[2];
+            rxVal = atoi(rxData+3);
+            
+            if (0 == strcmp(tmpCommand,"MOD")) 
+            {   
+            bt.puts("<MOD>\r\n");
+                modeNum = rxVal;                    // 1 ~ 6
+                sysStatus = rxVal;                  // Why this value are assigned directly?
+                                                    // assigning sys variable after checking condition is correct progress, i think.
+                ModeSelect(modeNum);
+            }
+            
+            else if (0 == strcmp(tmpCommand,"MOT"))
+            {           
+            bt.puts("<MOT>\r\n");
+                modeMotor = rxVal;
+                MotorTest(modeMotor);
+            }
+            
+            else if (0 == strcmp(tmpCommand,"POS"))
+            {
+            bt.puts("<POS>\r\n");
+                targetDis = rxVal;               
+                timer1.attach(&ControlAng,0.01);
+            }
+            
+            if (0 == strcmp(tmpCommand,"MIL"))
+            {
+            bt.puts("<MIL>\r\n");
+                sysStatus = 5;
+                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;
+   
+            }
+       }
+           
+        
+    }
+    
+}
 
 
 
-float parA =73.671f;
-float parB =-1*206.25f;
-float parC =106.43f;
 
 /*----------------------- Callback Functions ------------------------*/
 
@@ -90,7 +222,6 @@
     }  
 }
 
-
 void stop()
 {
     MotorTest(0);               
@@ -101,12 +232,7 @@
     float d1 = disSensor.read();
     float d2 = parA*d1*d1+parB*d1+parC;
 
-//    original
-//    bt.printf("%1.2f",d2);
-
-//    fixed
     bt.printf("<DIO%1.2f>",d2);
-    
 }
 
 void ModeSelect(int mode)      
@@ -115,12 +241,10 @@
     switch(mode) 
     {
         
-        case 0 :                            //Standard Mode
-         
-        timer1.detach();
-        timer2.detach();
-        timer3.detach();
-
+        case 0 :                            //Standard Mode        
+            timer1.detach();
+            timer2.detach();
+            timer3.detach();
         break;
             
         case 1 :                            //Reset Mode
@@ -136,7 +260,7 @@
             
             MotorTest(2);
             ResetTimer.attach(&stop,0.5f+d2/3.0f);
-            
+
             break;         
         }
             
@@ -163,7 +287,6 @@
     }
 }
 
-
 void OperateLED()
 {
     led =!led;
@@ -171,7 +294,6 @@
 
 void ManualMk()
 {
-    
     if( 1 == sysStatus | 2 == sysStatus)
     {
         bt.puts("Please Wait Moving Anybaro");              //<STA>
@@ -181,12 +303,15 @@
         sysStatus = 6;
         MkActionManual();
     }
-
-
 }
 
 void ButtonSys()
 {
+    #ifdef QUALIFYING_MODE
+    Qtimer.reset();
+    Qtimer.start();
+    Qflag = false;
+    #endif
     if( 1 == sysStatus | 2 == sysStatus)
     {
         bt.puts("Please Wait Moving Anybaro");              //<STA> 또는 ifdef debug 처리?
@@ -197,11 +322,7 @@
         MotorButton();
     }
 }
-//---------------------- testing ------------- 190703 BH
-InterruptIn         nFault(PB_15);               // Motor Driver DRV
-bool nFaultFlag = true;
 
-//this interrupt function might not be necessary. need to be tested.
 void nFaultHigh(){
     nFaultFlag = true;
 }
@@ -210,117 +331,5 @@
     nFaultFlag = false;
 }
 
-DigitalOut                  enable(PB_12);
-DigitalOut                  nreset(PC_6);   
-PwmOut                      p1(PB_13);               
-PwmOut                      p2(PB_14);               
 
-
-//---------------------- end ------------- 190703 BH
-int main()
-{
-    bt.baud(115200);
-    pc.baud(115200);
-    bt.puts("START 190703 Ver.8 \r\n");
-    pc.puts("START 190703 Ver.8 \r\n");
-    int modeNum = 0;   
-    int modeMotor = 0;
-
-    char tmpCommand[4]={0,};                                // [ADD] command
-    int rxVal;
-    
-    //---------------------- testing ------------- 190703 BH
-    nFault.mode(PullUp);
-    nFault.fall(&nFaultLow);
-
-        //this interrupt function might not be necessary. need to be tested.
-        nFault.rise(&nFaultHigh);
-        
-    //---------------------- end ------------- 190703 BH    
-    
-    up.mode(PullNone);
-    down.mode(PullNone);
-    bt_ml.mode(PullNone);                                   // [ADD] Setup Millking Action Manual Button
-    
-    up.fall(&ButtonSys);                  
-    up.rise(&ButtonSys);                 
-    down.fall(&ButtonSys);
-    down.rise(&ButtonSys);                
-    
-    bt_ml.fall(&ManualMk);                                     // [ADD] Interrupt Millking Action Manual Button    
-    
-    timer0.attach(&OperateLED,0.2);
-//  timer4.attach(&Test,0.1);
-    bt.attach(&ReadData, Serial::RxIrq);
-    
-    
-    while(1)
-    {
-     
-    //---------------------- need to be tested ------------- BH
-    if(!nFaultFlag)
-    {
-
-//        test below code (without rise interrupt callback)
-//        nFaultFlag = true;
-        enable = 0;
-        nreset = 0;
-        p1=0;
-        p2=0;
-        wait(0.01f);
-    }
-    //---------------------- end ------------- 190703 BH
-     
-        if (1 == flagRx)
-        {
-            
-            flagRx = 0;
-            tmpCommand[0] = rxData[0];
-            tmpCommand[1] = rxData[1];
-            tmpCommand[2] = rxData[2];
-            rxVal = atoi(rxData+3);
-            
-
-            if (0 == strcmp(tmpCommand,"MOD")) 
-            {   
-                modeNum = rxVal;                    // 1 ~ 6
-                sysStatus = rxVal;                  // Why this value are assigned directly?
-                                                    // assigning sys variable after checking condition is correct progress, i think.
-                ModeSelect(modeNum);
-            }
-            
-            else if (0 == strcmp(tmpCommand,"MOT"))
-            {           
-                modeMotor = rxVal;
-                MotorTest(modeMotor);
-            }
-            
-            else if (0 == strcmp(tmpCommand,"POS"))
-            {
-//                errorPrevious = 0;
-                targetDis = rxVal;
-//                float d1 = disSensor.read();
-//                float d2 = parA*d1*d1+parB*d1+parC;
-//                errorPrevious = targetDis - d2;
-                
-                timer1.attach(&ControlAng,0.01);
-            }
-            
-            if (0 == strcmp(tmpCommand,"MIL"))
-            {
-                sysStatus = 5;
-                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;
-   
-            }
-            
-     
-       }
-           
-        
-    }
-    
-}
\ No newline at end of file
+//---------------------- end -------------
\ No newline at end of file