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 } } }