Video Friday: Agile Upgrade
1 day ago
// Number of servos
// Due to timing limitations you can really only use 9 servos with a 4017
#define SERVOCOUNT 5
// This is 65536 minus number of clock ticks in 1 ms plus or minus a fudge factor
// This was determined using an oscilloscope
#define SERVOBASE 55067
// This union allows access to the servo timing as an integer and as two bytes
// This makes the interrupt code faster and the whole thing more stable
union ServoTimers
{
unsigned int position;
unsigned char byte[2];
} servotimer[SERVOCOUNT];
// This is the 12 bit servo value 0=>1ms 2048=>1.5ms 4096=>2ms
unsigned int servo[SERVOCOUNT];
// Servo trims can be used to align the center position of the servo a small ammount
signed int servotrim[SERVOCOUNT];
// Counter for sending the servo signals sequentially
unsigned char currentservo;
// Used to track the state of the Reset signal
BOOL servoReset;
// Used for pulsing the Clock signal
BOOL servoOn;
void HighPriorityISR()
{
// If Timer0 Interrupt is Flagged
if (INTCONbits.TMR0IF) {
// Reset Interrupt Flag
INTCONbits.TMR0IF = 0;
// If the servo has been reset hold C1 low for almost 20ms
if (servoReset) {
servoReset = 0;
TMR0H = 0xF8;
TMR0L = 0xAF;
LATCbits.LATC1 = 0;
// Otherwise bring Reset/C1 high briefly and start clocking
// servo data out via Timer 3
} else {
servoReset = 1;
TMR0H = 0xFF;
TMR0L = 0xF0;
TMR3H = 0xFF;
TMR3L = 0xF0;
currentservo=0;
LATCbits.LATC1 = 1;
}
}
if (PIR2bits.TMR3IF) {
PIR2bits.TMR3IF = 0;
// For each servo pulse set Clock/B7 high momentarily
// then hold B7 low for the current servos desired pulse width
if (currentservo < SERVOCOUNT) {
if (servoOn) {
TMR3H = servotimer[currentservo].byte[1];
TMR3L = servotimer[currentservo].byte[0];
LATBbits.LATB7 = 0;
servoOn = FALSE;
currentservo++;
} else {
TMR3H = 0xFF;
TMR3L = 0xF0;
LATBbits.LATB7 = 1;
servoOn = TRUE;
}
// Otherwise dont call this interrupt for a while
} else {
TMR3H = 0x00;
TMR3L = 0x00;
LATBbits.LATB7 = 0;
servoOn = FALSE;
}
}
}
void ServoInit(void)
{
int i;
servoReset = FALSE;
servoOn = FALSE;
// Center all servos and zero all trims
for (i=0; i<SERVOCOUNT; i++) {
servo[i] = 2048;
servotrim[i]=0;
}
// UpdateServos must be called after servo[i] or servotrim[i] is modified
UpdateServos();
currentservo = 0;
}
void UpdateServos(void) {
int i;
// Convert each 12 bit servo position into interrupt timing
// The maximum time for an interrupt to overflow and trigger
// with a given prescaler and clock is with TMRxH and TMRxL
// set to 0, a timer setting of 0xFFFE will trigger the interrupt instantly
for (i=0; i < SERVOCOUNT; i++) {
servotimer[i].position = SERVOBASE - (3*servo[i]) + servotrim[i];
}
}
I type plus plus plus
then I type eh tee haych zero
I hate serial