File content as of revision 3:7b195612e26d:

#ifndef _MOTOR_H_
#define _MOTOR_H_

#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);
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.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);
}

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,};


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*motor_gain[motor_num])
        percent=-500*motor_gain[motor_num];
    if(percent>500*motor_gain[motor_num])
        percent=500*motor_gain[motor_num];
        
        
    
        if(motor_num==0)
        {
            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(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(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(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(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(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