#include <S12e128bits.h> #include <mc9s12e128.h> #include <stdio.h> #include "Pins.h" #include "ServoControl.h" #define TIME_TO_TICKS 3 //microseconds to ticks multiplier /* Using Channel 4, 5, and 6 on Timer 0 for the servo */ void InitServo(void ){ // Timer Enable TIM0_TSCR1 |= _S12_TEN; // Set Clock divider to 8 TIM0_TSCR2 |= _S12_PR0 | _S12_PR1; TIM0_TSCR2 &= ~_S12_PR2; // Set to output capture TIM0_TIOS |= _S12_IOS4 | _S12_IOS5 | _S12_IOS6; // Set output line at output capture TIM0_TCTL1 |= _S12_OM4 | _S12_OL4; TIM0_TCTL1 |= _S12_OM5 | _S12_OL5; TIM0_TCTL1 |= _S12_OM6 | _S12_OL6; // Enable Timer Rollover Response TIM0_TTOV |= _S12_TOV4 | _S12_TOV5 | _S12_TOV6; // Start Launcher Servo at Closed GateServoToPosition(GATE_CLOSED_POSITION); // Start Lance at retracter LanceServoToPosition(RETRACTED_POSITION); SwingServoToPosition(FORWARD_POSITION); printf("Init Servo \n\r"); } /* Position as a percent of servo range */ void LanceServoToPosition(unsigned char position){ // Map position to ticks (position -> us -> ticks) unsigned int OC_Time = (position*20 + 530)*TIME_TO_TICKS; // Set Output Capture Time LANCE_SERVO_INPUT = 0xFFFF - OC_Time; //printf("OC_Time: %d \n\r", OC_Time); } /* Position as a percent of servo range */ void GateServoToPosition(unsigned char position){ // Map position to ticks (position -> us -> ticks) unsigned int OC_Time = (position*20 + 600)*TIME_TO_TICKS; // Set Output Capture Time GATE_SERVO_INPUT = 0xFFFF - OC_Time; //printf("OC_Time: %d \n\r", OC_Time); } /* Position as a percent of servo range */ void SwingServoToPosition(unsigned char position){ // Map position to ticks (position -> us -> ticks) unsigned int OC_Time = (position*20 + 520)*TIME_TO_TICKS; // Set Output Capture Time SWING_SERVO_INPUT = 0xFFFF - OC_Time; //printf("OC_Time: %d \n\r", OC_Time); }