Diff: motor.h

Revision:

0:7cff999a7f5c

Child:

3:7b195612e26d

Child:

4:bf278ddb8504

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/motor.h	Mon May 11 08:47:18 2020 +0000
@@ -0,0 +1,173 @@
+#ifndef _MOTOR_H_
+#define _MOTOR_H_
+
+#include "FastPWM.h"
+
+
+
+FastPWM pwm1(MOTOR_PWM1, -1);
+FastPWM pwm2(MOTOR_PWM2, -1);
+FastPWM pwm3(MOTOR_PWM3, -1);
+FastPWM pwm4(MOTOR_PWM4, -1);
+FastPWM pwm5(MOTOR_PWM5, -1);
+FastPWM pwm6(MOTOR_PWM6, -1);
+FastPWM pwm7(MOTOR_PWM7, -1);
+
+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);
+    
+    pwm7.write(0.2);
+}
+
+int ex_encoder[6]={0,0,0,0,0,0};
+int now_encoder[6]={0,0,0,0,0,0};
+int stop_cnt[6]={0,0,0,0,0,0};
+
+double now_motor_duty[6]={0,0,0,0,0,0};
+double after_motor_duty[6]={0,0,0,0,0,0};
+double motor_currnt_cut=1.0;
+double sum_duty_123axis=0;
+double sum_duty_456axis=0;
+double total_duty=0;;
+int duty_limit=1000;
+
+int duty_limit_cnt=0;
+bool duty_limit_on=false;
+
+int error_check_boost_duty[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(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);
+        }
+        else if(motor_num==1)
+        {
+            if(motor_onoff[motor_num]==false)
+                percent=1000;
+                
+            output = 1-(offset[motor_num] + percent)/2500;
+            pwm2.write(output);
+        }
+        else if(motor_num==2)
+        {
+            if(motor_onoff[motor_num]==false)
+                percent=1000;
+                
+            output = 1-(offset[motor_num] + percent)/2500;
+            pwm3.write(output);
+        }
+        else if(motor_num==3)
+        {
+            if(motor_onoff[motor_num]==false)
+                percent=1000;
+                
+            output = 1-(offset[motor_num] + percent)/2500;
+            pwm4.write(output);
+        }
+        else if(motor_num==4)
+        {
+            if(motor_onoff[motor_num]==false)
+                percent=1000;
+                
+            output = 1-(offset[motor_num] + percent)/2500;
+            pwm5.write(output);
+        }
+        else if(motor_num==5)
+        {
+            if(motor_onoff[motor_num]==false)
+                percent=1000;
+                
+            output = 1-(offset[motor_num] + percent)/2500;
+            pwm6.write(output);
+        }
+}
+
+
+#endif