File content as of revision 0:c21936a3520a:

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

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

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


#endif