Controlling a servo, in depth!

I was bored, so I decided to see how easy it was to control a servo with my USB joystick.

Here is the code:

using System;
using Microsoft.SPOT;
using GHIElectronics.NETMF.Hardware;
using GHIElectronics.NETMF.USBHost;
using System.IO;
using System.Threading;
using GHIElectronics.NETMF.FEZ;



namespace Servo
{
	public class Program
	{
		public static PWM servo = new PWM((PWM.Pin)FEZ_Pin.PWM.Di8);
		public static USBH_Joystick	joystick;
		public static bool	servoDisabled = false;
		public static int invert = -1;


		public static void Main()
		{
			USBHostController.DeviceConnectedEvent += joyConnected;
			Thread.Sleep(Timeout.Infinite);
		}

		public static long map(long x, long in_min, long in_max, long out_min, long out_max)
		{
			return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
		}

		public static void joyConnected(USBH_Device device)
		{
			if (device.TYPE == USBH_DeviceType.Joystick)
			{
				Debug.Print("Joystick connected...");

				joystick = new USBH_Joystick(device);

				joystick.JoystickXYMove      += moveServo;
				joystick.JoystickButtonDown  += checkButton;
			}
		}

		public static void moveServo(USBH_Joystick sender, USBH_JoystickEventArgs args)
		{
			if (!servoDisabled)
				servo.SetPulse(20 * 1000 * 1000, (uint)map(sender.Cursor.X * invert, -512, 512, 410, 2140) * 1000); //410, 2140 OLD: 150, 2400
		}

		public static void checkButton(USBH_Joystick sender, USBH_JoystickEventArgs args)
		{
			switch (args.ChangedButton)
			{
				// Lock servo
				case 2:
					servoDisabled = true; ;
					break;

				// Disengage the servo
				case 3:
					servo.Set(false);
					servoDisabled = true;
					break;

				// Enable the servo
				case 1:
					servoDisabled = false;
					long time = map(sender.Cursor.X * invert, -512, 512, 150, 2400);
					servo.SetPulse(20 * 1000 * 1000, (uint)time * 1000);
					break;

				// Toggle joystick orentation
				case 5:
					invert *= -1;
					break;

				case 4:
					invert *= -1;
					servo.SetPulse(20 * 1000 * 1000, (uint)map(sender.Cursor.X * invert, -512, 512, 150, 2400) * 1000);
					break;
			}
				
		}

	}
}

The code may be a little messy (and not very DRY), but it works extremely well.

It’s worth noting that I never found a good diagram listing how the PWM pulse relates to the angle of the control arm’s movement . In the end, I had to use my O-Scope to figure out that 0 degrees is .41ms, center is 1.269ms and 180 is 2.141ms.

I am sure will be very handy to somebody. Thanks :slight_smile:

Imagine i take the servo to a certain position and it can stay there for a while. I have a thread that normally updates the servo position. pseudo code

thread servoposition
{
while(true)
{
calculate newposition
if (oldposition != newposition)
{
enable servo
goto new position
}
else
disable servo

}
}

in fact I want the servo to stay in its position (there is no external force acting on the servo) like removing the control wire from the board. So the servo stays at its old position without continously being updated.

I hope it is understandable what i wrote above.

If you use my class to assign the servo to a position it will stay in that position as the PWM hardware will maintain the proper wave. No updating is required.

Chris,

Is it just me?

your posted code doesn’t show up properly on your post!
I mean not in the code format, but I saw you have the

 ... and 

before and after your posted code! ???

Hi Sam, you are correct. It’s some bug in the forum.

@ Chris

I know that the servo keeps it position by the pwm signal. But I want to switch off the pwm towards the servo. Meaning the servo stays in the last position when switched off (no pwm signal).
I am using the servo to control butterfly valves that needs a position update maybe 1 time per hour.
The reason to switch them off is the noise they make when the pwm is fed into the servo.
I have to say these servos are rather old.

How should I do this?

Actually, the opening code tag requires code=csharp. Using the code button will show this.

@ PEETEPEETE: The servos you are using will maintain their last position when they loose the command signal? Or do you remove power from the servo so the command signal becomes irrelevant?

If you just want to remove the PWM command signal you could dispose the PWM object for that pin and make it an output pin set to HI or LOW. I have not played with the PWM on the FEZ so I’m not sure if it is possible to specify the output signal have a 0% or 100% duty cycle which would be the same thing as having the pine set to an output and set LOW or HI.

You can do it two ways.
PWM already has Set(pinstate)
also, 0, 100 duty cycle works fine.

Cool, that makes it ‘Freaking Eazy’ ::slight_smile:

Later on I’ll post my new servo driver. This one is old I’ll bet.

Everything works like a charm no more buzzing. This is the small junk that does it.


if (positionold != position)
                {
                    rcservo.SetPulse(20 * 1000 * 1000, position * 1000);
                    positionold = position;
                }
                else
                    rcservo.Set(true); //This switches of the pwm to the servo.


Thanks guys for the help.

I havent try Chris new driver yet, but I notice that those existing drivers when I set it to sweep left to right some thing like this code would cause the servo to continue going some extra miles instead of immediately stop at 0 or 180 degree. It seems that the servo continue to go to -20,190 degree respectively. Does this driver address that issue?


for (int i=0;i<=180; i++)
{
 servo.setposition(i);
 sleep(sometime);
}

to prevent breaking my servo, I usually have to do some this like from 45 - 165, etc.
Thanks!

Hi Hai,

Make sure to set the timings to 1000 and 2000.Otherwise, the servo should never hunt for a position, unless it’s a result of the PWM bug that was fixed a long time ago.

As a long time c# coder (and MVP), I would only suggest some .net naming conventions as it really helps readability and makes things more ez. Camel case normal. All method names (private, public or static) start with upper case. private vars lower, public vars and properties upper. Sounds like a nit, but is really more important that it sounds as it also expresses intent and help communicate. Thanks for the code. Would this work with Mini?

Yep, will work with a Mini.

As far as naming standards go, this was like my first driver, cut me some slack :smiley: I am a professional dev, and most of my recent NETMF reflects that. This particular drive may have been a little rough while I adjusted to VS.

Hey Chris. No worries. Thanks again for your work :slight_smile: I was trying to be more general. I have noted this same issue with many codes here. If we don’t talk about it, nobody learns. Myself, on the HW end, have much more to learn. Keep up the good work!

Yep, no no problem :slight_smile:

We really do need to make a thread for NETMF best practices.