public static void Main()
{
SensorPoller s = new SensorPoller();
LED l = new LED(FEZ_Pin.Digital.Di34);
l.State = true;
while (true)
{
s.Poll();
string broken = s.BrokenIRSensors;
if (broken != "")
Debug.Print("IR Sensors " + broken + " are borked!");
else
Debug.Print("Working!");
Thread.Sleep(100);
}
}
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);
}
