void actuators_init ( void ) {
/* select clock pin as MAT0.1 output */
IO0DIR |= _BV(SERVO_CLOCK_PIN);
SERVO_CLOCK_PINSEL |= SERVO_CLOCK_PINSEL_VAL << SERVO_CLOCK_PINSEL_BIT;
/* select reset pin as GPIO output */
IO1DIR |= _BV(SERVO_RESET_PIN);
PINSEL2 &= ~(_BV(3)); /* P1.25-16 are used as GPIO */
/* assert RESET */
IO1SET = _BV(SERVO_RESET_PIN);
/* DATA pin output */
IO1DIR |= _BV(SERVO_DATA_PIN);
/* enable match 1 interrupt */
T0MCR |= TMCR_MR1_I;
/* lower clock */
T0EMR &= ~TEMR_EM1;
/* set high on match 1 */
T0EMR |= TEMR_EMC1_2;
/* set first pulse in a while */
T0MR1 = START_TIMEOUT;
servos_4015_idx = _4015_NB_CHANNELS;
/* Set all servos at their midpoints */
/* compulsory for unaffected servos */
uint8_t i;
for( i=0 ; i < _4015_NB_CHANNELS ; i++ )
servos_values[i] = SERVOS_TICS_OF_USEC(1500);
}
void servos_4017_init ( void )
{
uint8_t i;
/* select clock pin as MAT0.0 output */
SERVO_CLOCK_IODIR |= _BV(SERVO_CLOCK_PIN);
SERVO_CLOCK_PINSEL |= SERVO_CLOCK_PINSEL_VAL << SERVO_CLOCK_PINSEL_BIT;
/* select reset pin as GPIO output */
SERVO_RESET_IODIR |= _BV(SERVO_RESET_PIN);
/* assert RESET */
SetBit(SERVO_RESET_IOSET, SERVO_RESET_PIN);
/* enable match 0 interrupt */
T0MCR |= TMCR_MR0_I;
/* assert clock */
T0EMR |= TEMR_EM0;
/* set low on match 0 */
T0EMR |= TEMR_EMC0_1;
/* set first pulse in a while */
T0MR0 = START_TIMEOUT;
servos_4017_idx = SERVOS_4017_NB_CHANNELS;
/* Set all servos at their midpoints */
/* compulsory for unaffected servos */
for( i=0 ; i < SERVOS_4017_NB_CHANNELS ; i++ )
servos_4017_values[i] = SERVOS_TICS_OF_USEC(1500);
}
void actuators_init ( void ) {
/* select clock pin as MAT0.1 output */
IO0DIR |= _BV(SERVO_CLOCK_PIN);
SERVO_CLOCK_PINSEL |= SERVO_CLOCK_PINSEL_VAL << SERVO_CLOCK_PINSEL_BIT;
/* select reset pin as GPIO output */
IO1DIR |= _BV(SERVO_RESET_PIN);
PINSEL2 &= ~(_BV(3)); /* P1.25-16 are used as GPIO */
// SERVO_RESET_PINSEL |= SERVO_RESET_PINSEL_VAL << SERVO_RESET_PINSEL_BIT;
/* assert RESET */
IO1SET = _BV(SERVO_RESET_PIN);
/* enable match 1 interrupt */
T0MCR |= TMCR_MR1_I;
#ifndef SERVOS_4017_CLOCK_FALLING
/* lower clock */
T0EMR &= ~TEMR_EM1;
/* set high on match 1 */
T0EMR |= TEMR_EMC1_2;
#else
/* assert clock */
T0EMR |= TEMR_EM1;
/* set low on match 1 */
T0EMR |= TEMR_EMC1_1;
#endif
/* set first pulse in a while */
T0MR1 = START_TIMEOUT;
servos_4017_idx = _4017_NB_CHANNELS;
/* Set all servos at their midpoints */
/* compulsory for unaffected servos */
uint8_t i;
for( i=0 ; i < _4017_NB_CHANNELS ; i++ )
servos_values[i] = SERVOS_TICS_OF_USEC(1500);
#ifdef SERVO_MOTOR
servos_values[SERVO_MOTOR] = SERVOS_TICS_OF_USEC(SERVO_MOTOR_NEUTRAL);
#endif
#ifdef SERVO_MOTOR_LEFT
servos_values[SERVO_MOTOR_LEFT] = SERVOS_TICS_OF_USEC(SERVO_MOTOR_LEFT_NEUTRAL);
#endif
#ifdef SERVO_MOTOR_RIGHT
servos_values[SERVO_MOTOR_RIGHT] = SERVOS_TICS_OF_USEC(SERVO_MOTOR_RIGHT_NEUTRAL);
#endif
#ifdef SERVO_HATCH
servos_values[SERVO_HATCH] = SERVOS_TICS_OF_USEC(SERVO_HATCH_NEUTRAL);
#endif
}
void actuators_init ( void ) {
/* PWM selected as IRQ */
VICIntSelect &= ~VIC_BIT(VIC_PWM);
/* PWM interrupt enabled */
VICIntEnable = VIC_BIT(VIC_PWM);
VICVectCntl3 = VIC_ENABLE | VIC_PWM;
/* address of the ISR */
VICVectAddr3 = (uint32_t)PWM_ISR;
/* set clock, data and reset pins as output */
IO0DIR |= _BV(SERV0_CLOCK_PIN);
IO1DIR |= _BV(SERV0_DATA_PIN) | _BV(SERV0_RESET_PIN);
IO0DIR |= _BV(SERV1_CLOCK_PIN);
IO1DIR |= _BV(SERV1_DATA_PIN) | _BV(SERV1_RESET_PIN);
/* Configure the two servo clocks as PWM */
SERV0_CLOCK_PINSEL |= SERV0_CLOCK_PINSEL_VAL << SERV0_CLOCK_PINSEL_BIT;
SERV1_CLOCK_PINSEL |= SERV1_CLOCK_PINSEL_VAL << SERV1_CLOCK_PINSEL_BIT;
/* set first pulse to be very long */
PWMMR0 = 0XFFFFFF;
PWMMR_SERV1 = 0XFFF;
PWMMR_SERV0 = 0X0;
/* commit above changes */
PWMLER = PWMLER_LATCH0 | PWMLER_LATCH_SERV0 | PWMLER_LATCH_SERV1;
/* enable interrupt on serv1 PWM match */
PWMMCR = PWMMCR_MR0R | PWMMCR_MRI_SERV1;
/* enable PWM ouptputs */
PWMPCR = PWMPCR_ENA_SERV0 | PWMPCR_ENA_SERV1;
/* Prescaler */
PWMPR = PWM_PRESCALER-1;
/* enable PWM timer counter and PWM mode */
PWMTCR = PWMTCR_COUNTER_ENABLE | PWMTCR_PWM_ENABLE;
/* Load failsafe values */
/* Set all servos at their midpoints */
/* compulsory for unaffected servos */
uint8_t i;
for( i=0 ; i < _4015_NB_CHANNELS ; i++ )
servos_values[i] = SERVOS_TICS_OF_USEC(1500);
#ifdef SERVO_GAZ
servos_values[SERVO_GAZ] = SERVOS_TICS_OF_USEC(SERVO_GAZ_NEUTRAL);
#endif
}
void actuators_init ( void ) {
/* PWM selected as IRQ */
VICIntSelect &= ~VIC_BIT(VIC_PWM);
/* PWM interrupt enabled */
VICIntEnable = VIC_BIT(VIC_PWM);
VICVectCntl3 = VIC_ENABLE | VIC_PWM;
/* address of the ISR */
VICVectAddr3 = (uint32_t)PWM_ISR;
/* PW5 pin (P0.21) used for PWM */
IO0DIR |= _BV(SERV1_CLOCK_PIN);
IO1DIR |= _BV(SERV1_DATA_PIN) | _BV(SERV1_RESET_PIN);
SERV1_CLOCK_PINSEL |= SERV1_CLOCK_PINSEL_VAL << SERV1_CLOCK_PINSEL_BIT;
/* set match5 to go of a long time from now */
PWMMR0 = 0XFFFFFF;
PWMMR_SERV1 = 0XFFF;
/* commit above change */
PWMLER = PWMLER_LATCH0 | PWMLER_LATCH_SERV1;
/* interrupt on PWMMR5 match */
PWMMCR = PWMMCR_MR0R | PWMMCR_MRI_SERV1;
/* enable PWM5 ouptput */
PWMPCR = PWMPCR_ENA_SERV1;
/* Prescaler */
PWMPR = PWM_PRESCALER-1;
/* enable PWM timer counter and PWM mode */
PWMTCR = PWMTCR_COUNTER_ENABLE | PWMTCR_PWM_ENABLE;
/* Load failsafe values */
/* Set all servos at their midpoints */
/* compulsory for unaffected servos */
uint8_t i;
for( i=0 ; i < _4015_NB_CHANNELS ; i++ )
servos_values[i] = SERVOS_TICS_OF_USEC(1500);
#ifdef SERVO_GAZ
servos_values[SERVO_GAZ] = SERVOS_TICS_OF_USEC(SERVO_GAZ_NEUTRAL);
#endif
}
