File content as of revision 0:7cff999a7f5c:

#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