Diff: motor.h

Revision:

3:7b195612e26d

Parent:

0:7cff999a7f5c

--- a/motor.h	Tue Jul 28 01:42:16 2020 +0000
+++ b/motor.h	Tue Dec 08 01:27:11 2020 +0000
@@ -3,7 +3,7 @@
 
 #include "FastPWM.h"
 
-
+uint32_t motor_stop_cnt[6] = {0, 0, 0, 0, 0, 0};
 
 FastPWM pwm1(MOTOR_PWM1, -1);
 FastPWM pwm2(MOTOR_PWM2, -1);
@@ -15,13 +15,13 @@
 
 void motor_init()
 {
-    pwm1.period_us(2500);
-    pwm2.period_us(2500);
-    pwm3.period_us(2500);
-    pwm4.period_us(2500);
-    pwm5.period_us(2500);
-    pwm6.period_us(2500);
-    pwm7.period_us(2500);
+    pwm1.period_us(2500.0);
+    pwm2.period_us(2500.0);
+    pwm3.period_us(2500.0);
+    pwm4.period_us(2500.0);
+    pwm5.period_us(2500.0);
+    pwm6.period_us(2500.0);
+    pwm7.period_us(2500.0);
     
     pwm7.write(0.2);
 }
@@ -43,131 +43,176 @@
 
 int error_check_boost_duty[6]={0,};
 
+
+double motor_offset[6]={0,0,0,0,0,0};
+
+double last_percent[6]={0,};
+
+bool zero_state[6] = {false, false, false, false, false, false};
+
 void motor_power(int motor_num,double percent)
 {
     
     percent=-percent;
     double output=offset[motor_num];
-    if(percent<-500)
-        percent=-500;
-    if(percent>500)
-        percent=500;
-    
-        
-    if(percent>150 || percent<-150)
-    {
-        now_encoder[motor_num]=encoder_data[motor_num];
-    
-        if(ex_encoder[motor_num]==now_encoder[motor_num])
-            stop_cnt[motor_num]++;
-        else
-            stop_cnt[motor_num]=0;
-    
-        ex_encoder[motor_num]=now_encoder[motor_num];
+    if(percent<-500*motor_gain[motor_num])
+        percent=-500*motor_gain[motor_num];
+    if(percent>500*motor_gain[motor_num])
+        percent=500*motor_gain[motor_num];
         
-        if(stop_cnt[motor_num]>300)
-        {
-            if(abs(error_check_boost_duty[motor_num])>500)
-                motor_onoff[motor_num]=false;  
-            else
-            {
-                if(percent<0)
-                    error_check_boost_duty[motor_num]--;
-                else
-                    error_check_boost_duty[motor_num]++;
-                
-                percent=percent+error_check_boost_duty[motor_num];
-            }
-                
-        }
-    }else
-    {
-      stop_cnt[motor_num]=0;  
-      
-      if(error_check_boost_duty[motor_num]<0)
-        error_check_boost_duty[motor_num]++;
-      else if(error_check_boost_duty[motor_num]>0)
-        error_check_boost_duty[motor_num]--;
         
-    }
-    
-    
-    
-        now_motor_duty[motor_num]=abs(percent);
-    
-    sum_duty_123axis    =   now_motor_duty[0]+now_motor_duty[1]+now_motor_duty[2]+now_motor_duty[3];
-    sum_duty_456axis    =   now_motor_duty[4]+now_motor_duty[5];
-    
-    total_duty=sum_duty_123axis;
-    
-    
-    if(total_duty>duty_limit)
-        motor_currnt_cut=(duty_limit/total_duty);
-    else
-        motor_currnt_cut=1.0;
-    
-    if(duty_limit_on)
-    {
-        if( (motor_num==1) || (motor_num==2) || (motor_num==3) || (motor_num==4) )
-            percent=percent*motor_currnt_cut;
-        
-        duty_limit_cnt++;
-    if(duty_limit_cnt>200)
-    {
-      duty_limit_cnt=0;
-      pc.printf("%0.1f,%4.0f,%4.0f,%4.0f,%4.0f\r\n",motor_currnt_cut,now_motor_duty[0],now_motor_duty[1],now_motor_duty[2],now_motor_duty[3]);  
-    }   
-    }
     
         if(motor_num==0)
         {
-            if(motor_onoff[motor_num]==false)
-                percent=1000;
-                
-            output = 1-(offset[motor_num] + percent)/2500;
-            pwm1.write(output);
+            if(percent != 0)
+            {
+                last_percent[motor_num]=percent;
+                output = 1-(offset[motor_num] + percent)/2500;
+                pwm1.write(output);
+                zero_state[motor_num] = false;
+            }
+            else if(zero_state[motor_num] == false && percent == 0)
+            {
+                pwm1.pulsewidth_us(1040.0);
+                zero_state[motor_num] = true;
+            }
+            else if(zero_state[motor_num] == true)
+            {
+                pwm1.pulsewidth_us(2500.0);
+                motor_stop_cnt[motor_num]++;
+                if(motor_stop_cnt[motor_num] > 4000)
+                {
+                    zero_state[motor_num] = false;
+                    motor_stop_cnt[motor_num] = 0;
+                }
+            }
         }
         else if(motor_num==1)
         {
-            if(motor_onoff[motor_num]==false)
-                percent=1000;
-                
-            output = 1-(offset[motor_num] + percent)/2500;
-            pwm2.write(output);
+            if(percent != 0)
+            {
+                last_percent[motor_num]=percent;
+                output = 1-(offset[motor_num] + percent)/2500;
+                pwm2.write(output);
+                zero_state[motor_num] = false;
+            }
+            else if(zero_state[motor_num] == false && percent == 0)
+            {
+                pwm2.pulsewidth_us(1030.0);
+                zero_state[motor_num] = true;
+            }
+            else if(zero_state[motor_num] == true)
+            {
+                pwm2.pulsewidth_us(2500.0);
+                motor_stop_cnt[motor_num]++;
+                if(motor_stop_cnt[motor_num] > 4000)
+                {
+                    zero_state[motor_num] = false;
+                    motor_stop_cnt[motor_num] = 0;
+                }
+            }
         }
         else if(motor_num==2)
         {
-            if(motor_onoff[motor_num]==false)
-                percent=1000;
-                
-            output = 1-(offset[motor_num] + percent)/2500;
-            pwm3.write(output);
+            if(percent != 0)
+            {
+                last_percent[motor_num]=percent;
+                output = 1-(offset[motor_num] + percent)/2500;
+                pwm3.write(output);
+                zero_state[motor_num] = false;
+            }
+            else if(zero_state[motor_num] == false && percent == 0)
+            {
+                pwm3.pulsewidth_us(1000.0);
+                zero_state[motor_num] = true;
+            }
+            else if(zero_state[motor_num] == true)
+            {
+                pwm3.pulsewidth_us(2500.0);
+                motor_stop_cnt[motor_num]++;
+                if(motor_stop_cnt[motor_num] > 4000)
+                {
+                    zero_state[motor_num] = false;
+                    motor_stop_cnt[motor_num] = 0;
+                }
+            }
         }
         else if(motor_num==3)
         {
-            if(motor_onoff[motor_num]==false)
-                percent=1000;
-                
-            output = 1-(offset[motor_num] + percent)/2500;
-            pwm4.write(output);
+            if(percent != 0)
+            {
+                last_percent[motor_num]=percent;
+                output = 1-(offset[motor_num] + percent)/2500;
+                pwm4.write(output);
+                zero_state[motor_num] = false;
+            }
+            else if(zero_state[motor_num] == false && percent == 0)
+            {
+                pwm4.pulsewidth_us(1000.0);
+                zero_state[motor_num] = true;
+            }
+            else if(zero_state[motor_num] == true)
+            {
+                pwm4.pulsewidth_us(2500.0);
+                motor_stop_cnt[motor_num]++;
+                if(motor_stop_cnt[motor_num] > 4000)
+                {
+                    zero_state[motor_num] = false;
+                    motor_stop_cnt[motor_num] = 0;
+                }
+            }
         }
         else if(motor_num==4)
         {
-            if(motor_onoff[motor_num]==false)
-                percent=1000;
-                
-            output = 1-(offset[motor_num] + percent)/2500;
-            pwm5.write(output);
+            if(percent != 0)
+            {
+                last_percent[motor_num]=percent;
+                output = 1-(offset[motor_num] + percent)/2500;
+                pwm5.write(output);
+                zero_state[motor_num] = false;
+            }
+            else if(zero_state[motor_num] == false && percent == 0)
+            {
+                pwm5.pulsewidth_us(1000.0);
+                zero_state[motor_num] = true;
+            }
+            else if(zero_state[motor_num] == true)
+            {
+                pwm5.pulsewidth_us(2500.0);
+                motor_stop_cnt[motor_num]++;
+                if(motor_stop_cnt[motor_num] > 4000)
+                {
+                    zero_state[motor_num] = false;
+                    motor_stop_cnt[motor_num] = 0;
+                }
+            }
         }
         else if(motor_num==5)
         {
-            if(motor_onoff[motor_num]==false)
-                percent=1000;
-                
-            output = 1-(offset[motor_num] + percent)/2500;
-            pwm6.write(output);
+            if(percent != 0)
+            {
+                last_percent[motor_num]=percent;
+                output = 1-(offset[motor_num] + percent)/2500;
+                pwm6.write(output);
+                zero_state[motor_num] = false;
+            }
+            else if(zero_state[motor_num] == false && percent == 0)
+            {
+                pwm6.pulsewidth_us(1000.0);
+                zero_state[motor_num] = true;
+            }
+            else if(zero_state[motor_num] == true)
+            {
+                pwm6.pulsewidth_us(2500.0);
+                motor_stop_cnt[motor_num]++;
+                if(motor_stop_cnt[motor_num] > 4000)
+                {
+                    zero_state[motor_num] = false;
+                    motor_stop_cnt[motor_num] = 0;
+                }
+            }
         }
 }
 
 
-#endif
+#endif
\ No newline at end of file