//
// Name: servo.c
//
//
// Revision History:
//
// M. Gray 15 Jan 2002 V1.00 Initial release.
//
// M. Gray 02 Dec 2002 V1.01 Incremental release for web.
//
//
// COPYRIGHT (c) 2002 Michael Gray, KD7LMO
//
// This program is free software; you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation; either version 2 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program; if not, write to the Free Software
// Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
//
#include "16f876.h"
#fuses XT,NOWDT,NOPROTECT,NOBROWNOUT,NOLVP
// These compiler directives set the clock and baud rate information.
#use delay(clock=3686400)
#use rs232(baud=38400, xmit=PIN_C6, rcv=PIN_C7)
#use fast_io(B)
#use fast_io(C)
#byte PORTB=6
// We define types that are used for all variables. These are
// declared because compilers have different sizes for int and long.
typedef signed int int8;
typedef int uint8;
typedef signed long int16;
typedef long uint16;
// PROGRAMMING NOTE: Since we don't have embedded C++ for this processor,
// we try to use C in a manner that emulates classes. Each function
// and variable is prefaced by the name of the class. The classes
// have a function that acts as a constructor and sometimes a distructor.
// Even though variables are declared as globals, they are only accessed
// through the appropriate class function. The order of the classes and
// variables is not important in the application because we only access
// the class through a variable.
// Public methods for each class.
void commandConstructor();
uint8 commandRead();
void servoCheckState();
void servoConstructor();
void servoTimerInterrupt();
void servoSetCommand(uint8 *buffer);
/**
* Class to handle the timer for servo control.
*/
// Number of seconds between each servo PWM output. Since we have 4 channels,
// the update rate is 20mS or 50Hz for all the channels.
// value = 5mS * (clockHz / 4) => 0.005 * (3686400/4) = 4608
#define SERVO_PRI 4608
// Minimum pulse width. value = 950uS * (clockHz / 4) => 0.000950 * (3686400/4) = 875
#define SERVO_MIN_PW 875
// Default servo position. value = 1.5mS * (clockHz / 4) => 0.001500 * (3686400/4) = 1382
#define SERVO_DEFAULT_POSITION 1382
// Upper and lower limits for a select servo PW.
#define SERVO_MIN_SELECT_PW 875
#define SERVO_MAX_SELECT_PW 1382
// State machine constants.
#define SERVO_RISE_STATE 0
#define SERVO_FALL_STATE 1
// Defines the bitmap used for the servo output pins.
#define SERVO_OUT1 0x08
#define SERVO_OUT2 0x10
#define SERVO_OUT3 0x40
#define SERVO_OUT4 0x20
// Defines pin number for each servo switch.
#define SERVO_SW1 PIN_B7
#define SERVO_SW2 PIN_B2
#define SERVO_SW3 PIN_C4
#define SERVO_SW4 PIN_C5
// Define the pin number for the heart beat LED.
#define SERVO_LED PIN_C0
// Define the pin number for the R/C input selector.
#define SERVO_SELECT PIN_C2
// Define the pin numbers for the aux outputs.
#define SERVO_AUX1 PIN_C2
#define SERVO_AUX2 PIN_C3
// Define the status LED on/off time constants.
#define SERVO_LED_ONTIME_NORM 20
#define SERVO_LED_OFFTIME_NORM 180
#define SERVO_LED_ONTIME_FAULT 30
#define SERVO_LED_OFFTIME_FAULT 30
// The number of 5mS periods without a command packet before a fault is declared.
#define SERVO_COMM_TIMEOUT_COUNT 16
// Servo LED state machine constants.
#define SERVO_LED_NORM_HIGH 0
#define SERVO_LED_NORM_LOW 1
#define SERVO_LED_FAULT_HIGH 2
#define SERVO_LED_FAULT_LOW 3
// State machine.
uint8 servoState;
// Index 0:3 of the servo we are currently processing.
uint8 servoIndex;
// Track the last command so we only change control switches if the state changes.
uint8 servoLastCommand;
// Current Port B state.
uint8 servoPortB;
// An array of bit masks used to turn PWM states on and off.
uint8 servoMask[4];
// Port B tristate buffer.
uint8 servoTriState;
// Stores the time of the next interrupt.
uint16 servoTimerCompare;
// Current servo position.
uint16 servoPosition[4];
// Commanded servo position that is updated on the PWM output.
uint16 newServoPosition[4];
// Servo LED state, normal or fault condition.
uint8 servoLEDState;
// Count down timer used for the heart beat LED.
uint8 servoLEDTick;
// Timer used to determine if the comm link has failed.
uint8 servoCommTimeOut;
// Last state of select channel override.
uint8 servoLastState;
// Time of select channel low to high transition.
uint16 servoLastRiseTime;
// Flag used to indicate the R/C radio is allowing remote servo operation.
uint8 servoSelectFlag;
void servoCheckState()
{
int16 pulseWidth;
// We'll just exit if the input state hasn't changed.
if (servoLastState == input(SERVO_SELECT))
return;
// Record the time of the low to high transition.
if (servoLastState == 0) {
servoLastState = 1;
servoLastRiseTime = CCP_1;
} else {
servoLastState = 0;
pulseWidth = CCP_1 - servoLastRiseTime;
if (pulseWidth > SERVO_MIN_SELECT_PW && pulseWidth < SERVO_MAX_SELECT_PW)
servoSelectFlag = 0x80;
else
servoSelectFlag = 0x00;
}
}
void servoConstructor()
{
uint8 i;
// Set our initial conditions.
servoState = SERVO_RISE_STATE;
servoIndex = 0;
servoLastCommand = 0x0f;
servoLastState = 0;
servoLastRiseTime = 0;
servoSelectFlag = 0;
servoLEDTick = SERVO_LED_ONTIME_NORM;
servoCommTimeOut = SERVO_COMM_TIMEOUT_COUNT;
servoLEDState = SERVO_LED_NORM_HIGH;
servoPortB = 0x84;
servoTimerCompare = 4 * SERVO_PRI;
servoTriState = SERVO_OUT1 | SERVO_OUT2 | SERVO_OUT3 | SERVO_OUT4 | 0x03;
// Default servo position (mid-range) in uS.
for (i = 0; i < 4; ++i) {
servoPosition[i] = SERVO_DEFAULT_POSITION;
newServoPosition[i] = SERVO_DEFAULT_POSITION;
}
// These bits are AND/ORed with the output register to determine what output is
// enabled or disabled.
servoMask[0] = SERVO_OUT1;
servoMask[1] = SERVO_OUT2;
servoMask[2] = SERVO_OUT3;
servoMask[3] = SERVO_OUT4;
// Enable the interrupts and clock the timer register.
setup_ccp1( CCP_COMPARE_INT );
setup_timer_1( T1_INTERNAL | T1_DIV_BY_1 );
// We will wait 20mS before the servo interrupts start.
CCP_1 = servoTimerCompare;
set_timer1(0);
// Configure the output ports for the servo switch, control, and heart beat LED.
set_tris_b (servoTriState);
set_tris_c (0x82);
// Set the servo switch to pass through mode, turn off the heart beat LED.
output_high (SERVO_SW1);
output_high (SERVO_SW2);
output_high (SERVO_SW3);
output_high (SERVO_SW4);
output_high (SERVO_LED);
output_low (SERVO_AUX1);
output_low (SERVO_AUX2);
}
/**
* This function is called when the timer 1 compare register matches.
*/
#INT_CCP1
void servoTimerInterrupt()
{
uint8 i;
// The first thing we do is update the servo output pins.
PORTB = servoPortB;
// We toggle the PWM output on or off and setup for the next interrupt.
// This method works because the PRI is 20mS and our PW is 1 to 2 mS.
// If had a very low or high duty cycle, we wouldn't have time to toggle states.
switch (servoState) {
case SERVO_RISE_STATE:
// Send the last servo command as a sync pulse to the main controller.
if (servoIndex == 0)
puts (servoLastCommand | servoSelectFlag);
// In the next interrupt we'll turn off the current servo.
servoPortB &= ~servoMask[servoIndex];
servoTimerCompare += servoPosition[servoIndex];
servoState = SERVO_FALL_STATE;
// If the comm times out, then set default servo positions.
if (servoCommTimeOut != 0)
if (--servoCommTimeOut == 0)
for (i = 0; i < 4; ++i)
servoPosition[i] = SERVO_DEFAULT_POSITION;
// We'll turn off the LED
if (servoLEDTick == 0) {
switch (servoLEDState) {
case SERVO_LED_NORM_HIGH:
output_low (SERVO_LED);
if (servoCommTimeOut != 0) {
servoLEDState = SERVO_LED_NORM_LOW;
servoLEDTick = SERVO_LED_OFFTIME_NORM;
} else {
servoLEDState = SERVO_LED_FAULT_LOW;
servoLEDTick = SERVO_LED_OFFTIME_FAULT;
}
break;
case SERVO_LED_NORM_LOW:
output_high (SERVO_LED);
if (servoCommTimeOut != 0) {
servoLEDState = SERVO_LED_NORM_HIGH;
servoLEDTick = SERVO_LED_ONTIME_NORM;
} else {
servoLEDState = SERVO_LED_FAULT_HIGH;
servoLEDTick = SERVO_LED_ONTIME_FAULT;
}
break;
case SERVO_LED_FAULT_HIGH:
output_low (SERVO_LED);
if (servoCommTimeOut == 0) {
servoLEDState = SERVO_LED_FAULT_LOW;
servoLEDTick = SERVO_LED_OFFTIME_FAULT;
} else {
servoLEDState = SERVO_LED_NORM_LOW;
servoLEDTick = SERVO_LED_OFFTIME_NORM;
}
break;
case SERVO_LED_FAULT_LOW:
output_high (SERVO_LED);
if (servoCommTimeOut == 0) {
servoLEDState = SERVO_LED_FAULT_HIGH;
servoLEDTick = SERVO_LED_ONTIME_FAULT;
} else {
servoLEDState = SERVO_LED_NORM_HIGH;
servoLEDTick = SERVO_LED_ONTIME_NORM;
}
break;
} // END switch
} else
--servoLEDTick;
break;
case SERVO_FALL_STATE:
// The number of uS until we turn on the next servo.
servoTimerCompare += (SERVO_PRI - servoPosition[servoIndex]);
// Update the servo posotion.
servoPosition[servoIndex] = newServoPosition[servoIndex];
// Select the next servo in the list.
if (++servoIndex == 4)
servoIndex = 0;
// In the next interrupt, we'll turn on the next servo.
servoPortB |= servoMask[servoIndex];
servoState = SERVO_RISE_STATE;
break;
} // END switch
// Update the timer compare register for the next interrupt.
CCP_1 = servoTimerCompare;
}
/**
* Process the command stream. Byte 0 contains the control switch
* status. Bytes 1-4 contain the control position for servos
* 1 through 4.
*/
void servoSetCommand(uint8 *buffer)
{
uint8 i;
// We only need to update the servo enable/disable if they change.
if (buffer[0] != servoLastCommand) {
servoLastCommand = buffer[0];
// Bits 0:3 select servo status 1 through 4.
if ((servoLastCommand & 0x01) == 0x01) {
servoPortB |= 0x80;
servoTriState |= SERVO_OUT1;
} else {
servoPortB &= ~0x80;
servoTriState &= ~SERVO_OUT1;
}
if ((servoLastCommand & 0x02) == 0x02) {
servoPortB |= 0x04;
servoTriState |= SERVO_OUT2;
} else {
servoPortB &= ~0x04;
servoTriState &= ~SERVO_OUT2;
}
if ((servoLastCommand & 0x04) == 0x04) {
output_high (SERVO_SW3);
servoTriState |= SERVO_OUT3;
} else {
output_low (SERVO_SW3);
servoTriState &= ~SERVO_OUT3;
}
if ((servoLastCommand & 0x08) == 0x08) {
output_high (SERVO_SW4);
servoTriState |= SERVO_OUT4;
} else {
output_low (SERVO_SW4);
servoTriState &= ~SERVO_OUT4;
}
// Bits 4:5 select the aux outputs.
if ((servoLastCommand & 0x10) == 0x10)
output_high (SERVO_AUX1);
else
output_low (SERVO_AUX1);
if ((servoLastCommand & 0x20) == 0x20)
output_high (SERVO_AUX2);
else
output_low (SERVO_AUX2);
// Update the tri-state buffer.
set_tris_b (servoTriState);
}
// We'll save the new servo positions for application later.
for (i = 0; i < 4; ++i)
newServoPosition[i] = SERVO_MIN_PW + (((uint16) buffer[i + 1]) << 2);
servoCommTimeOut = SERVO_COMM_TIMEOUT_COUNT;
}
/**
* Class of commands to read and process the serial port commands.
*/
// Value that indicates the start of a new command stream.
#define COMMAND_HEADER 0xfe
// The number of bytes in a command stream.
#define COMMAND_LENGTH 5
// State machine constants.
#define COMMAND_WAIT_HEADER 0
#define COMMAND_DATA 1
#define COMMAND_CHECKSUM 2
// Private variables.
uint8 commandState, commandChecksum, commandIndex, commandBuffer[5];
/**
* Initialize the state machine.
*/
void commandConstructor()
{
commandState = COMMAND_WAIT_HEADER;
}
/**
* Read the serial port and process the command stream. The stream includes the
* header <b>COMMAND_HEADER 0xfe</b>, 5 command bytes, and a single byte checksum.
*/
void commandRead()
{
uint8 value;
// Read the serial port and return if it is empty.
if (!kbhit())
return;
value = getc();
switch (commandState) {
// When we find the header, reset the buffer pointer and checksum.
case COMMAND_WAIT_HEADER:
if (value == COMMAND_HEADER) {
commandState = COMMAND_DATA;
commandChecksum = 0x00;
commandIndex = 0;
} // END if
break;
// Accumulate the data bytes.
case COMMAND_DATA:
commandBuffer[commandIndex++] = value;
commandChecksum ^= value;
if (commandIndex == COMMAND_LENGTH)
commandState = COMMAND_CHECKSUM;
break;
// Verify the checksum and if it is good, update the servo information.
case COMMAND_CHECKSUM:
if (value == commandChecksum)
servoSetCommand (commandBuffer);
commandState = COMMAND_WAIT_HEADER;
break;
} // END switch
}
void main()
{
// Initialize all our classes.
servoConstructor();
commandConstructor();
// Setup our interrupts.
enable_interrupts(INT_CCP1);
enable_interrupts(GLOBAL);
while (1) {
commandRead();
servoCheckState();
} // END while
}