ROBOFRIEN_MOTOR.cpp

00001 #include "ROBOFRIEN_MOTOR.h"
00002 
00003 float PWM_MIN = 1/3.0f;
00004 float PWM_MAX = 2/3.0f;
00005 
00006 PwmOut  pwm1(p21);
00007 PwmOut  pwm2(p22);
00008 PwmOut  pwm3(p23);
00009 PwmOut  pwm4(p24);
00010 PwmOut  pwm5(p25);
00011 PwmOut  pwm6(p26);
00012 
00013 void ROBOFRIEN_MOTOR::OFF(){
00014     pwm1.write(PWM_MIN);
00015     pwm2.write(PWM_MIN);
00016     pwm3.write(PWM_MIN);
00017     pwm4.write(PWM_MIN);
00018     pwm5.write(PWM_MIN);
00019     pwm6.write(PWM_MIN);    
00020 }
00021 
00022 void ROBOFRIEN_MOTOR::Init(){
00023     pwm1.period(0.003f);     // 3 ms
00024     pwm2.period(0.003f);     // 3 ms
00025     pwm3.period(0.003f);     // 3 ms
00026     pwm4.period(0.003f);     // 3 ms
00027     pwm5.period(0.003f);     // 3 ms
00028     pwm6.period(0.003f);     // 3 ms
00029     OFF();
00030 }
00031 
00032 void ROBOFRIEN_MOTOR::update(){        
00033     /* ***** */
00034     // 1ms = 1/3;
00035     // 2ms = 2/3;
00036     // Motor Power : 0 ~ 10000   
00037     float PWM_DATA[6];
00038     for(int i=0; i<6; i++){
00039         PWM_DATA[i] = PWM_MIN + (PWM_MAX - PWM_MIN) * (Value[i]/10000.0);
00040         if(PWM_DATA[i] > PWM_MAX)PWM_DATA[i] = PWM_MAX;
00041     }
00042     pwm1.write(PWM_DATA[0]);
00043     pwm2.write(PWM_DATA[1]);
00044     pwm3.write(PWM_DATA[2]);
00045     pwm4.write(PWM_DATA[3]);
00046     pwm5.write(PWM_DATA[4]);
00047     pwm6.write(PWM_DATA[5]);
00048 }
00049