/* * Servo Controller (C) June 2008 JWBrown * Control 6 model-control servos via SPI * * the servos are old

Futaba FP-S128 */ #include <p18f452.h> #include <delays.h> #include <usart.h> /* for TRIS and PORT declarations */ /* Delays declarations */ /* Serial UART stuff */

/* Set configuration bits: * - set HS oscillator * - disable watchdog timer * - disable low voltage programming */ #pragma config OSC=HS #pragma config WDT=OFF #pragma config LVP=OFF // max value to send is 170 // minimum is 0 #define START_ANGLE 85 #define MAX_ANGLE 170 #define MIN_ANGLE 0 #define WAITING_COMMAND 1 #define WAITING_CHANNEL 2 #define WAITING_ANGLE 3 #define SERVO_CMD 180 #define SERVO_SUPPLY_ON 181 #define SERVO_SUPPLY_OFF 182 #define TRUE 1 #define FALSE 0 unsigned int position_array[6] = {START_ANGLE,START_ANGLE,START_ANGLE,START_ANGLE,START_ANGLE,START _ANGLE}; char tmr0lcopy; // Copy of sixteen-bit Timer0 used by LoopTime char tmr0hcopy; char intconcopy; // Copy of INTCON for LoopTime subroutine char state; unsigned int angle; unsigned int channel; int command_rdy; void InterruptHandlerHigh (void); // prototype for int handler void delay_us(int count);

//--------------------------------------------------------------------------// High priority interrupt vector #pragma code InterruptVectorHigh = 0x08 void InterruptVectorHigh (void) { _asm goto InterruptHandlerHigh //jump to interrupt routine _endasm } //--------------------------------------------------------------------------// High priority interrupt routine #pragma code #pragma interrupt InterruptHandlerHigh void InterruptHandlerHigh () { if (INTCONbits.TMR0IF) //check for TMR0 overflow { } } //---------------------------------------------------------------------------void DoCommands(void) { while (!INTCONbits.T0IF) // Wait until 20 milliseconds are up, polling UART/SPI while waiting { unsigned char rdy, rdyspi; if ((PIR1bits.SSPIF == 1) && (SSPSTATbits.BF)) // if byte received from SPI { rdyspi = SSPBUF; // get the byte from SPI PIR1bits.SSPIF = 0; if ( (state == WAITING_COMMAND) && (rdyspi > SERVO_CMD) ) { // handle atomic commands switch (rdyspi) { case SERVO_SUPPLY_ON: PORTBbits.RB7 = 0; // servo supply ON break; case SERVO_SUPPLY_OFF: PORTBbits.RB7 = 1; // servo supply

OFF break; } // following for debug only while(BusyUSART()); // wait till UART TX ready WriteUSART(rdyspi); rdyspi = 0; } else if ( (state == WAITING_COMMAND) && (rdyspi == SERVO_CMD) ) { PORTBbits.RB0 = 1; // light LED showing command in process // its a command if (state == WAITING_COMMAND) { state = WAITING_CHANNEL; rdyspi = 0; } } else { switch (state) { case WAITING_CHANNEL: channel = rdyspi; state = WAITING_ANGLE; rdyspi = 0; break; case WAITING_ANGLE: angle = rdyspi; state = WAITING_COMMAND; command_rdy = TRUE; rdyspi = 0; PORTBbits.RB0 = 0; // extinguish LED showing command complete // following for debug only WriteUSART(SERVO_CMD); while(BusyUSART()); // wait till UART TX ready WriteUSART(channel); while(BusyUSART()); // wait till UART TX ready WriteUSART(angle); break; } } }

} } /******************************* * LoopTime() * * This subroutine waits for Timer0 to complete a 20 millisecond count * sequence. It does so by waiting for sixteen-bit Timer0 to roll over. ******************************* */ void LoopTime(void) { // trim Bignum to set 20ms #define Bignum 40450 int saveit; DoCommands(); // perform commands from SPI/UART - routine constructed so as to take exactly 20ms intconcopy = INTCON; INTCONbits.GIEH = 0; from CPU INTCONbits.GIEL = 0; // Critical section // interrupts are disabled here tmr0lcopy = TMR0L; // Read 16-bit counter at this moment tmr0hcopy = TMR0H; tmr0lcopy += Bignum & 0x00FF; // add LSB if (STATUSbits.C) // If Carry, increment high byte tmr0hcopy++; tmr0hcopy += (Bignum>>8) & 0x00FF; // add MSB TMR0H = tmr0hcopy; TMR0L = tmr0lcopy; // Write 16-bit counter at this moment saveit = intconcopy & 0b11000000; // Reenable interrupts to CPU if enabled prior INTCON = INTCON | saveit; // to LoopTime // interrupts have been re-enabled if enabled INTCONbits.T0IF = 0; // Clear Timer0 flag } //////////////////////////////////// // Perform LoopTime() a given number // of times //////////////////////////////////// void DelayLoopTimes(const int num) // Save INTCON bits // Disable all interrupts

{ int i; for (i = 0; i < num; i++) { LoopTime(); } } void delay_ms(int count) { int i; for (i = 0; i < count; i++) { Delay1KTCYx(2); } } void delay_us(int count) { int i; for (i = 0; i < count; i++) { Delay1TCY(); } } void main (void) { unsigned int x; unsigned int y; state = WAITING_COMMAND; command_rdy = FALSE; // USART is used for debug purposes - leave out if required OpenUSART ( USART_TX_INT_OFF & USART_RX_INT_OFF & USART_ASYNCH_MODE & USART_EIGHT_BIT & USART_CONT_RX & USART_BRGH_HIGH, 0x40 ); PORTB = 0x80; // Servo supply off (ON is active low on RB7) TRISB = 0; // PORTB all output INTCON = 0x00; TMR0H = 0; TMR0L = 0; T0CON = 0b10000001; looptime of 20 ms // init serial expansion as slave // disable global interrupt // clear timer // clear timer // Set up Timer0 for a

SSPCON1 = 0b00100101; SSPSTAT = 0b01000000; TRISCbits.TRISC3 = 1; TRISCbits.TRISC4 = 1; PIR1bits.SSPIF = 0; PORTBbits.RB7 = 0; //

// // // //

CKP=0, CKE=1, RC3 is RC3 is

slave mode - no SS used SMP=0 input SCK for SPI input SDI for SPI

servo supply ON

PORTBbits.RB0 = 1; DelayLoopTimes(50); PORTBbits.RB0 = 0; // flash LED0 momentarily DelayLoopTimes(50); while(1) { if (command_rdy == TRUE) { // update channel info if ( (channel >= 0) && (channel < 6) ) { if ( (angle >= MIN_ANGLE) && (angle <= MAX_ANGLE) ) { position_array[channel] = angle; } } command_rdy = FALSE; } for(x = 0; x < 6; x++) // step through the array and send out the data to b1-b6 { y = position_array[x]; // get servo pulse length multiplier switch (x) // servo control line goes high { case 0: PORTBbits.RB1 = 1; break; case 1: PORTBbits.RB2 = 1; break; case 2: PORTBbits.RB3 = 1; break; case 3: PORTBbits.RB4 = 1; break; case 4: PORTBbits.RB5 = 1; break; case 5: PORTBbits.RB6 = 1; break; } delay_ms(1); // all servo pulses are at least 1ms in length delay_us(y << 1); // add discrete channel delay switch (x) // servo control line goes low

{ case 0: PORTBbits.RB1 break; case 1: PORTBbits.RB2 break; case 2: PORTBbits.RB3 break; case 3: PORTBbits.RB4 break; case 4: PORTBbits.RB5 break; case 5: PORTBbits.RB6 break; } } LoopTime(); // use up any time remaining of 20ms } } = 0; = 0; = 0; = 0; = 0; = 0;