/*
*
#region Demo 2 - Custom heartbeat
*
* static async Task Demo2()
* {
* //await mcu.DigitalWrite(0, true);
* //await mcu.DigitalWrite(0, false);
*
* //await mcu.PinMode(21, true);
* //var foo = await mcu.AnalogRead(21);
* //await mcu.Execute(Brief.Compile("[42 event( 21 analogRead edata )event] setLoop")); // TODO: dangerous!!
* await mcu.Execute(OldBrief.Compile("[loopTicks 10 mod 0 = [42 event( 20 analogRead edata 21 analogRead edata )event] [] choice] setLoop")); // TODO: dangerous!!
* const int wait = 5000;
* await Task.Delay(wait);
* await mcu.Execute(OldBrief.Compile("stopLoop"));
* await Task.Delay(1000); // let console catch up
* Console.WriteLine();
* Console.WriteLine("Frames/sec = {0} (dropped {1})", (float)frames / (float)wait * 1000.0, dropped);
* //await mcu.Execute(Brief.Compile("40 loopTicks event"));
* //await Task.Delay(1000); // give time for event
* }
*
#endregion Demo 2
*
#region Demo 3 - MonsterMoto driver
*
* static async Task Demo3()
* {
* var left = new Motor(mcu, 8, 7, 5, "left");
* await left.Initialize();
*
* var right = new Motor(mcu, 9, 4, 6, "right");
* await right.Initialize();
*
* await left.SetSmoothing(1);
* await left.SetTargetPwmPercentage(0.92);
* await left.SetTargetPwmPercentage(0.21);
* await left.SetTargetPwmPercentage(-0.54);
* await left.SetTargetPwmPercentage(0.0);
* await left.SetSmoothing(2);
* await left.SetTargetPwmPercentage(1);
* await left.Brake();
*
* await right.SetTargetPwmPercentage(-1);
* await right.Brake();
*
* await mcu.Execute(OldBrief.Compile("left 255 drive right -255 drive"));
* await mcu.Execute(OldBrief.Compile("left 50 drive right 50 drive"));
* await mcu.Execute(OldBrief.Compile("left stop right stop"));
* }
*
#endregion Demo 3
*
#region Demo 4 - Pololu Gyro
*
* static async Task Demo4()
* {
* var gyro = new Gyro(mcu, "gyro");
* await gyro.Initialize();
*
* for (var i = 0; i < 10; i++)
* {
* Console.WriteLine("Gyro: {0}", await gyro.AngularSpeedRaw());
* await Task.Delay(100);
* }
*
* var test = 0.0;
* for (var i = 0; i < 10; i++)
* {
* var speed = await gyro.AngularSpeedDps();
* test += speed;
* Console.WriteLine("GyroDps: {0} ({1})", speed, test);
* await Task.Delay(100);
* }
*
* await mcu.Execute(OldBrief.Compile("[42 gyroDps event 100 delay] setLoop")); // TODO: dangerous!!
* await Task.Delay(1000);
* await mcu.Execute(OldBrief.Compile("stopLoop"));
* }
*
#endregion Demo 4
*
#region Demo 5 - Pololu Accelerometer
*
* static async Task Demo5()
* {
* var accelerometer = new Accelerometer(mcu, "acceleration");
* await accelerometer.Initialize();
*
* for (var i = 0; i < 1000; i++)
* {
* Console.WriteLine("Acceleration: {0}", await accelerometer.Acceleration());
* await Task.Delay(100);
* }
*
* await mcu.Execute(OldBrief.Compile("[42 acceleration event 100 delay] setLoop")); // TODO: dangerous!!
* await Task.Delay(1000);
* await mcu.Execute(OldBrief.Compile("stopLoop"));
* }
*
#endregion Demo 5
*
#region Demo 6 - Balancing
*
* static async Task Demo6()
* {
* var balancing = new Balancing(mcu);
* await balancing.Initialize();
*
* var lastY = float.NaN;
* var lastX = float.NaN;
* var lastStart = false;
* var lastA = false;
*
#if SHORT_BOT
* var parameters = new Tuple<string, double>[] {
* new Tuple<string, double>("MinLeft", 30),
* new Tuple<string, double>("MinRight", 20),
* new Tuple<string, double>("SPD", 0),
* new Tuple<string, double>("Trim", -0.3),
* new Tuple<string, double>("PA", 30),
* new Tuple<string, double>("IA", 0.95),
* new Tuple<string, double>("PM", 0.9),
* new Tuple<string, double>("IM", 0.01),
* new Tuple<string, double>("DM", 10) };
#else
* var parameters = new Tuple<string, double>[] {
* new Tuple<string, double>("MinLeft", 20),
* new Tuple<string, double>("MinRight", 20),
* new Tuple<string, double>("SPD", 0),
* new Tuple<string, double>("Trim", 0.6),
* new Tuple<string, double>("PA", 30),
* new Tuple<string, double>("IA", 0.95),
* new Tuple<string, double>("PM", 0.9),
* new Tuple<string, double>("IM", 0.01),
* new Tuple<string, double>("DM", 10) };
#endif
* var selected = -1;
* GamePadState pad;
*
* foreach (var p in parameters)
* await mcu.Execute(OldBrief.Compile("{0} set{1}", (int)(p.Item2 * 1000 + 0.5), p.Item1));
*
* bool allowParameterTweaks = false;
* bool exit = false;
* var dampenDrive = 1.0f;
*
* do
* {
* pad = GamePad.GetState(PlayerIndex.One);
*
* var y = pad.ThumbSticks.Left.Y;
*
* const double easingThreshold = 0.2;
* if (Math.Abs(y) < easingThreshold)
* dampenDrive = 1.0f; // turn off dampening once user eases off
* y *= dampenDrive;
* dampenDrive *= (Math.Abs(y) > easingThreshold ? 0.95f : 1.0f); // increase dampening until eased off
*
* if (y.CompareTo(lastY) != 0)
* {
* lastY = y;
* Console.WriteLine("Y: {0}", y);
#if SHORT_BOT
* var d = y * (float)(y > 0 ? 2.75 : 3.5);
#else
* var d = y * (float)(y > 0 ? 1.75 : 1.5);
#endif
* await balancing.SetDrive(d);
* //Console.WriteLine("Drive: {0}", d);
* }
*
* var x = pad.ThumbSticks.Left.X;
* if (x.CompareTo(lastX) != 0)
* {
* lastX = x;
* var t = x * 50;
#if !SHORT_BOT
* t = -t; // motors reversed
#endif
* await balancing.SetTurn(t);
* Console.WriteLine("Turn: {0}", t);
* }
*
* if (allowParameterTweaks)
* {
* var s = pad.IsButtonDown(Buttons.Start);
* if (s && !lastStart)
* {
* await balancing.ToggleBalance();
* Console.WriteLine("Toggle balancing");
* }
* lastStart = s;
*
* var a = pad.IsButtonDown(Buttons.A);
* if (a && !lastA)
* {
* selected = (selected + 1) % parameters.Length;
* Console.WriteLine("Selected: {0}", parameters[selected]);
* }
* lastA = a;
*
* if (pad.IsButtonDown(Buttons.X))
* {
* for (var i = 0; i < parameters.Length; i++)
* {
* var n = parameters[i].Item1;
* parameters[i] = new Tuple<string, double>(n, 0);
* await mcu.Execute(OldBrief.Compile("0 set{0}", n));
* }
* Console.WriteLine("Reset parameters");
* }
*
* var adjust = 0.0;
* var fine = pad.IsButtonDown(Buttons.Y);
* if (pad.IsButtonDown(Buttons.DPadUp))
* adjust = fine ? 0.01 : 0.1;
* if (pad.IsButtonDown(Buttons.DPadDown))
* adjust = fine ? -0.01 : -0.1;
*
*
* if (adjust != 0.0 && selected != -1)
* {
* var p = parameters[selected];
* var n = p.Item1;
* var v = p.Item2 + adjust;
* parameters[selected] = new Tuple<string, double>(n, v);
* await mcu.Execute(OldBrief.Compile("{0} set{1}", (int)(v * 1000 + 0.5), n));
* Console.WriteLine("{0} = {1}", n, v);
* await Task.Delay(100);
* }
*
* if (pad.IsButtonDown(Buttons.Back))
* exit = true;
* }
*
* if (pad.IsButtonDown(Buttons.LeftShoulder) &&
* pad.IsButtonDown(Buttons.RightShoulder) &&
* pad.Triggers.Left == 1 &&
* pad.Triggers.Right == 1 &&
* pad.IsButtonDown(Buttons.LeftStick) &&
* pad.IsButtonDown(Buttons.B))
* {
* allowParameterTweaks = !allowParameterTweaks;
* Console.WriteLine("Allow Parameter Tweaks: {0}", allowParameterTweaks);
* Thread.Sleep(500);
* }
*
* Thread.Sleep(10);
* } while (!exit);
* }
*
#endregion Demo 6
*/
#region Demo 7 - Servo
static async Task Demo7()
{
var pan = new Servo(mcu, 1, 1000, 2000);
await pan.Initialize();
var tilt = new Servo(mcu, 0, 1000, 2000);
await tilt.Initialize();
var lastY = float.NaN;
var lastX = float.NaN;
GamePadState pad;
do
{
pad = GamePad.GetState(PlayerIndex.One);
var y = pad.ThumbSticks.Left.Y;
var x = pad.ThumbSticks.Left.X;
if (x.CompareTo(lastX) != 0)
{
lastX = x;
Console.WriteLine("X: {0}", x);
await pan.Write(x);
}
if (y.CompareTo(lastY) != 0)
{
lastY = y;
Console.WriteLine("Y: {0}", y);
await tilt.Write(y);
}
} while (true);
}
public async Task Grip(float value)
{
await grip.Write(value);
}
private async Task InitServo(Servo servo)
{
await servo.Initialize();
await servo.Write(0);
}