File content as of revision 4:15547d07f8e5:

#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 = MOTOR_START_NUM; i < MOUNTING_MOTOR_NUM * 2; i++) {
            directions[i] = 0;
        }

        //Pwm Initialize
        for(uint8_t i = MOTOR_START_NUM; i < MOUNTING_MOTOR_NUM; i++) {
            pwms[i].period_us(50);  //20kHz
            pwms[i] = 0.0;
        }

        SetDefault();
    }

    void Motor::SetDefault(void) {
        for(uint8_t i = MOTOR_START_NUM; i < MOUNTING_MOTOR_NUM; i++) {
            motor[i].dir = FREE;
            motor[i].pwm = 0;
        }
    }

    void Motor::Update(MotorStatus *status) {
        for(uint8_t i = MOTOR_START_NUM; i < MOUNTING_MOTOR_NUM; i++) motor[i] = status[i];

        //PWM Update
        for(uint8_t i = MOTOR_START_NUM; i < MOUNTING_MOTOR_NUM; i++) pwms[i] = percentage_to_ratio(motor[i].pwm);

        //Port Update
        for(uint8_t i = MOTOR_START_NUM; 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.0f;
    }

    int Motor::SetStatus(float pwm) {
        if(pwm > 0.0f)       return FOR;
        else if(pwm < 0.0f)  return BACK;
        else                return BRAKE;
    }
    
    float Motor::SetPWM(float pwm) {
        if(pwm > 0.0f)       return pwm;
        else if(pwm < 0.0f)  return -pwm;
        else                return 80;
    }
    
    
}