File content as of revision 4:bf278ddb8504:

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


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*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)
        {    
            last_percent[motor_num]=percent;
            output = 1-(offset[motor_num] + percent)/2500;
            pwm1.write(output);
        }
        else if(motor_num==1)
        {
            last_percent[motor_num]=percent;
            output = 1-(offset[motor_num] + percent)/2500;
            pwm2.write(output);
        }
        else if(motor_num==2)
        {
            last_percent[motor_num]=percent;
            output = 1-(offset[motor_num] + percent)/2500;
            pwm3.write(output);
        }
        else if(motor_num==3)
        {   last_percent[motor_num]=percent;        
            output = 1-(offset[motor_num] + percent)/2500;
            pwm4.write(output);
        }
        else if(motor_num==4)
        {
            last_percent[motor_num]=percent;
            output = 1-(offset[motor_num] + percent)/2500;
            pwm5.write(output);
        }
        else if(motor_num==5)
        {
            last_percent[motor_num]=percent;
            output = 1-(offset[motor_num] + percent)/2500;
            pwm6.write(output);
        }
}


#endif