File content as of revision 26:7258d5ad0bff:

#include "wheel_unit.h"

WheelUnit::WheelUnit(DigitalOut* RS485Controller, Serial* RS485) :
	omni(4),
	wheels({
		ikarashiMDC({RS485Controller, 0, 1, SM, RS485}),
		ikarashiMDC({RS485Controller, 0, 2, SM, RS485}),
		ikarashiMDC({RS485Controller, 0, 3, SM, RS485}),
		ikarashiMDC({RS485Controller, 0, 0, SM, RS485})
	})
{
	omni.wheel[0].setRadian(PI / 4.0 * 5.0);
	omni.wheel[1].setRadian(PI / 4.0 * 7.0);
	omni.wheel[2].setRadian(PI / 4.0 * 1.0);
	omni.wheel[3].setRadian(PI / 4.0 * 3.0);
	for(int i = 0; i < 4; i++) {
		wheels[i].braking = true;
	}
}

void WheelUnit::moveXY(float X, float Y, float moment)
{
	omni.computeXY(X, Y, moment);
	for(int i = 0; i < 4; i++) {
		wheels[i].setSpeed(omni.wheel[i]);
	}
}

void WheelUnit::moveXY(float X, float Y, float gX, float gY, float moment)
{
	omni.computeXY(X, Y, gX, gY, moment);
	for(int i = 0; i < 4; i++) {
		wheels[i].setSpeed(omni.wheel[i]);
	}
}

void WheelUnit::moveCircular(float r, float radian, float moment)
{
	omni.computeCircular(r, radian, moment);
	for(int i = 0; i < 4; i++) {
		wheels[i].setSpeed(omni.wheel[i]);
	}
}

void WheelUnit::moveCircular(float r, float radian, float gX, float gY, float moment)
{
	omni.computeCircular(r, radian, gX, gY, moment);
	for(int i = 0; i < 4; i++) {
		wheels[i].setSpeed(omni.wheel[i]);
	}
}