File content as of revision 1:e1e9671724e7:

#include "Motor.h"

#include <stdint.h>
#include "mbed.h"

namespace MOTOR
{
    namespace
    {
        MotorStatus motor[MOUNTING_MOTOR_NUM];
        DigitalOut directions[] = {
            DigitalOut(MOTOR0_D1_PIN),
            DigitalOut(MOTOR0_D2_PIN),
            DigitalOut(MOTOR1_D1_PIN),
            DigitalOut(MOTOR1_D2_PIN),
            DigitalOut(MOTOR2_D1_PIN),
            DigitalOut(MOTOR2_D2_PIN),
            DigitalOut(MOTOR3_D1_PIN),
            DigitalOut(MOTOR3_D2_PIN),
            DigitalOut(MOTOR4_D1_PIN),
            DigitalOut(MOTOR4_D2_PIN),
        };
        PwmOut pwms[] = {
            PwmOut(MOTOR0_PWM_PIN),
            PwmOut(MOTOR1_PWM_PIN),
            PwmOut(MOTOR2_PWM_PIN),
            PwmOut(MOTOR3_PWM_PIN),
            PwmOut(MOTOR4_PWM_PIN),
        };
    }
    
    float percentage_to_ratio(float percentage);
    
    void Motor::Initialize(void){
        //Port Initialize
        for(uint8_t i = 0; i < MOUNTING_MOTOR_NUM * 2; i++){
            directions[i] = 0;
        }
        
        //Pwm Initialize
        for(uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++){
            pwms[i].period_us(50); //20kHz
            pwms[i] = 0.0;
        }
        
        SetDefault();
    }
    
    void Motor::SetDefault(void){
        for(uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++){
            motor[i].dir = FREE;
            motor[i].pwm = 0;
        }
    }
    
    void Motor::Update(MotorStatus *status){
        for(uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++){
            motor[i] = status[i];
        }
        
        //PWM Update
        for(uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++){
            pwms[i] = percentage_to_ratio(motor[i].pwm);
        }
        
        //Port Update
        for(uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++){
            directions[i * 2]     = motor[i].d1;
            directions[i * 2 + 1] = motor[i].d2;
        }
    }

    float percentage_to_ratio(float percentage){
        return percentage / 100.0;
    }
    
    int Motor::SetStatus(float pwm){
        if(pwm > 0.0){
            return FOR;
        }else if(pwm < 0.0){
            return BACK;
        }else{
            return BRAKE;
        }
    }
}