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); }