Diff: motor.h

Revision:

4:bf278ddb8504

Parent:

0:7cff999a7f5c

--- a/motor.h	Tue Jul 28 01:42:16 2020 +0000
+++ b/motor.h	Tue Dec 08 01:25:06 2020 +0000
@@ -43,127 +43,55 @@
 
 int error_check_boost_duty[6]={0,};
 
+
+double motor_offset[6]={0,0,0,0,0,0};
+
+double last_percent[6]={0,};
+
 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;
-                
+        {    
+            last_percent[motor_num]=percent;
             output = 1-(offset[motor_num] + percent)/2500;
             pwm1.write(output);
         }
         else if(motor_num==1)
         {
-            if(motor_onoff[motor_num]==false)
-                percent=1000;
-                
+            last_percent[motor_num]=percent;
             output = 1-(offset[motor_num] + percent)/2500;
             pwm2.write(output);
         }
         else if(motor_num==2)
         {
-            if(motor_onoff[motor_num]==false)
-                percent=1000;
-                
+            last_percent[motor_num]=percent;
             output = 1-(offset[motor_num] + percent)/2500;
             pwm3.write(output);
         }
         else if(motor_num==3)
-        {
-            if(motor_onoff[motor_num]==false)
-                percent=1000;
-                
+        {   last_percent[motor_num]=percent;        
             output = 1-(offset[motor_num] + percent)/2500;
             pwm4.write(output);
         }
         else if(motor_num==4)
         {
-            if(motor_onoff[motor_num]==false)
-                percent=1000;
-                
+            last_percent[motor_num]=percent;
             output = 1-(offset[motor_num] + percent)/2500;
             pwm5.write(output);
         }
         else if(motor_num==5)
         {
-            if(motor_onoff[motor_num]==false)
-                percent=1000;
-                
+            last_percent[motor_num]=percent;
             output = 1-(offset[motor_num] + percent)/2500;
             pwm6.write(output);
         }