Diff: main.cpp

Revision:

10:3fcaf50f528f

Parent:

9:7d6fa62f9022

Child:

11:5f05b14649ee

--- a/main.cpp	Tue Jun 25 02:10:31 2019 +0000
+++ b/main.cpp	Tue Jul 02 04:44:35 2019 +0000
@@ -2,8 +2,6 @@
 #include "MotorControl.h"
 #include "ForceRead.h"
 
-#define DEBUG
-
 Serial              bt(PA_9,PA_10);              // USART1 Bluetooth (Tx, Rx)
 Serial              pc(PA_2,PA_3);               // USART2 pc (Tx, Rx)
 
@@ -17,7 +15,7 @@
 
 Timeout             ResetTimer;                  // Reset angle 0 position
 
-Ticker              timer0;                      // Operating MCU LED on 
+Ticker              timer0;
 Ticker              timer1;                      // Control Angle By Using Distance Sensor
 Ticker              timer2;                      // Force Sensor
 Ticker              timer3;                      // Millking Action Mode
@@ -36,9 +34,11 @@
 char                rxData[7];
 bool                flagRx = 0;
 
-#define parA 343.75f        // parmeter of equation (values of distance sensor)
-#define parB -1*508.08f
-#define parC 166.67f
+
+
+float parA =73.671f;
+float parB =-1*206.25f;
+float parC =106.43f;
 
 /*----------------------- Callback Functions ------------------------*/
 
@@ -90,35 +90,6 @@
 }
 
 
-void ControlLED(int mode)
-{
-
-    
-    switch (mode)
-    {
-        case 0 :
-        timer0.detach();
-        led = 0;
-        
-        break;
-        
-        case 1 :
-        timer0.detach();
-        led = 1;
-        
-        break;
-        
-        case 2 :
-        bt.puts("ANYBARO");
-        
-        break;
-        
-        default:
-            break;
-    }
-    
-}
-
 void stop()
 {
     MotorTest(0);               
@@ -133,7 +104,7 @@
 //    bt.printf("%1.2f",d2);
 
 //    fixed
-    bt.printf("<DIO%1.4f>",d2);
+    bt.printf("<DIO%1.2f>",d2);
     
 }
 
@@ -230,9 +201,8 @@
 {
     bt.baud(115200);
 //    pc.baud(115200);
-    bt.puts("START 181228 Ver.6 \n");
+    bt.puts("START 190627 Ver.8 \n");
     int modeNum = 0;   
-    int modeLED = 0;
     int modeMotor = 0;
 
     char tmpCommand[4]={0,};                                // [ADD] command
@@ -247,10 +217,10 @@
     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);
-//    timer4.attach(&Test,0.1);
+//  timer4.attach(&Test,0.1);
     bt.attach(&ReadData, Serial::RxIrq);
     
     
@@ -266,20 +236,9 @@
             tmpCommand[2] = rxData[2];
             rxVal = atoi(rxData+3);
             
-            if (0 == strcmp(tmpCommand,"LED"))
-            {
-                #ifdef DEBUG
-                bt.puts("\nLED CONTROL MODE!!\n");
-                #endif
-                modeLED = rxVal;                    //  if(modeLED != 0 || modeLED != 1 || modeLED != 2) 
-                ControlLED(modeLED);
-            }
 
-            else if (0 == strcmp(tmpCommand,"MOD")) 
+            if (0 == strcmp(tmpCommand,"MOD")) 
             {   
-                #ifdef DEBUG
-                bt.puts("\nMODE SELECT!!\n");
-                #endif
                 modeNum = rxVal;                    // 1 ~ 6
                 sysStatus = rxVal;                  // Why this value are assigned directly?
                                                     // assigning sys variable after checking condition is correct progress, i think.
@@ -287,10 +246,7 @@
             }
             
             else if (0 == strcmp(tmpCommand,"MOT"))
-            {   
-                #ifdef DEBUG            
-                bt.puts("\nMOTOR TEST CONTROL MODE!!\n");
-                #endif
+            {           
                 modeMotor = rxVal;
                 MotorTest(modeMotor);
             }
@@ -298,9 +254,6 @@
             else if (0 == strcmp(tmpCommand,"POS"))
             {
 //                errorPrevious = 0;
-                #ifdef DEBUG
-                bt.puts("\nMOTOR DISTANCE CONTROL MODE!!\n");
-                #endif
                 targetDis = rxVal;
 //                float d1 = disSensor.read();
 //                float d2 = parA*d1*d1+parB*d1+parC;
@@ -312,9 +265,6 @@
             if (0 == strcmp(tmpCommand,"MIL"))
             {
                 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);