public Robot()
{
Sensors = new SensorPoller();
#if NoMotors
MotorA = new FakeMotor{Name = "A"};
MotorB = new FakeMotor{Name = "B"};
MotorC = new FakeMotor{Name = "C"};
#else
MotorA = new DCMotor(PWM.Pin.PWM1, FEZ_Pin.Digital.Di28, FEZ_Pin.Digital.Di29) { SpeedCharacteristic = DCMotor.Characteristic.Linear };
MotorB = new DCMotor(PWM.Pin.PWM2, FEZ_Pin.Digital.Di30, FEZ_Pin.Digital.Di31) { SpeedCharacteristic = DCMotor.Characteristic.Linear };
MotorC = new DCMotor(PWM.Pin.PWM3, FEZ_Pin.Digital.Di32, FEZ_Pin.Digital.Di33) { SpeedCharacteristic = DCMotor.Characteristic.Linear };
#endif
Matrix wheelMatrix = new Matrix(-60 * Math.PI, 0, 0, 12 * Math.PI);
Vector wheelPosition = Vector.J * 95;
Compass = new FieldHeadingFinder(new HMC6352());
Drive = new ControlledHolonomicDrive(Compass,
new HolonomicDrive.Wheel(
Matrix.FromClockwiseRotation(Math.PI / 3) * wheelPosition,
Matrix.FromClockwiseRotation(Math.PI / 3) * wheelMatrix,
// MotorA
new RegulatedMotor(MotorA)
),
new HolonomicDrive.Wheel(
Matrix.Rotate180 * wheelPosition,
Matrix.Rotate180 * wheelMatrix,
// MotorB
new RegulatedMotor(MotorB)
),
new HolonomicDrive.Wheel(
Matrix.FromClockwiseRotation(-Math.PI / 3) * wheelPosition,
Matrix.FromClockwiseRotation(-Math.PI / 3) * wheelMatrix,
//MotorC
new RegulatedMotor(MotorC)
)
);
#if USEOLDDRIVE
OldDrive = new HolonomicDrive(
new HolonomicDrive.Wheel(
Matrix.FromClockwiseRotation(Math.PI / 3) * wheelPosition,
Matrix.FromClockwiseRotation(Math.PI / 3) * wheelMatrix,
MotorA
),
new HolonomicDrive.Wheel(
Matrix.Rotate180 * wheelPosition,
Matrix.Rotate180 * wheelMatrix,
MotorB
),
new HolonomicDrive.Wheel(
Matrix.FromClockwiseRotation(-Math.PI / 3) * wheelPosition,
Matrix.FromClockwiseRotation(-Math.PI / 3) * wheelMatrix,
MotorC
)
);
#endif
//Kicker = new Solenoid(PWM.Pin.PWM5);
BallDetector = IntensityDetectorArray.FromRadialSensors(Sensors.IR);
Button = new Button(FEZ_Pin.Digital.An5);
LightGate = new LightGate(FEZ_Pin.AnalogIn.An0, 350, 160);
LEDs = new LEDGroup(FEZ_Pin.Digital.An1, FEZ_Pin.Digital.An2, FEZ_Pin.Digital.An3, FEZ_Pin.Digital.An4);
RegulatedMotor.EnableRegulatedMotorAlgorithm(true);
}
public AddLedGroupUndoEvent(LedGroupCollection container, LEDGroup group)
{
_container = container;
_group = group;
}
public RemoveLedGroupUndoEvent(LedGroupCollection container, LEDGroup group, int index)
{
_container = container;
_group = group;
_index = index;
}
public RemoveLedGroupUndoEvent(LedGroupCollection container, LEDGroup group, int index)
{
_container = container;
_group = group;
_index = index;
}
public AddLedGroupUndoEvent(LedGroupCollection container, LEDGroup group)
{
_container = container;
_group = group;
}
