Diff: limit.h

Revision:

4:bf278ddb8504

Parent:

1:7b5469bf5994

--- a/limit.h	Tue Jul 28 01:42:16 2020 +0000
+++ b/limit.h	Tue Dec 08 01:25:06 2020 +0000
@@ -3,6 +3,7 @@
 
 #define limit_debug_print false
 #define start_duty 200
+#define limit_max_power 500
 
 bool limit_find[6]={false,false,false,false,false,false};
 
@@ -68,9 +69,9 @@
     if(limit_count1<-50)
         limit_count1=-50;
         
-    if(limit_count1>30)
+    if(limit_count1>15)
         limit_result1=0;
-    if(limit_count1<-30)
+    if(limit_count1<-15)
         limit_count1=1;
 }
 
@@ -87,9 +88,9 @@
     if(limit_count2<-50)
         limit_count2=-50;
         
-    if(limit_count2>30)
+    if(limit_count2>15)
         limit_result2=0;
-    if(limit_count2<-30)
+    if(limit_count2<-15)
         limit_count2=1;
 }
 
@@ -106,9 +107,9 @@
     if(limit_count3<-50)
         limit_count3=-50;
         
-    if(limit_count3>30)
+    if(limit_count3>15)
         limit_result3=0;
-    if(limit_count3<-30)
+    if(limit_count3<-15)
         limit_count3=1;
 }
 
@@ -126,9 +127,9 @@
     if(limit_count4<-50)
         limit_count4=-50;
         
-    if(limit_count4>30)
+    if(limit_count4>15)
         limit_result4=0;
-    if(limit_count4<-30)
+    if(limit_count4<-15)
         limit_count4=1;
 }
 
@@ -146,9 +147,9 @@
     if(limit_count5<-50)
         limit_count5=-50;
         
-    if(limit_count5>30)
+    if(limit_count5>15)
         limit_result5=0;
-    if(limit_count5<-30)
+    if(limit_count5<-15)
         limit_count5=1;
 }
 
@@ -166,9 +167,9 @@
     if(limit_count6<-50)
         limit_count6=-50;
         
-    if(limit_count6>30)
+    if(limit_count6>15)
         limit_result6=0;
-    if(limit_count6<-30)
+    if(limit_count6<-15)
         limit_count6=1;
 }
 
@@ -321,10 +322,10 @@
         sum_error[i]+=limit_error[i]*0.2;
         limit_result[i]=limit_error[i]*2+sum_error[i]*1;
         
-        if(limit_result[i]>200)
-            limit_result[i]=200;
-        if(limit_result[i]<-200)
-            limit_result[i]=-200;
+        if(limit_result[i]>limit_max_power)
+            limit_result[i]=limit_max_power;
+        if(limit_result[i]<-limit_max_power)
+            limit_result[i]=-limit_max_power;
         
         limit_output[i]=motor_duty[i]+limit_result[i];
         
@@ -342,10 +343,10 @@
         sum_error[i]+=limit_error[i]*0.2;
         limit_result[i]=limit_error[i]*2+sum_error[i]*1;
         
-        if(limit_result[i]>200)
-            limit_result[i]=200;
-        if(limit_result[i]<-200)
-            limit_result[i]=-200;
+        if(limit_result[i]>limit_max_power)
+            limit_result[i]=limit_max_power;
+        if(limit_result[i]<-limit_max_power)
+            limit_result[i]=-limit_max_power;
         
         limit_output[i]=motor_duty[i]+limit_result[i];
 
@@ -363,10 +364,10 @@
         sum_error[i]+=limit_error[i]*0.2;
         limit_result[i]=limit_error[i]*2+sum_error[i]*1;
         
-        if(limit_result[i]>200)
-            limit_result[i]=200;
-        if(limit_result[i]<-200)
-            limit_result[i]=-200;
+        if(limit_result[i]>limit_max_power)
+            limit_result[i]=limit_max_power;
+        if(limit_result[i]<-limit_max_power)
+            limit_result[i]=-limit_max_power;
         
         limit_output[i]=motor_duty[i]+limit_result[i];
         
@@ -425,8 +426,11 @@
 
 bool limit_check_done=false;
 int limit_check_done_cnt=0;
+
 void find_limit()
 {
+    bool limit_sucess=false;
+    
     wait_ms(0);
     
     encoder_reset_cnt();
@@ -435,9 +439,31 @@
     {
         if(limit_check())
         {
+            limit_sucess=true;
             break;
         }
-        reset_check();
+        
+        if(encoder_data[0]==-1 && encoder_data[1]==-1 && encoder_data[2]==-1 && encoder_data[3]==-1 && encoder_data[4]==1 && encoder_data[5]==1)
+            {
+                while(1)
+                {
+                    motor_power(0,0);
+                    motor_power(1,0);
+                    motor_power(2,0);
+                    motor_power(3,0);
+                    motor_power(4,0);
+                    motor_power(5,0);
+    
+                    pc.printf("encoder_data[0]==-1 && encoder_data[1]==-1 && encoder_data[2]==-1 && encoder_data[3]==-1 && encoder_data[4]==1 && encoder_data[5]==1\r\n");
+                    //encoder_data[0] = encoder1.read();
+                    //if(encoder_data[0]!=-1)
+                    //{
+                        //NVIC_SystemReset();
+                    //}
+                    wait_us(2500);
+                }
+            }
+            
         wait_ms(1);
         
     }
@@ -448,6 +474,12 @@
     motor_power(3,0);
     motor_power(4,0);
     motor_power(5,0);
+    
+    while(!limit_sucess)
+    {
+        pc.printf("Limit-faild\r\n");
+    }
+    
 }
 
 #endif