Servo class

I coded up a quick servo class. Hopefully someone will find it useful.

Full example download: http://files.chrisseto.com/fnh

Most of the code should be self explanatory. Post if you have any questions. when the WIki is up, this will be posted there. Currently, the class only supports 180 degree servos. Later version may support 360 degree and free spin servos.


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 Servo servo = new Servo((PWM.Pin)FEZ_Pin.PWM.Di8);
		public static USBH_Joystick joystick;
		public static bool servoDisabled = false;

		public static void Main()
		{
			servo.inverted = true;


			// Sweep the servo to 90 degrees and back
			for (int i = 0; i <= 180; i++)
			{
				if (i == 90)
					servo.inverted = false;

				servo.sendServoToDeg(i);
				Thread.Sleep(100);
			}

			servo.inverted = true;


			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.sendServoToDeg((int)map(sender.Cursor.X, -512, 512, 0, 180));
		}

		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.disengage();
					servoDisabled = true;
					break;

				// Enable the servo
				case 1:
					servoDisabled = false;
					break;

				// Toggle joystick orentation
				case 5:
					servo.inverted = !servo.inverted;
					break;
			}

		}

	}

	/*
	 * Servo Class
	 *	Coded by Chris Seto  
	 *	
	 *	Use, copy and redistribute all you want,
	 *	but please keep this header intact.
	 * 
	 *  This is version 1.00
	 * */
	public class Servo
	{
		public PWM servo;
		public int[] range = new int[2];
		public bool inverted = false;

		public Servo(PWM.Pin pin)
		{
			servo = new PWM(pin);

			// Settings for the Futaba S3003
			range[0] = 410;
			range[1] = 2140;
		}

		// Set the timming for zero and 180 degrees 
		public void setRange(int fullLeft, int fullRight)
		{
			range[1] = fullLeft;
			range[0] = fullRight;
		}

		// Pull the signal low to disengage
		public void disengage()
		{
			servo.Set(false);
		}

		public void sendServoToDeg(int deg)
		{
			if (inverted)
				deg = 180 - deg;

			servo.SetPulse(20 * 1000 * 1000, (uint)map(deg, 0, 180, range[0], range[1]) * 1000);
		}

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

Hmm… That’s weird. The forum isn’t parsing the second code tags.

We will look into it