Very interesting idea about monitoring current draw. That's something I never would have thought of.
After some more tinkering this afternoon, I think I have a decent start. I decided that the solution was to implement a servo controller class that could handle both situations. Then use the appropriate method where applicable.
Keyframes are stored in a queue that are executed in order.
The servo controller has two methods:
void AddFrame(double targetAngle, double pause)
- go to this angle and wait before going to another angle
void AddFramesAtDuration(double targetAngle, double duration, int steps, double pause)
- go to target angle, making it get there in the given duration, how many between steps, then pause before next key
Next Steps include an "Ease In/Out routines" using a sigmoid curve to control acceleration and deceleration.
Sample code for Controller class below.
*Note, this class is designed to work with the Servo Controller class by Chris Seto which can be found on another thread*
/* * Servo NETMF Controller * Coded by Brandon Watt Sept 2012 * * Use this code for whatveer you want. Modify it, redistribute it, I don't care. * I do ask that you please keep this header intact, however. * If you modfy the driver, please include your contribution below: * * Brandon Watt: Initial release (1.0) * * */ using System; using System.Threading; using Microsoft.SPOT; using Microsoft.SPOT.Hardware; using SecretLabs.NETMF.Hardware; using SecretLabs.NETMF.Hardware.Netduino; namespace Servo_API { /// <summary> /// Class to Store KeyFrames (What angle, and how long to wait before sending the next angle) /// </summary> public class servoKeyFrame { public double angle; public double seconds; public servoKeyFrame(double targetAngle, double howManySeconds) { this.angle = targetAngle; this.seconds = howManySeconds; } } class ServoController : IDisposable { // Servo to Control public Servo servo; // Queue of keyframes (FIFO) private System.Collections.Queue frames = new System.Collections.Queue(); // Private Thread for this servo, used to manage position and saves CPU cycles since mostly sleeping public Thread servoThread; // Most recent angle added, for comparing angular difference private double lastAngleAdded; /// <summary> /// Initializes a servo on given channel /// </summary> /// <param name="channelPin"></param> public ServoController(Cpu.PWMChannel channelPin) { servo = new Servo(channelPin); servoThread = new Thread(new ThreadStart(ServoThreadFunction)); servoThread.Start(); } /// <summary> /// Add a keyframe /// </summary> /// <param name="targetAngle">Angle to Move to</param> /// <param name="pause">How long to pause afer arrival</param> public void AddFrame(double targetAngle, double pause) { frames.Enqueue(new servoKeyFrame(targetAngle, pause)); // add the latest keyframe lastAngleAdded = targetAngle; // used in 'AddFramesAtDuration' // if the thread is suspended, wake it. if (servoThread.ThreadState == ThreadState.Suspended) { servoThread.Resume(); } } /// <summary> /// Add a set of frames, enabling you to control how long it takes to hit the target angle /// </summary> /// <param name="targetAngle">Angle to Move to</param> /// <param name="duration">How long should it take to get there?</param> /// <param name="steps">How many incremental steps should it make?</param> /// <param name="pause">How long to Pause after arrival?</param> public void AddFramesAtDuration(double targetAngle, double duration, int steps, double pause) { // figure out, how far we are actually going to move double startAngle = lastAngleAdded; double diff = targetAngle - startAngle; double anglePerStep = diff / (steps + 1); // degrees per step double pausePerStep = duration / (steps + 1); // pause per step for (int s = 1; s < steps; s++) { frames.Enqueue(new servoKeyFrame(startAngle + s * anglePerStep, pausePerStep)); // enqueue all those steps } AddFrame(targetAngle, pause); // Add the final step and wake thread } /// <summary> /// Main worker thread function, moves servo to position and waits /// </summary> private void ServoThreadFunction() { servoKeyFrame nextKey = null; while (true) { if (frames.Count > 0) { nextKey = (servoKeyFrame)frames.Dequeue(); servo.Degree = nextKey.angle; Thread.Sleep((int)(nextKey.seconds * 1000)); // sleep until ready for next move } else servoThread.Suspend(); // Completed all the steps, goto sleep } } public void Dispose() { servoThread.Abort(); servo.Dispose(); } } }
To use this code
using System; using System.Threading; using Microsoft.SPOT; using Microsoft.SPOT.Hardware; using SecretLabs.NETMF.Hardware; using SecretLabs.NETMF.Hardware.Netduino; using Servo_API; namespace NetduinoApplication5 { public class Program { static ServoController s1 = new ServoController(PWMChannels.PWM_PIN_D9); static ServoController s2 = new ServoController(PWMChannels.PWM_PIN_D10); static InterruptPort sw = new InterruptPort(Pins.ONBOARD_SW1, false, Port.ResistorMode.Disabled, Port.InterruptMode.InterruptEdgeLow); static bool runMainThread = true; public static void Main() { sw.OnInterrupt += new NativeEventHandler(sw_OnInterrupt); EnqueueDemoFrames(); while (runMainThread) { // Do something else TimeSpan t = Microsoft.SPOT.Hardware.Utility.GetMachineTime(); double seconds = t.Ticks / TimeSpan.TicksPerSecond; Debug.Print(seconds.ToString()); Thread.Sleep(1000); } } public static void EnqueueDemoFrames() { // This demo will show that total movement time for both motors can remain synced (for the most part) s1.AddFrame(0, 0); s1.AddFrame(180, 2.5); s1.AddFramesAtDuration(0, 2, 100, 0.5); s1.AddFrame(90, 1); s1.AddFrame(0, 0); // 0 + 2.5 + 2 + 0.5 + 1 + 0 = 6 seconds s2.AddFrame(0, 0); s2.AddFramesAtDuration(180, 2, 100, 0.5); s2.AddFrame(0, 2.5); s2.AddFrame(90, 1); s2.AddFrame(0, 0); // 0 + 2 + 0.5 + 2.5 + 1 = 6 seconds } public static void sw_OnInterrupt(uint data1, uint data2, DateTime time) { //Debug.Print("Button Presses" + data1 + " " + data2); EnqueueDemoFrames(); } } }