2 replies to this topic

[BrandonW6]

Hello, I'm new to netduino programming and also to this forum. I'm wondering if you guys can perhaps provide some insight onto a topic I'm working on. Basically, I am writing some controllers that will manage multiple servos together in an IK chain as an arm or leg configuration. My question is this, with no positional feedback from the servos, how should I track where my servo is? As I see it, there are two solutions. A: Provide many updates constantly that are only tiny angle changes. B: Provide less frequent updates, and try to "guess" how long it will take to get to that angle before sending the next one. With solution A: I will spend alot of CPU time sending updates to angles. Which isn't ideal since the servos are capable of some level of target seeking. With solution B: I have the issue of how frequently to update. If I update too slowly, the servo will have hit the target and will be stopped waiting for the next angle. But if I update too quickly, I risk the servo skipping over desired angles. This would be especially apparent when the servo direction changes. If I send "goto 45 degress, then 90 degrees, then 45 degrees" to quickly, it may not reach 90 before it returns. The end goal is to be able to keyframe a sequence of movements, and have it execute those movements as smoothly as possible with no start/stop stutter. I figure this must be an issue someone has come across. Just curious to hear what you guys think. Thanks Brandon

[Paul Newton]

Hi Brandon, I have not had to solve this problem myself, so this is just how I think I might go about it. 1/ Try and calibrate the movement - measure how long each move takes and build up some tables of current position (angle) and time to reach the next. I know this is not perfect, the times will change with load on the limb. 2/ Whilst you have said that you can't measure the position of each servo, you could measure the current taken by the servo. The idea is that a drop in current would tell you when a servo reaches each position. This would allow you to use small steps but not waste time at the end of each step. Hope this gets the ball rolling, there should be lots more ideas from the forum team to come! Paul

[BrandonW6]

Hey Paul,

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();
        }
    }


}