Kevin Gagnon
Published © CC BY-SA

Servo Trigger Firmware on Arduino

Overcoming the one servo restriction and replacing switches, jumpers and potentiometers with software controlled variables.

IntermediateProtip30 minutes1,106
Servo Trigger Firmware on Arduino

Things used in this project

Hardware components

Arduino UNO & Genuino UNO
Arduino UNO & Genuino UNO
Servos (Tower Pro MG996R)
Jumper wires (generic)
Jumper wires (generic)


Read more


Breadboard Layout

Timer 1 of the ATMega328 outputs PWM pulses on Pin 9 / Port B PB1 and Pin 10 / Port B PB2. Hookup your servo(s) like this.
Breadboard layout circuitsio

Pan/Tilt Chassis Assembly Prototype

Since I mentioned the larger project, here's an idea of what I describe as a potentiometer positional feedback system. The chassis is not assembled in the picture, but one side will be X and the other will be Y. The potentiometers adjacent each servo will eventually provide feedback to the ATMega328/Arduino of the attached servo's actual position. At least, that's the theory... Stay tuned.
20160826 090251

Oscilloscope display of two FSMs working independently

The sample application running Servo 0 in astableFSM and Servo 1 in bistableFSM.
Oscope servo0andservo1



Once the Servo_Trigger_Arduino.ino, G2G_FSM.h and G2G_FSM.cpp are downloaded. Open the Servo_Trigger_Arduino.ino. There's really not much to this code. The setup() simply sets some default values for the servos and FSM, the loop() is used to toggle "triggers" to move the servos from one "state" to the next. The loop delay time is controlled by the _delayLoop variable. Change this value to change the speed at which the triggers are toggled. All of the heavy lifting is done at the bottom of the file in the Interrupt Service Request (ISR) being fired by Timer 1. Every time this ISR handles the interrupt, it recalculates the selected FSM and stores a microsecond value to update the Output Control Register to modify the pulse width controlling the specific servo on Pin 9 or Pin 10. If you're a beginner, I'd suggest experimenting with this file for a while by changing the _loopDelay and the fsmMode variables. This should give you a better understanding of each FSM mode and how the triggers affect them. CAUTION: If you like your servos, don't change the positionA or positionB variables without being ready to cut power to the servos when the gears start grinding! "With great power, comes great responsibility!" The Sparkfun Servo Trigger adjusts these values via trimmer potentiometers and keeps them in a safe range automatically.
 * Servo_Trigger_Arduino.ino
 * Created:		8/31/2016 7:01:59 AM
 * Author:		Kevin Gagnon
 * Twitter:		@GadgetsToGrow
 * Purpose:		Emulate Sparkfun's Servo Trigger Firmware on an Arduino Uno,
 *				utilizing 2 servos, with independently controlled software 
 *				configuration of the servo associated Finite State Machines (FSMs).	
 * References and Credit:

 Include the G2G_FSM library and instantiate the fsm object
#include "G2G_FSM.h"
G2G_FSM fsm;

Global variables

//Non-blocking loop delay
unsigned long _previousMillis = 0;
unsigned long _currentMillis = 0;

void setup()
	Setup Timer 1 on the Arduino to output a stable 50Hz pulse

	Setup pin 9 (PORTB PB1) and/or pin 10 (PORTB PB2) for OUTPUT 
	and set the fsm_status[servoID].attached variable to TRUE.

	Set the initial Finite State Machine (FSM) "state" of each servo.
	fsm.fsm_status[0].servo_state = fsm.eIDLE;
	fsm.fsm_status[1].servo_state = fsm.eIDLE;
	positionA	//BOTTOM
	positionB	//TOP
	travelTime	//Time to move to each position
	Initial values for the Servos - the position numbers are unusually 
	large and were originally set by potentiometers on the Servo Trigger. 
	I haven't dug into the code enough to see if I can scale these further 
	and I ended up grinding a lot of gears	to figure out the ranges during 
	my initial coding attempts.  These seem to work on the servos I've tested. 
	CAUTION: If you change the positionA or positionB values, be ready to pull 
	the power quickly when you start hearing the grinding of gears!!  
	//Servo 0
	fsm.fsm_status[0].positionA = 10000;
	fsm.fsm_status[0].positionB = 180000;
	fsm.fsm_status[0].travelTime = 750;
	//Servo 1
	fsm.fsm_status[1].positionA = 10000;
	fsm.fsm_status[1].positionB = 180000;
	fsm.fsm_status[1].travelTime = 1000;
	fsm.fsm_status[Servo 0 or Servo 1].fsmMode
	Sets the initial FSM Mode for each servo attached.  For testing purposes of 
	the included FSM methods, Servo 0 will use ASTABLE and Servo 1 will use BISTABLE.  
	The Interrupt Service Request (ISR) will do all of the heavy lifting while the 
	main loop will toggle the "trigger" to allow experimentation with the different 
	FSMs and how the trigger affects each.  Create your own, or use the other FSMs 
	located in the G2G_FSM.cpp file.
	To keep things less cluttered and easy to understand I've included 3 of the 
	original FSMs (slightly modified) from the Sparkfun Servo Trigger firmware. 
	Note: The Servo Trigger allows one of two modes to be selected by soldering a 
	jumper on the board. If you want to change modes with the Servo Trigger for 
	Arduino, simply change the fsmMode below to the desired FSM.
	Description of available FSMs:
	ASTABLE -	This FSM does a complete cycle A-to-B-to-A while trigger is set to 
	BISTABLE -	This FSM sits in bottom state (positionA) waiting for trigger 
				to set to true. When it is set, it transits to positionB, taking 
				time travelTime. It stays in positionB until the trigger clears, 
				then it transits back to positionA, taking time travelTime.

	ONESHOT -	This FSM sits in idle state (positionA) waiting for trigger to 
				set to true. When it is set, the servo does a complete cycle 
				A-to-B-to-A, and waits for trigger to clear.
	fsm.fsm_status[0].fsmMode = fsm.ASTABLE;
	fsm.fsm_status[1].fsmMode = fsm.BISTABLE;
	fsm.fsm_status[Servo 0 or Servo 1].trigger
	Note: replaces "input" on the Sparkfun Servo Trigger firmware
	Sets the initial trigger value (moves FSM from state-to-state) 
	The loop() function below toggles this trigger to see how it affects each FSM.
	The _loopDelay variable determines the amount of time in milliseconds between 
	each toggle.
	fsm.fsm_status[0].trigger = true;
	fsm.fsm_status[1].trigger = true;	


	Change this variable in milliseconds to determine when each trigger is toggled 
	true or false for each FSM.  
	unsigned long _loopDelay = 5000;

void loop()
	//Get the current millis()
	_currentMillis = millis();
	//Time to toggle?
	if ((unsigned long)(_currentMillis - _previousMillis >= _loopDelay)) {
			Toggle the FSM trigger for Servo 0
		if(fsm.fsm_status[0].trigger == true) {
			fsm.fsm_status[0].trigger = false;
			} else {
			fsm.fsm_status[0].trigger = true;
			Toggle the FSM trigger for Servo 1
		if(fsm.fsm_status[1].trigger == true) {
			fsm.fsm_status[1].trigger = false;
		} else {
			fsm.fsm_status[1].trigger = true;
		//Print results to the Serial Monitor
		Serial.println("Servo 0 Trigger Status: " + String(fsm.fsm_status[0].trigger));
		Serial.println("Servo 1 Trigger Status: " + String(fsm.fsm_status[1].trigger));
		//Update the previous Millis count to the current Millis
		_previousMillis = _currentMillis;


INTERRUPT SERVICE REQUEST (ISR) <-- Where the magic happens

Capture the interrupt setup by initPWM() and calculate the next position
of the servo(s).

	// ***
	// *** Call the appropriate FSM:
	// *** Calculates the microsecond value to add to the OCR
	// *** for each servo.  Uses fsm_status[0 or 1].attached
	// *** to determine which to calculate.
	// ***
	// *** Update the Output Control Registers with the calculated
	// *** microsecond value for OCR1A or OCR1B to modify pulse width
	// ***
	if(fsm.fsm_status[0].attached) {
		OCR1A = fsm.PWM_MIN_USEC + fsm.fsm_status[0].us_val;
	if (fsm.fsm_status[1].attached) {
		OCR1B = fsm.PWM_MIN_USEC + fsm.fsm_status[1].us_val;

Servo Trigger for Arduino

Github repository for the files needed for this project. There are two folders. One is an Atmel Studio 7 Solution Folder and the other is a typical Arduino IDE folder with just the 3 files necessary to run the code in the Arduino IDE. Make sure the G2G_FSM.h and G2G_FSM.cpp are in the same folder as the Servo_Trigger_Arduino.ino file.

Original Sparkfun Servo Trigger Github Link

For convenience, this is the link to the original source code for the Sparkfun Servo Trigger that was used as a basis for this project. This repository has more information about FSMs in general, as well as a handy (although I haven't experimented with it yet) spreadsheet to modify the look up table used to adjust the travel time of the servo.


Kevin Gagnon

Kevin Gagnon

2 projects • 3 followers
Tinkered and maker of things.