private async Task MotorControlSmokeTest()
{
ZumoMotorShieldConfig config = new ZumoMotorShieldConfig();
config.LeftMotorDirPin = 4;
config.RightMotorDirPin = 5;
config.LeftPwmChannel = 0;
config.RightPwmChannel = 1;
config.PwmDriverSlaveAddress = 0x40;
config.BuzzerPwmChannel = 2;
using (ZumoMotorShield motorDriver = new ZumoMotorShield(config))
{
await motorDriver.Init();
bool flipDir = false;
ZumoMotorDirection dirA = ZumoMotorDirection.Forward;
ZumoMotorDirection dirB = ZumoMotorDirection.Backward;
for (int i = 20; i <= 100; i += 20)
{
Debug.WriteLine("Motor Control Ticking");
if (!flipDir)
{
motorDriver.SetLeftMotorPower(dirA, (float)i / 100.0f);
motorDriver.SetRightMotorPower(dirB, (float)i / 100.0f);
}
else
{
motorDriver.SetLeftMotorPower(dirB, (float)i / 100.0f);
motorDriver.SetRightMotorPower(dirA, (float)i / 100.0f);
}
flipDir = !flipDir;
await Task.Delay(1500);
}
motorDriver.LeftMotorStop();
motorDriver.RightMotorStop();
await Task.Delay(1000);
motorDriver.SetLeftMotorPower(ZumoMotorDirection.Forward, 0.5f);
await Task.Delay(2000);
motorDriver.LeftMotorStop();
await Task.Delay(500);
motorDriver.SetRightMotorPower(ZumoMotorDirection.Forward, 0.5f);
await Task.Delay(2000);
motorDriver.RightMotorStop();
}
}
public void SetLeftMotorPower(ZumoMotorDirection dir, float power)
{
Debug.WriteLine("LeftMotor: {0} {1}", dir, power * 100.0f);
if (dir == ZumoMotorDirection.Forward)
LeftMotorDir.Write(GpioPinValue.Low);
else
LeftMotorDir.Write(GpioPinValue.High);
PwmDriver.SetChannelDutyCycle(Config.LeftPwmChannel, power);
}
public void SetRightMotorPower(ZumoMotorDirection dir, float power)
{
Debug.WriteLine("RightMotor: {0} {1}", dir, power * 100.0f);
if (dir == ZumoMotorDirection.Forward)
{
RightMotorDir.Write(GpioPinValue.Low);
}
else
{
RightMotorDir.Write(GpioPinValue.High);
}
PwmDriver.SetChannelDutyCycle(Config.RightPwmChannel, power);
rightMotorPower = power;
rightDir = dir;
}
public Task RunAsync()
{
// Set LED to blue
pixyCam.SetLED(0, 0, 255);
// Start reading frames from camera
return(ThreadPool.RunAsync((s) =>
{
Debug.WriteLine("Starting ObjectTracking loop");
long previousTime = 0;
watch.Start();
while (!shutdown)
{
long diff = watch.ElapsedMilliseconds - previousTime;
if (diff > 20)
{
Debug.WriteLine("Diff time: " + diff + "ms");
var blocks = pixyCam.GetBlocks(10);
if (blocks != null && blocks.Count > 0)
{
pixyCam.SetLED(0, 255, 0);
// Move camera servos to track object
var trackedBlock = trackBlock(blocks);
// Move Zumo motors to follow object
if (trackedBlock != null)
{
followBlock(trackedBlock);
}
// Keep track of the time the last block came in
previousTime = watch.ElapsedMilliseconds;
// Fire event
OnBlocksReceived(new ObjectBlocksEventArgs()
{
Blocks = blocks.ToArray()
});
}
else
{
oldBlock = null;
}
++frameCount;
fps = frameCount / (float)watch.Elapsed.TotalSeconds;
Debug.WriteLineIf(
watch.ElapsedMilliseconds % 5000 == 0,
string.Format("{0}s: FPS={1}, Frame-time={2}ms", watch.Elapsed.TotalSeconds, fps, 1000.0f / fps));
}
// If we lose sight of the object, start slowing down to a stop
if (diff > 100)
{
pixyCam.SetLED(255, 0, 0);
float currLeftPower = motorDriver.GetLeftMotorPower();
float currRightPower = motorDriver.GetRightMotorPower();
ZumoMotorDirection leftDir = motorDriver.GetLeftDir();
ZumoMotorDirection rightDir = motorDriver.GetRightDir();
// Decelerate
motorDriver.SetLeftMotorPower(leftDir, Constrain(currLeftPower - 0.05f, 0, 1));
motorDriver.SetRightMotorPower(rightDir, Constrain(currRightPower - 0.05f, 0, 1));
}
}
watch.Stop();
motorDriver.LeftMotorStop();
motorDriver.RightMotorStop();
Debug.WriteLine("Exiting ObjectTracking loop");
}).AsTask());
}
