Servo.c

//#define TEST
/****************************************************************************
 Module
     Servo.c
 
 Description
     Contains fuunction which initialize and run the servos
 
 Notes
 
 
 History
 When           Who     What/Why
 -------------- ---     --------
                NRM
****************************************************************************/
 
/*----------------------------- Include Files -----------------------------*/
#include <hidef.h>      /* common defines and macros */
#include <mc9s12e128.h>     /* derivative information */
#include <stdio.h>
#include <S12E128bits.h>
#include "ADS12.h"
#include "SCI.h"
#include "HC_Controller.h"
#include "EventChecker.h"
#include "Servo.h"
#include "S12eVec.h"
 
#pragma LINK_INFO DERIVATIVE "SampleS12"
/*----------------------------- Module Defines ----------------------------*/
#define PWM_PERIOD (255)
#define _PERIOD_Servo 180
#define _PERIOD_ 180
/*---------------------------- Module Functions ---------------------------*/
void Servo_Init(void);
void Servo_Set_Position(unsigned char,unsigned char);
/*------------------------------ Module Code ------------------------------*/
/****************************************************************************
 Function
     Servo_Init()
 
 Parameters
 
 
 Returns
     None.
 
 Description
     Initilizes E128 ports for the servos.. initialize PWM for servos..
     Intitializes timer system for turning the power off to the servos, if needed
 Notes
 
 
 Author
     Nick Musser
****************************************************************************/
void Servo_Init(void)
{
 
  DDRT |= BIT0HI;               // Output for servo power control
  PTT  |= BIT0HI;               // Initialize servo power to be on
 
 
  PWMPRCLK = (_S12_PCKA0 |_S12_PCKA1);// |_S12_PCKA2);  //Set PreScaler A to 2
  PWMSCLA = 140;      //Set Post scale factor A  - set at 140.  Period ~ 23ms
 
 
  //Crab Pot ID Servo - Port U0
  MODRR = MODRR|_S12_MODRR0;    //Set PWM1 (U1) to be a PWM port
  PWME = PWME|_S12_PWME0;       //Enable Pulse Width Channel on PWM1
  PWMPOL |= _S12_PPOL0;         //Sets polarity high duty cycle for PWM1
  PWMCLK = PWMCLK | _S12_PCLK0; //Set PWM1 to use clock SA
  PWMPER0 = PWM_PERIOD;         //Set Period of PWM1
  PWMDTY0 = 6;                  //Begin in zero position
 
  //Crab Rate Indicator - Port U1
  MODRR = MODRR|_S12_MODRR1;    //Set PWM1 (U1) to be a PWM port
  PWME = PWME|_S12_PWME1;       //Enable Pulse Width Channel on PWM1
  PWMPOL |= _S12_PPOL1;         //Sets polarity high duty cycle for PWM1
  PWMCLK = PWMCLK | _S12_PCLK1; //Set PWM1 to use clock SA
  PWMPER1 = PWM_PERIOD;         //Set Period of PWM1
  PWMDTY1 = 6;                  //Begin in zero position
 
 
    // Initialize Timer System
  //TIM2_TSCR1 = _S12_TEN;      // turn the timer system on
  TIM2_TSCR2 = (_S12_PR0 | _S12_PR1 | _S12_PR2 );  // set prescaler to
  TIM2_TIOS = _S12_IOS4;        // set cap/comp 4 to output compare
  TIM2_TCTL1 = TIM2_TCTL1 & ~(_S12_OL4 | _S12_OM4);  // no pin  connected
  TIM2_TC4 = TIM2_TCNT + _PERIOD_Servo; /* schedule first event */
  TIM2_TFLG1 = _S12_C4F;        // Clear 0C4 flag
 
}

/****************************************************************************
 Function
     Servo_Init()
 
 Parameters
     desired position and channel
 
 Returns
     None.
 
 Description
     there are 20 possible positions.  select 1-20
 Notes
 
 
 Author
     Nick Musser
****************************************************************************/
void Servo_Set_Position(unsigned char Position, unsigned char Channel) // feed it desired posistion (1-20)
{
  unsigned char HiTime;
  HiTime = Position + 5;
 
  // Guard against extreme conditions
  if(HiTime > 24)
    HiTime = 24;
  if (HiTime < 6)
    HiTime = 6;
 
  // assign hi time
  if(Channel == 0)
    PWMDTY0 = HiTime;
  if(Channel == 1)
    PWMDTY1 = HiTime;
 
  //TIM2_TC4 = TIM2_TCNT + _PERIOD_; // program next compare
  //TIM2_TFLG1 = _S12_C4F; // clear OC4 flag
  //TIM2_TIE |= _S12_C4I; // enable OC4 interrupt so we can start the timer
}
 
 
/****************************************************************************
 Function
     Timer
 
 Parameters
     None.
 
 Returns
     None.
 
 Description
     This is the interrupt response associated with the timer used for turning off
     the servo power.
 
 
 Notes
     This function may or may not be used....
 
 Author
     Nick Musser
****************************************************************************/
void interrupt _Vec_tim2ch4 Timer_Servo(void)
{
  TIM2_TFLG1 = _S12_C4F; // clear OC4 flag
  EnableInterrupts;
  TIM2_TIE &= (~_S12_C4I); // disable OC4 interrupt
  PTT  &= BIT0LO;  // Turn Off Servo power
}
 
/*------------------------------- Footnotes -------------------------------*/
#ifdef TEST
 
void main(void) {
  Servo_Init();
  Servo_Set_Position(15,0);
  Servo_Set_Position(15,1);
  while(1) {
  }
}
#endif
/*------------------------------ End of file ------------------------------*/