PrinceNai
Joined: 31 Oct 2016 Posts: 471 Location: Montenegro
|
FT90R servo test program |
Posted: Sun Dec 11, 2022 10:22 am |
|
|
Hi, below is a very simple test program for 9g FT90R 360 deg servo. No libraries, nothing. It uses Timer0 and CCP1 interrupts to generate pulses. It is done for 32MHz clock, for any other some modifications must be done. 8ms time window between two pulses.
Code: |
#include <18F46K22.h>
#device ADC=10
#FUSES NOWDT //No Watch Dog Timer
#FUSES NOPUT
#device ICD=TRUE
#use delay(internal=32000000)
#byte CCPR1H=getenv("SFR:CCPR1H")
#byte CCPR1L=getenv("SFR:CCPR1L")
#DEFINE SERVO_PIN PIN_A6
int16 DesiredPulseWidth = 900; // desired pulse width in us
int8 Right = TRUE;
int8 ChangeSpeed = 0;
// ************************************************************
// ************************************************************
#INT_TIMER0
void TIMER0_isr(void)
{
clear_interrupt(INT_CCP1); // because of possible different speeds between TMR0 and TMR1, Timer1 might have already set CCP1IF, so prevent jumping to the CCP1 interrupt
// 1 in DesiredPulseWidth means a pulse of 1us (with 32MHz clock)
// Meant to be used with real numbers from 900 - 2000
set_timer1((-8 * DesiredPulseWidth)); // preload Timer1 to trigger CCP1 interrupt in DesiredPulseWidth microseconds
enable_interrupts(INT_CCP1); // enable interrupt on CCP1 match
output_high(SERVO_PIN); // SERVO_PIN high, CCP1 interrupt will set it low
ChangeSpeed = 1; // remind main it has work to do
}
// ------------------------------------------------------------
#INT_CCP1
void CCP1_isr(void)
{
clear_interrupt(INT_CCP1);
disable_interrupts(INT_CCP1); // disable interrupt until TMR0 interrupt enables it in the next time frame
output_low(SERVO_PIN);
}
// ************************************************************
// ************************************************************
void main()
{
setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1); //at 32MHz 8,1 ms overflow, also time frame for servo cycle
setup_timer_1(T1_INTERNAL|T1_DIV_BY_1); //8,191875 ms overflow. 0,125us resolution
setup_ccp1(CCP_COMPARE_INT);
enable_interrupts(INT_TIMER0);
enable_interrupts(GLOBAL);
CCPR1H = 0; // preset CCP1 registers
CCPR1L = 0;
output_low(SERVO_PIN);
while(TRUE)
{
if(ChangeSpeed){
ChangeSpeed = 0; // reset flag
// SOME TEST, SPIN THE MOTOR LEFT AND RIGHT AT DIFFERENT SPEEDS
if(Right){
DesiredPulseWidth++;
if(DesiredPulseWidth == 2000){
Right = FALSE;
}
}
else{
DesiredPulseWidth--;
if(DesiredPulseWidth == 900){
Right = TRUE;
}
}
} // if ChangeSpeed
} // while TRUE
} //main
|
|
|