Servo Control Question, sometimes works sometimes doesn't

My project is pretty straight forward.

FEZ
Button
IR Receiver
Server Control Board
Ethernet (to be added later)

I am using my TiVo remote to activate a servo that will either raise (thumbs up button) or lower (thumbs down) a cabinet door that sits in front of a television. The door will be raised/lowered through the use of a servo and a long control arm.

I have a single method in my program for changing the state of the door based on an input from the remote. TiVo is an extended NEC IR Protocol, so I made a class to detect the various button presses. I will post it here once it is a little more cleaned up.

Additionally, I have a method that the button click event is tied to that simple opens if closed or closes if open.

Both methods call the same single function to rotate the servo.

So here is the problem.

  1. The servo will work on a hard button (not on remote) press just after I’ve started the program. Will work on multiple presses. Each button press I see the Servo control board data LED flash letting me know it is receiving an instruction from the main board.
  2. Once I hit a button on the TiVo remote, the servo stops responding. I can see on the board that it is receiving commands via the LED flashes, but the servo isn’t moving.
  3. Pressing the button again at this point, the servo doesn’t respond.
  4. Pressing reset on the servo control board allows the button to once again control the servo.
  5. At no point am I able to control the servo via the IR command.

The servo I am using is a HiTEC Digital HSR-5498sg

Please let me know if you have any ideas.

The best thing about FEZ over anything out there is that it supports debugging. Put some breakpoints and step in your code to see exactly what is hapenning

I have a servo library you can use, if you want. This is completely tested.


	/*
	 * 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] = 1000;
			range[1] = 2000;
		}

		// 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 (deg > 180)
				deg = 180;

			if (deg < 0)
				deg = 0;

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

Thanks, Chris. I will check into your class. I’m assuming from your code that it works directly from the FEZ pin and not through the servo 16 board. Is this correct?

@ Chimp, the problem with debugging is that is all appears to be working correctly while stepping through, it is simply that the servo isn’t rotating.

As I said, with a button press, it works just fine, with an IR command received, all of code that should run does run, event the same command is sent to the servo, but the servo doesn’t rotate.


private void DoorSetState(DoorStates doorState) {
			switch (DoorState) {
				case DoorStates.Closed:
					DoorServo.SetServoPosition(8, 20, 90);
					break;
				case DoorStates.Open:
					DoorServo.SetServoPosition(8, 10, 0);					
					break;
			}
			
			DoorState = doorState;
		}

This is what is executed regardless of a button press or IR command. In both cases it is run.

The steps are:

  1. Run program
  2. Press Button
  3. Servo16 data light flashes
  4. Servo rotates
  5. Press Button
  6. Servo16 data light flashes
  7. Servo rotates
  8. Send IR command
  9. Servo 16 data light flashes
  10. Servo doesn’t move
  11. Press Button
  12. Servo 16 data light flashes
  13. Servo doesn’t move
  14. Press Reset on Servo 16 board
  15. Servo resets itself to 0 point if necessary
  16. Press Button
  17. Servo 16 data light flashes
  18. Servo rotates

No matter the combination of steps, the debugger always states the commands are being sent to the servo. But as soon as I send an IR command, the servo stops responding.

As I said, debugging is proving difficult because I can’t see beyond the

oc.Set(false, buffer, 0, currentBufferIndex, false);

in the FEZ_Components_Servo16.cs class.

Is there anything I should be looking for?

My main program code is:


		public static void Main() {
			Program program = new Program();
			Thread.Sleep(Timeout.Infinite);
		}

		public Program() {

			OnBoardLED = new FEZ_Components.LED(FEZ_Pin.Digital.LED);
			
			TvRemote = new TiVoReceiver(FEZ_Pin.Digital.Di13);
			
			TvRemote.DataReceived += new TVRemoteReceiver.TVRemoteDataHandler(TvRemote_DataReceived);
			
			MainButton = new FEZ_Components.Button(FEZ_Pin.Interrupt.Di4);
			DigitalLED = new FEZ_Components.LED(FEZ_Pin.Digital.Di12);
			DoorServo = new FEZ_Components.Servo16(FEZ_Pin.Digital.Di0);

			DoorState = DoorStates.Closed;

			MainButton.ButtonPressEvent += new FEZ_Components.Button.ButtonPressEventHandler(ButtonPressEvent);

			OnBoardLED.TurnOn();
			DigitalLED.ShutOff();
		}
				
		private void TvRemote_DataReceived(TVRemoteReceiver sender, int command, int address) {
			TivoButtons buttonPressed = (TivoButtons)command;
			DigitalLED.TurnOn(); 
			if ((TivoButtons)command == TivoButtons.ThumbsUp) {
				DoorChangeState();
			}
			else if ((TivoButtons)command == TivoButtons.ThumbsDown) {
				DoorChangeState();
			}
			DigitalLED.ShutOff();
		}

		protected TVRemoteReceiver TvRemote;
		protected FEZ_Components.LED OnBoardLED;
		protected FEZ_Components.Button MainButton;
		protected FEZ_Components.LED DigitalLED;
		protected FEZ_Components.Servo16 DoorServo;
		protected DoorStates DoorState;


		protected enum DoorStates { Open, Closed };

		private void ButtonPressEvent(FEZ_Pin.Interrupt pin, FEZ_Components.Button.ButtonState state) {
			
			if (state == FEZ_Components.Button.ButtonState.Pressed) {
				DigitalLED.TurnOn(); 
				DoorChangeState();
				DigitalLED.ShutOff();
			}
			
		}

		#region DoorState Function
		private void DoorSetState(DoorStates doorState) {
			switch (DoorState) {
				case DoorStates.Closed:
					DoorServo.SetServoPosition(8, 20, 90);
					break;
				case DoorStates.Open:
					DoorServo.SetServoPosition(8, 10, 0);					
					break;
			}
			
			DoorState = doorState;
		}

		private void DoorChangeState() {
			switch (DoorState) {
				case DoorStates.Closed:
					DoorOpen();
					break;
				case DoorStates.Open:
					DoorClose();
					break;
			}
		}

		#endregion



Could the IR input on one pin through the receiver be throwing off the communication of the servo16 board through the board’s channel to the servo? Everything else functions properly, the LEDs, etc, but no response from servo.

Hey hey, look at that. Updating to 1.0.5 resolved my servo IR issues. The TiVo remote is now controlling my servo. Woot.

Are we going to see your project here? (link removed)

Yeah, the servo lib works on a PWM pin, not any other servo hardware.

@ Chimp, one I get it all up and running… and comment/organize the code a little better, I think I will submit it.

@ Chris, It seems using the PWM pin would potentially be a better solution given that I’m only controlling 1 servo. Forgive my noobness, but how would I connect it? I tried to connect to Di5 using your class, modifying a connector to allow direct connection to the servo, but this just shuts the FEZ down. In spite of being a C# web developer full time for 8 years, this is my first attempt at robotics so I am acclimating to the hardware and differing approaches.

Chris, for the sake of being FEZ friendly, isn’t it better to use FEZ.Pin.PWM in the constructor instead of using PWM.Pin?

This way, users (especially beginners) will not have to think about which pins are PWM and which ones are not. See our servo driver for example.

Hi Chimp and santanaf,

@ Chimp, yeah, in fact I think I did that in a newer version. I will look and see if that is the newer one when I am at my desktop later today. If not, I’ll make the changes and shoot it up to the wiki.

@ santanaf, Hmmm… That’s weird. Can you post the few lines that you are using to construct and control the class? I have tested it with the last firmware release. I will test it with the new firmware later today.

Uh, actually, wait a sec. Might be a stupid question, but the FEZ is connected to external power, right? The USB port doesn’t supply enough current to feed a servo, or at least not in my experiences. The FEZ will simply reset, exactly as you said.

EDIT: I noticed there were a few other issues in that class I posted, like in the header. It is suppose to read “Use, modify, and redistribute”. I’m going to release a new version later today. I’ll link to the wiki page when I get done.

New version!

This version now supports output compare, so you can use any digital output pin the FEZ to control the servo. The lines of code responsible for setting the output compare channel are much like GHI’s servo driver, but in my experiments I found these timings to work a bit better.

Anyway, enjoy!


	/*
	 * Servo Class
	 *	Coded by Chris Seto  chris@ chrisseto.com www.chrisseto.com
	 *	
	 *	Use, modify and redistribute all you want,
	 *	but please keep this header intact.
	 * 
	 *  This is version 1.01
	 * */
	public class Servo : IDisposable
	{
		public OutputCompare servo;
		// This is the range of timings to use
		public int[] range = new int[2];
		// If true, the servo is inverted
		public bool inverted = false;
		// This is where we keep the calculated timings for the set() method
		uint[] timings = new uint[2];

		public Servo(FEZ_Pin.Digital pin)
		{
			servo = new OutputCompare((Cpu.Pin)pin, true, 5);

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

		public void Dispose()
		{
			servo.Dispose();
		}

		// Set the timing 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 (deg > 180)
				deg = 180;

			if (deg < 0)
				deg = 0;

			if (inverted)
				deg = 180 - deg;

			timings[0] = (uint)map(deg, 0, 180, range[0], range[1]);
			timings[1] = 20000;

			servo.Set(true, timings, 0, 2, true);
		}

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

This code was run through a test case with the latest firmware and SDK (GHI and .NETMF) and it works perfectly.

I would post this to the wiki, but for some reason I can’t log in.