private void ValidateSetup(dynamic config, ServoSimpleAction action)
{
// the rotation degrees and rotation delays arrays should the same length
if (action.RotationDegrees.Length != action.RotationDelayMs.Length)
{
throw new ApplicationException("rotation[] and rotationDelay[] lengths are not the same");
}
//
// From this point we know the rotation degrees and delay are the same length so
// we can use either of them as a check value
//
// user's rotation stuff can't exceed the configured plugin channel count
int[] channels = Newtonsoft.Json.JsonConvert.DeserializeObject <int[]>(config.i2cChannel.ToString());
if (action.RotationDegrees.Length > channels.Length)
{
throw new ApplicationException($"rotationDegree[] and rotationDelay lengths cannot exceed maximum allowed by plugin configuration ({channels.Length})");
}
// rotation delay must be >= 0; rotation degrees must be 0 - 180
for (int i = 0; i < action.RotationDegrees.Length; i++)
{
if (action.RotationDelayMs[i].Any(d => d < 0))
{
throw new ApplicationException($"rotationDelay[{i}] has delay less than zero");
}
if (action.RotationDegrees[i].Any(r => r < 0 || r > 180))
{
throw new ApplicationException($"rotation[{i}] contains invalid rotation; must be in range 0-180");
}
}
// min/max pulses array lengths should be the same, and should be the
// same length as rotation array length
_pwmMinPulse = Newtonsoft.Json.JsonConvert.DeserializeObject <int[]>(config.pwmMinPulse.ToString());
_pwmMaxPulse = Newtonsoft.Json.JsonConvert.DeserializeObject <int[]>(config.pwmMaxPulse.ToString());
if ((_pwmMinPulse.Length != _pwmMaxPulse.Length) || (_pwmMinPulse.Length != channels.Length))
{
throw new ApplicationException($"config error: pwmMinPulse[] and pwmMaxPulse[] must be the same length as i2cChannel[]");
}
}
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
}
}
}
