/**************************************************************************** Module Servos.h Description Handles Turret, Shooter, and Gate servos ****************************************************************************/ /*----------------------------- Include Files -----------------------------*/ #include "GlobalHeader.h" #include "Servos.h" /*----------------------------- Module Defines ----------------------------*/ //turret positioning #define TURRET_STRAIGHT 2220 //calibrated #define TURRET_MIN 900 #define TURRET_MAX 3600 #define TURRET_SCALE -15 //ticks per degree #define SERVOS_PERIOD 30000 //20ms at 3MHz /*---------------------------- Module Variables ---------------------------*/ static unsigned int Servos_TurretOnTime = TURRET_STRAIGHT; unsigned int Servos_ShooterOnTime = Shooter_Reset; static unsigned int Servos_GateOnTime = Gate_Reset; /*------------------------------ Module Code ------------------------------*/ /**************************************************************************** Function ServosInit Parameters none Returns none Description Initialization for servos -- output compare timers and interrupts ****************************************************************************/ void ServosInit(void){ TIM1_TSCR1 |= _S12_TEN; TIM1_TSCR2 |= _S12_PR2;//16 = 1.5MHz. //(_S12_PR1 | _S12_PR0); //Prescale of 8. 3 MHz TIM1_TCTL1 |= (_S12_OM4 | _S12_OM5 | _S12_OM6); //clear line on interrupt TIM1_TCTL1 &=~(_S12_OL4 | _S12_OL5 | _S12_OL6); TIM1_TCTL1 &= ~_S12_OM7; //toggle port 7 every period TIM1_TCTL1 |= _S12_OL7; TIM1_OC7M |= (_S12_OC7M4 | _S12_OC7M5 | _S12_OC7M6); //Set Mask registers TIM1_OC7D |= (_S12_OC7D4 | _S12_OC7D5 | _S12_OC7D6); //make sure mask clears bits TIM1_TIOS |= (_S12_IOS4 | _S12_IOS5 | _S12_IOS6 | _S12_IOS7); //T(4-7) output compare TIM1_TIE |= (_S12_C4I | _S12_C5I | _S12_C6I | _S12_C7I); //enable interrupts TIM1_TC7 = TIM1_TCNT + SERVOS_PERIOD; //time next rising edge TIM1_TC4 = TIM1_TC7 + Servos_TurretOnTime; //time next falling edge for each servo TIM1_TC5 = TIM1_TC7 + Servos_ShooterOnTime; TIM1_TC6 = TIM1_TC7 + Servos_GateOnTime; TIM1_TFLG1 = (_S12_C4F | _S12_C5F | _S12_C6F | _S12_C7F); printf("Init done\r\n"); } /**************************************************************************** Function SetTurretServo Parameters signed char Angle (in degrees, left is +) Returns none Description Sets angle of turret servo, bounded by MIN and MAX ****************************************************************************/ void SetTurretServo (signed char Angle){ signed int OnTime; OnTime = (signed int)Angle * TURRET_SCALE + TURRET_STRAIGHT; OnTime = ((OnTime < TURRET_MIN) ? TURRET_MIN : OnTime); OnTime = ((OnTime > TURRET_MAX) ? TURRET_MAX : OnTime); Servos_TurretOnTime = (unsigned int)OnTime; } /**************************************************************************** Function SetShooterServo / SetGateServo Parameters unsigned int ...Position - pusle width, in 0.67us's Returns none Description sets Servo position based on provided pulse width ****************************************************************************/ void SetShooterServo (unsigned int ShooterPosition){ Servos_ShooterOnTime = ShooterPosition; } void SetGateServo (unsigned int GatePosition){ Servos_GateOnTime = GatePosition; } /**************************************************************************** Interrupt TurretInterrupt Description Creates falling edge for turret servo ****************************************************************************/ void interrupt _Vec_tim1ch4 TurretInterrupt(void) //port T4 { TIM1_TC4 += SERVOS_PERIOD; TIM1_TFLG1 = _S12_C4F; } /**************************************************************************** Interrupt ShooterInterrupt Description Creates falling edge for shooter servo ****************************************************************************/ void interrupt _Vec_tim1ch5 ShooterInterrupt(void) //port T5 { TIM1_TC5 += SERVOS_PERIOD; TIM1_TFLG1 = _S12_C5F; } /**************************************************************************** Interrupt GateInterrupt Description Creates falling edge for gate servo ****************************************************************************/ void interrupt _Vec_tim1ch6 GateInterrupt(void) //port T6 { TIM1_TC6 += SERVOS_PERIOD; TIM1_TFLG1 = _S12_C6F; } /**************************************************************************** Interrupt ServoPeriodInterrupt Description Creates rising edge for all three servos ****************************************************************************/ void interrupt _Vec_tim1ch7 ServoPeriodInterrupt(void) //port T6 { TIM1_TC4 = TIM1_TC7 + Servos_TurretOnTime; TIM1_TC5 = TIM1_TC7 + Servos_ShooterOnTime; TIM1_TC6 = TIM1_TC7 + Servos_GateOnTime; TIM1_TC7 += SERVOS_PERIOD; TIM1_TFLG1 = _S12_C7F; }