Diff: main.cpp

Revision:

8:a435e7aa7a02

Parent:

7:8fb5513a2231

Child:

9:7d6fa62f9022

--- a/main.cpp	Wed Dec 26 11:04:24 2018 +0000
+++ b/main.cpp	Fri Mar 29 01:29:39 2019 +0000
@@ -9,7 +9,7 @@
 
 InterruptIn         up(PC_8);                    // Button Interrupt Motor control
 InterruptIn         down(PC_9);
-InterruptIn         bt_ml(PC_10);                 // Millking action button       
+InterruptIn         bt_ml(PC_10);                // Millking action button       
 
 DigitalOut          led(LED2);                   // Operating LED signal 
 
@@ -25,6 +25,8 @@
 
 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;
@@ -43,20 +45,25 @@
 void Test()
 {
     float d1 = disSensor.read();
+    float d2 = parA*d1*d1+parB*d1+parC;
 
-    bt.printf("%1.3f\n",d1);
+    bt.printf("%1.3f\n",d2);
 }
 
 void ReadData()
 {
     char inChar;
+    
     static char rxCount = 0;
     static char rxBuf[7];
     
     while(1 == bt.readable())
     {
+        
         inChar = bt.getc();
-     
+        
+        bt.putc(inChar);
+        
         if('<' == inChar)
         {
             rxCount = 1;
@@ -74,6 +81,11 @@
             flagRx = 1;
             memcpy(rxData, rxBuf, 7);
         }
+        else{
+            rxCount = 0;
+            flagRx = 0;
+        }
+            
     }  
 }
 
@@ -109,7 +121,7 @@
 
 void stop()
 {
-    MotorTest(0);               // [ADD] Stop
+    MotorTest(0);               
 }
 
 void ReadAng()
@@ -125,7 +137,7 @@
     
 }
 
-void ModeSelect(int mode)       // [ADD] MODE
+void ModeSelect(int mode)      
 {
     
     switch(mode) 
@@ -140,21 +152,25 @@
         break;
             
         case 1 :                            //Reset Mode
-        
-        timer3.detach();
-        timer2.detach();
-        timer1.detach();
-        onewayNum = 0;
-        milLoop = 0;
-
-        MotorTest(2);
-        ResetTimer.attach(&stop,10);
-        
-        break;         
+        {
+            timer3.detach();
+            timer2.detach();
+            timer1.detach();
+            onewayNum = 0;
+            milLoop = 0;
+            
+            float d1 = disSensor.read();
+            float d2 = parA*d1*d1+parB*d1+parC;
+            
+            MotorTest(2);
+            ResetTimer.attach(&stop,0.5f+d2/3.0f);
+            
+            break;         
+        }
             
         case 2 :                            //Read Force
         
-        timer2.attach(&CalForce,0.05);
+        timer2.attach(&ReadForce,0.05);
 
         break;
 
@@ -213,28 +229,28 @@
 int main()
 {
     bt.baud(115200);
-    pc.baud(115200);
-    bt.puts("START 181203 Ver.6 \n");                       // ifdef debug
+//    pc.baud(115200);
+    bt.puts("START 181228 Ver.6 \n");
 
     int modeNum = 0;   
     int modeLED = 0;
     int modeMotor = 0;
 
-    char tmpCommand[4]={0,};                     // [ADD] command
+    char tmpCommand[4]={0,};                                // [ADD] command
     int rxVal;
     
     up.mode(PullNone);
     down.mode(PullNone);
-    bt_ml.mode(PullNone);                   // [ADD] Setup Millking Action Manual Button
+    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    
+    bt_ml.fall(&ManualMk);                                  // [ADD] Interrupt Millking Action Manual Button    
     
-    timer0.attach(&OperateLED,0.2);
+//    timer0.attach(&OperateLED,0.2);
 //    timer4.attach(&Test,0.1);
     bt.attach(&ReadData, Serial::RxIrq);
     
@@ -244,6 +260,7 @@
      
         if (1 == flagRx)
         {
+            
             flagRx = 0;
             tmpCommand[0] = rxData[0];
             tmpCommand[1] = rxData[1];
@@ -281,11 +298,16 @@
             
             else if (0 == strcmp(tmpCommand,"POS"))
             {
+//                errorPrevious = 0;
                 #ifdef DEBUG
                 bt.puts("\nMOTOR DISTANCE CONTROL MODE!!\n");
                 #endif
                 targetDis = rxVal;
-                timer1.attach(&ControlAng,0.8);
+//                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"))