调试OK

app/user/MotorCtrl.c

@@ -1,10 +1,11 @@
-
+
 #include "MotorCtrl.h"
 
 #include "hwCtrl.h"
 #include "PINdef.h"
 #include "appTask.h"
 #include "pfdl.h"
+#include "buzzer.h"
 
 
 uint8_t MotorState[6],MotorStateReal[6];
@@ -12,7 +13,7 @@ uint16_t MotorHallLoc[6],MotorHardStop1[6],MotorHardStop2[6];
 uint8_t MotorLearnState[6];
 uint8_t MotorErr[6];
 uint16_t current1,current2,current3;
-
+uint16_t MotorTarget[6] = {0};
 static uint8_t AutoCalState;
 
 uint8_t EEL_SAVE_REQUIRE_FLAG;
@@ -133,12 +134,21 @@ void MotorValueInit(void)
 void setMotorState(uint8_t motorid,uint8_t act)
 {
     //motorid-=1;
+    uint8_t i;
     if (act <= ACT_XH && motorid < 6)
     {
         MotorState[motorid] = act;
+        if (act!=ACT_NOACT)
+        {
+            for (i = 0; i < 4; i++)
+            {
+                MotorTarget[i] = 0;
+            }
+            
+        }
     }
 }
-uint16_t MotorTarget[6] = {0};
+
 void setMotorTarget(uint8_t motorid,uint16_t target)
 {
     
@@ -156,7 +166,15 @@ void setMotorTarget(uint8_t motorid,uint16_t target)
 
 void StartAutoCal(void)
 {
-    AutoCalState = AUTOCAL_START;
+    //AutoCalState = AUTOCAL_START;
+    uint8_t i;
+    for (i = 0; i < 4; i++)
+    {
+        MotorHardStop1[i] = 0;
+        MotorHardStop2[i] = 0;
+        
+    }
+    
     
 }
 void StopAutoCal(void)
@@ -173,6 +191,22 @@ void StopAutoCal(void)
     }
 }
 
+void FW_act(void)
+{
+    setMotorTarget(MOTOR1,MotorHardStop1[MOTOR1]-100);
+    setMotorTarget(MOTOR2,MotorHardStop1[MOTOR2]-100);
+    setMotorTarget(MOTOR3,MotorHardStop2[MOTOR3]+100);
+    setMotorTarget(MOTOR4,MotorHardStop1[MOTOR4]-100);
+}
+
+void TP_act(void)
+{
+    setMotorTarget(MOTOR1,MotorHardStop2[MOTOR1]+500);//kb
+    setMotorTarget(MOTOR2,MotorHardStop2[MOTOR2]+100);//hg
+    setMotorTarget(MOTOR3,MotorHardStop1[MOTOR3]-100);//zd
+    setMotorTarget(MOTOR4,MotorHardStop2[MOTOR4]+100);//tt
+}
+
 
 
 void AutoCalCtrl(void)
@@ -464,7 +498,7 @@ void MotorMemoryKeyM1Release(void)
         flagMotorMemoryKeyStart = 0;
         for (i = 0; i < 6; i++)
         {
-            
+            buzzer_start();
             if (MotorHardStop1[i]!=0 && MotorHardStop2[i]!=0)
             {
                 MemoryLoc[0][i] = MotorHallLoc[i];
@@ -492,6 +526,7 @@ void MotorMemoryKeyM2Release(void)
     if (flagMotorMemoryKeyStart == 1)
     {
         flagMotorMemoryKeyStart = 0;
+        buzzer_start();
         for (i = 0; i < 6; i++)
         {
             if (MotorHardStop1[i]!=0 && MotorHardStop2[i]!=0)
@@ -521,6 +556,7 @@ void MotorMemoryKeyM3Release(void)
     if (flagMotorMemoryKeyStart == 1)
     {
         flagMotorMemoryKeyStart = 0;
+        buzzer_start();
         for (i = 0; i < 6; i++)
         {
             if (MotorHardStop1[i]!=0 && MotorHardStop2[i]!=0)
@@ -575,19 +611,19 @@ void MotorCtrl(void)//10ms
     if (OC1flag == 1)
     {
         OC1flag = 0;
-        if (MotorState[0] == ACT_XQ)
+        if (MotorState[0] == ACT_XQ && MotorState[1] == ACT_NOACT && MotorErr[0] == 0)
         {
             MotorHardStop1[0] = MotorHallLoc[0];
         }
-        else if (MotorState[0] == ACT_XH)
+        else if (MotorState[0] == ACT_XH && MotorState[1] == ACT_NOACT && MotorErr[0] == 0)
         {
             MotorHardStop2[0] = MotorHallLoc[0];
         }
-        else if (MotorState[1] == ACT_XQ)
+        else if (MotorState[1] == ACT_XQ && MotorState[0] == ACT_NOACT  && MotorErr[1] == 0)
         {
             MotorHardStop1[1] = MotorHallLoc[1];
         }
-        else if (MotorState[1] == ACT_XH)
+        else if (MotorState[1] == ACT_XH && MotorState[0] == ACT_NOACT  && MotorErr[1] == 0)
         {
             MotorHardStop2[1] = MotorHallLoc[1];
         }
@@ -597,19 +633,19 @@ void MotorCtrl(void)//10ms
     if (OC2flag == 1)
     {
         OC2flag = 0;
-        if (MotorState[2] == ACT_XQ)
+        if (MotorState[2] == ACT_XQ && MotorState[3] == ACT_NOACT && MotorErr[2] == 0)
         {
             MotorHardStop1[2] = MotorHallLoc[2];
         }
-        else if (MotorState[2] == ACT_XH)
+        else if (MotorState[2] == ACT_XH && MotorState[3] == ACT_NOACT && MotorErr[2] == 0)
         {
             MotorHardStop2[2] = MotorHallLoc[2];
         }
-        else if (MotorState[3] == ACT_XQ)
+        else if (MotorState[3] == ACT_XQ && MotorState[2] == ACT_NOACT  && MotorErr[3] == 0)
         {
             MotorHardStop1[3] = MotorHallLoc[3];
         }
-        else if (MotorState[3] == ACT_XH)
+        else if (MotorState[3] == ACT_XH && MotorState[2] == ACT_NOACT  && MotorErr[3] == 0)
         {
             MotorHardStop2[3] = MotorHallLoc[3];
         }
@@ -683,10 +719,10 @@ void CurrentDetecte(void)
     current2 = getAdval(ADCH_RLY1);
 
     
-    if (current1 > 30U && OC1flag == 0)
+    if (current1 > 160U && OC1flag == 0)
     {
         OC_Count1++;
-        if (OC_Count1 >= 100)
+        if (OC_Count1 >= 300)
         {
             OC_Count1 = 0;
             OC1flag = 1;
@@ -697,10 +733,10 @@ void CurrentDetecte(void)
         OC_Count1 = 0;
     }
 
-    if (current2 > 30U && OC2flag == 0)
+    if (current2 > 160U && OC2flag == 0)
     {
         OC_Count2++;
-        if (OC_Count2 >= 100)
+        if (OC_Count2 >= 300)
         {
             OC_Count2 = 0;
             OC2flag = 1;

app/user/MotorCtrl.h

@@ -19,7 +19,8 @@ void MotorValueInit(void);
 
 void StartAutoCal(void);
 void StopAutoCal(void);
-
+void FW_act(void);
+void TP_act(void);
 
 
 

app/user/appTask.c

@@ -125,10 +125,12 @@ void KeyPressLogic(uint8_t keyid)
         setMotorState(MOTOR4,ACT_XH);
         break;
     case KEYID_FW:
-        buzzer_start();
+        //buzzer_start();
+        TP_act();
         break;
     case KEYID_TP:
-        buzzer_start();
+        //buzzer_start();
+        FW_act();
         break;
     case KEYID_JY_SET:
         MotorMemoryKeyMMPress();