public static void Main()
{
Robot r = new Robot();
r.Button.WaitForPress();
double startHeading = r.Compass.Angle;
while (true)
{
r.Sensors.Poll();
Vector raw = r.BallDetector.Get();
Vector filtered = filter.apply(r.BallDetector.Get());
double headingError = HeadingRange.Wrap(r.Compass.Angle - startHeading);
Debug.Print((headingError / Math.PI).ToString("f1"));
if (r.LightGate.IsObstructed)
{
r.Drive.RotationPoint = Vector.I * 250;
r.Drive.DriveVelocity = 0;
r.Drive.TurnVelocity = -3;
Thread.Sleep(500);
r.Drive.TurnVelocity = 0;
Thread.Sleep(500);
}
else
{
r.Drive.RotationPoint = 0;
r.Drive.DriveVelocity = 20 * filtered;
r.Drive.TurnVelocity = 0.5 * headingError;
Thread.Sleep(10);
}
}
}
public static void Main()
{
using (Soccer.Robot r = new Soccer.Robot())
{
r.Button.WaitForPress();
HMC6352 compass = r.Compass.AngleFinder as HMC6352;
r.Drive.RotationPoint = Vector.J * -500;
r.Drive.TurnVelocity = -0.25;
compass.StartCalibration();
Thread.Sleep(20000);
compass.EndCalibration();
r.Drive.TurnVelocity = 0;
}
}
public static void Main()
{
using (Robot r = new Robot())
{
//pid = new PIDController(0.75, 0.1, 0.005);
//PIDController.CoefficientsFromZieglerNicholsMethod(1.05, 0.95)
/*
double[] pidK = PIDController.CoefficientsFromZieglerNicholsMethod(
1.15, 0.87,
PIDController.ZieglerNicholsType.SomeOvershoot
);
pid = new PIDController(0.5, 1.5, 0.075)
{
Input = new PIDController.InputFunction(
() => r.Compass.Angle,
Range.Angle
),
Output = new PIDController.OutputFunction(
(value) => { Debug.Print(value+""); r.OldDrive.TurnVelocity = -value; },
Range.SignedAngle * 2
),
Continuous = true,
SetPoint = 0
};
r.Button.WaitForPress();
*/
//pid.Enabled = true;
Thread.Sleep(500);
Debug.Print(""+r.Compass.Angle);
Thread.Sleep(500);
r.Compass.RecordNorthDirection();
r.Drive.ControlEnabled = true;
r.Drive.TargetHeading = 0;
Thread.Sleep(1000);
r.Drive.TargetHeading = System.Math.PI / 2;
Thread.Sleep(1000);
r.Drive.TargetHeading = System.Math.PI;
Thread.Sleep(1000);
r.Drive.TargetHeading = -System.Math.PI / 2;
Thread.Sleep(1000);
r.Drive.ControlEnabled = false;
}
}
public static void Main()
{
int angle = 0;
Robot r = new Robot();
r.Button.WaitForPress();
while (true)
{
r.Drive.DriveVelocity = Vector.FromPolarCoords(200, System.Math.PI * angle / 3);
Thread.Sleep(750);
/*if (angle % 6 == 0)
{
r.Kicker.State = Solenoid.SolenoidState.Out;
Thread.Sleep(200);
r.Kicker.State = Solenoid.SolenoidState.In;
}*/
angle++;
}
}
public static void Main()
{
Robot r = new Robot();
IRangeFinder[] us = r.Sensors.US;
Debug.Print(""+new Range().Contains(1));
USSensorAggregator uss = new USSensorAggregator()
{
Front = us[0],
Right = us[1],
Back = us[2],
Left = us[3],
Compass = r.Compass
};
while (true)
{
r.Sensors.Poll();
Debug.Print(""+uss.GetPosition());
}
}
