private void RotateServo(RotateServoConfiguration rotateConfig, CancellationToken cancelToken)
{
// init
rotateConfig.PcaConnection.SetPwm(rotateConfig.PwmChannel, 0, 0);
for (int i = 0; i < rotateConfig.RotationDegree.Length; i++)
{
SetState($"rotate_{rotateConfig.PwmChannel}_{i}");
// rotate
int cycle;
if (!DegreeToCycle(rotateConfig.RotationDegree[i], out cycle, rotateConfig.PwmMinimumPulse, rotateConfig.PwmMaximumPulse))
{
throw new ApplicationException($"Invalid rotation at index {i}: {rotateConfig.RotationDegree[i]}");
}
rotateConfig.PcaConnection.SetPwm(rotateConfig.PwmChannel, 0, cycle);
// rotation wait until possible cancel
SetState($"rotateDelay_c{rotateConfig.PwmChannel}_{i}");
if (cancelToken.WaitHandle.WaitOne(rotateConfig.RotationDelayMs[i]))
{
break; // looks like we're cancelling
}
}
}
public void Action(ActionBase baseAction, CancellationToken cancelToken, dynamic config)
{
ServoSimpleAction action = (ServoSimpleAction)baseAction;
SetState("validateSetup");
ValidateSetup(config, action);
ProcessorPin sda = config.i2cSdaBcmPin;
ProcessorPin scl = config.i2cSclBcmPin;
using (var driver = new I2cDriver(sda, scl))
{
SetState("setup");
PwmChannel[] channels = Newtonsoft.Json.JsonConvert.DeserializeObject <PwmChannel[]>(config.i2cChannel.ToString());
int i2cAddress = config.i2cAddress;
int pwmFrequency = config.pwmFrequency;
Frequency frequency = Frequency.FromHertz(pwmFrequency);
// device support and prep channel
SetState("connection");
var pcaConnection = new Pca9685Connection(driver.Connect(i2cAddress));
pcaConnection.SetPwmUpdateRate(frequency);
// pre delay
SetState("preDelay");
if (cancelToken.WaitHandle.WaitOne(action.PreDelayMs))
{
return; // looks like we're cancelling
}
// handle each rotation
//
// Note: there could be more configured channels (i.e. servos) in the configuration; the user doesn't
// have to use them all; we map the user's n array elements to first n configured channels in
// the server plugin config
List <Task> servoTasks = new List <Task>();
for (int i = 0; i < action.RotationDegrees.Length; ++i)
{
// if we're skipping this servo, loop to next
if (action.RotationDegrees.Length == 0)
{
continue;
}
SetState($"startRotate_{i}");
RotateServoConfiguration options = new RotateServoConfiguration
{
PcaConnection = pcaConnection,
PwmChannel = channels[i],
PwmMaximumPulse = _pwmMaxPulse[i],
PwmMinimumPulse = _pwmMinPulse[i],
RotationDegree = action.RotationDegrees[i],
RotationDelayMs = action.RotationDelayMs[i],
};
Task task = Task.Run(() => RotateServo(options, cancelToken));
servoTasks.Add(task);
}
// wait for all servos to complete; don't set state since tasks will be doing that
// as the servos rotate
Task.WaitAll(servoTasks.ToArray());
// post delay
SetState("postDelay");
if (cancelToken.WaitHandle.WaitOne(action.PostDelayMs))
{
return; // looks like we're cancelling
}
}
}
