Motor.cpp

00001 #include "Motor.h"
00002 
00003 #include <stdint.h>
00004 #include "mbed.h"
00005 
00006 namespace MOTOR
00007 {
00008     namespace
00009     {
00010         MotorStatus motor[MOUNTING_MOTOR_NUM];
00011         DigitalOut directions[] = {
00012             DigitalOut(MOTOR0_D1_PIN),
00013             DigitalOut(MOTOR0_D2_PIN),
00014             DigitalOut(MOTOR1_D1_PIN),
00015             DigitalOut(MOTOR1_D2_PIN),
00016             DigitalOut(MOTOR2_D1_PIN),
00017             DigitalOut(MOTOR2_D2_PIN),
00018             DigitalOut(MOTOR3_D1_PIN),
00019             DigitalOut(MOTOR3_D2_PIN),
00020             DigitalOut(MOTOR4_D1_PIN),
00021             DigitalOut(MOTOR4_D2_PIN),
00022         };
00023         PwmOut pwms[] = {
00024             PwmOut(MOTOR0_PWM_PIN),
00025             PwmOut(MOTOR1_PWM_PIN),
00026             PwmOut(MOTOR2_PWM_PIN),
00027             PwmOut(MOTOR3_PWM_PIN),
00028             PwmOut(MOTOR4_PWM_PIN),
00029         };
00030     }
00031 
00032     float percentage_to_ratio(float percentage);
00033 
00034     void Motor::Initialize(void) {
00035         //Port Initialize
00036         for(uint8_t i = MOTOR_START_NUM; i < MOUNTING_MOTOR_NUM * 2; i++) {
00037             directions[i] = 0;
00038         }
00039 
00040         //Pwm Initialize
00041         for(uint8_t i = MOTOR_START_NUM; i < MOUNTING_MOTOR_NUM; i++) {
00042             pwms[i].period_us(50);  //20kHz
00043             pwms[i] = 0.0;
00044         }
00045 
00046         SetDefault();
00047     }
00048 
00049     void Motor::SetDefault(void) {
00050         for(uint8_t i = MOTOR_START_NUM; i < MOUNTING_MOTOR_NUM; i++) {
00051             motor[i].dir = FREE;
00052             motor[i].pwm = 0;
00053         }
00054     }
00055 
00056     void Motor::Update(MotorStatus *status) {
00057         for(uint8_t i = MOTOR_START_NUM; i < MOUNTING_MOTOR_NUM; i++) motor[i] = status[i];
00058 
00059         //PWM Update
00060         for(uint8_t i = MOTOR_START_NUM; i < MOUNTING_MOTOR_NUM; i++) pwms[i] = percentage_to_ratio(motor[i].pwm);
00061 
00062         //Port Update
00063         for(uint8_t i = MOTOR_START_NUM; i < MOUNTING_MOTOR_NUM; i++) {
00064             directions[i * 2]       = motor[i].d1;
00065             directions[i * 2 + 1]   = motor[i].d2;
00066         }
00067     }
00068 
00069     float percentage_to_ratio(float percentage) {
00070         return percentage / 100.0f;
00071     }
00072 
00073     int Motor::SetStatus(float pwm) {
00074         if(pwm > 0.0f)       return FOR;
00075         else if(pwm < 0.0f)  return BACK;
00076         else                return BRAKE;
00077     }
00078     
00079     float Motor::SetPWM(float pwm) {
00080         if(pwm > 0.0f)       return pwm;
00081         else if(pwm < 0.0f)  return -pwm;
00082         else                return 80;
00083     }
00084     
00085     
00086 }