File content as of revision 23:98746361f827:

#include "motor_driver.h"

MotorDriver::MotorDriver() :
    MDC(MDSDA, MDSCL),
    quadOmni(WHEEL_NUMBER),
    solenoid(solenoidPin)
{
    MDC::MotorSTOP();
    quadOmni.setWheelRadian(
        WHEEL_RADIAN / 2.0,
        WHEEL_RADIAN /2.0 + WHEEL_RADIAN,
        -WHEEL_RADIAN / 2.0,
        -WHEEL_RADIAN /2.0 - WHEEL_RADIAN
    );
    solenoid = 0;
}

MotorDriver::MotorDriver(PinName sda, PinName scl, PinName solPin) :
    MDC(sda, scl),
    quadOmni(WHEEL_NUMBER),
    solenoid(solPin)
{
    MDC::MotorSTOP();
    quadOmni.setWheelRadian(
        WHEEL_RADIAN / 2.0,
        WHEEL_RADIAN /2.0 + WHEEL_RADIAN,
        -WHEEL_RADIAN / 2.0,
        -WHEEL_RADIAN /2.0 - WHEEL_RADIAN
    );
    solenoid = 0;
}

void MotorDriver::moveSlider(float speed)
{
    if(!arm.isResetted() && speed < 0) speed = 0;
    if(arm.isPushed() && speed > 0) speed = 0;
    MDC::write(ARM_MDC_ADDR, SLIDER_NUMBER, speed);
}

void MotorDriver::swing(float speed)
{
    MDC::write(ARM_MDC_ADDR, SWORD_NUMBER, speed);
}

void MotorDriver::shakeHead(float speed)
{
    MDC::write(ARM_MDC_ADDR, SHAKE_NUMBER, speed);
}

void MotorDriver::release()
{
    solenoid = 1;
}

void MotorDriver::destroy(float speed)
{
    write(ARM_MDC_ADDR, ARM_NUMBER, speed);
}

void MotorDriver::goXY(float X, float Y, float moment)
{
    quadOmni.computeXY(X, Y, moment);
    for(int i = 0; i < 4; i++) {
        write(WHEEL_MDC_ADDR, i, quadOmni.getOutput(i));
    }
}

void MotorDriver::goCircular(float r, float rad, float moment)
{
    quadOmni.computeCircular(r, rad, moment);
    for(int i = 0; i < 4; i++) {
        write(WHEEL_MDC_ADDR, i, quadOmni.getOutput(i));
    }
}