Servo.c
/****************************************************************************
Module
Servo.c
****************************************************************************/
/*----------------------------- Include Files -----------------------------*/
// Basic includes for a program using the Events and Services Framework
#include "ES_Configure.h"
#include "ES_Framework.h"
#include "ES_ServiceHeaders.h"
/*----------------------------- Module Defines ----------------------------*/
#define TckPerMicroS 3
/*---------------------------- Module Functions ---------------------------*/
/*---------------------------- Module Variables ---------------------------*/
static int aimmin = 55;
static int fidmin = 0;
static int ballmin = 100;
static int loadmin = 55;
static int aimmax = 125;
static int fidmax = 180;
static int ballmax = 250;
static int loadmax = 112;
/* PWM CLOCK IS SET AT /64 /8, SO IT IS 24MHz/64/8 = 46.875 kHz */
/* PWM PER = 250, ~5.33 ms */
/* Servos on 3 - Aim, 4 - Fiducial, 5 - Ball Spinning */
// 600usec = 0 degrees
// 1500usec = 90 degrees
// 2400usec = 180 degrees
void Servo_SetAngle(int num, int angle)
{
if(num == AimServo)
{
if (angle < aimmin) angle = aimmin;
if (angle > aimmax) angle = aimmax;
ServoPulseWidthCh1(AimServo, 600+10*angle);
}
if(num == FiducialServo)
{
if (angle < fidmin) angle = fidmin;
if (angle > fidmax) angle = fidmax;
ServoPulseWidthCh1(FiducialServo, 600+10*angle);
}
if(num == BallSpinServo)
{
if (angle < ballmin) angle = ballmin;
if (angle > ballmax) angle = ballmax;
ServoPulseWidthCh1(BallSpinServo, 0+10*angle);
}
if(num == LoadServo)
{
if (angle < loadmin) angle = loadmin;
if (angle > loadmax) angle = loadmax;
ServoPulseWidthCh1(LoadServo, 600+10*angle);
}
}
void ServoPulseWidthCh1(int PortNum, int NewWidth)
{
//Port Num is between 4-7
switch(PortNum)
{
case 4:
TIM1_TC4 = 0xFFFF - (NewWidth * TckPerMicroS);
break;
case 5:
TIM1_TC5 = 0xFFFF - (NewWidth * TckPerMicroS);
break;
case 6:
TIM1_TC6 = 0xFFFF - (NewWidth * TckPerMicroS);
break;
case 7:
TIM1_TC7 = 0xFFFF - (NewWidth * TckPerMicroS);
break;
}
}
Module
Servo.c
****************************************************************************/
/*----------------------------- Include Files -----------------------------*/
// Basic includes for a program using the Events and Services Framework
#include "ES_Configure.h"
#include "ES_Framework.h"
#include "ES_ServiceHeaders.h"
/*----------------------------- Module Defines ----------------------------*/
#define TckPerMicroS 3
/*---------------------------- Module Functions ---------------------------*/
/*---------------------------- Module Variables ---------------------------*/
static int aimmin = 55;
static int fidmin = 0;
static int ballmin = 100;
static int loadmin = 55;
static int aimmax = 125;
static int fidmax = 180;
static int ballmax = 250;
static int loadmax = 112;
/* PWM CLOCK IS SET AT /64 /8, SO IT IS 24MHz/64/8 = 46.875 kHz */
/* PWM PER = 250, ~5.33 ms */
/* Servos on 3 - Aim, 4 - Fiducial, 5 - Ball Spinning */
// 600usec = 0 degrees
// 1500usec = 90 degrees
// 2400usec = 180 degrees
void Servo_SetAngle(int num, int angle)
{
if(num == AimServo)
{
if (angle < aimmin) angle = aimmin;
if (angle > aimmax) angle = aimmax;
ServoPulseWidthCh1(AimServo, 600+10*angle);
}
if(num == FiducialServo)
{
if (angle < fidmin) angle = fidmin;
if (angle > fidmax) angle = fidmax;
ServoPulseWidthCh1(FiducialServo, 600+10*angle);
}
if(num == BallSpinServo)
{
if (angle < ballmin) angle = ballmin;
if (angle > ballmax) angle = ballmax;
ServoPulseWidthCh1(BallSpinServo, 0+10*angle);
}
if(num == LoadServo)
{
if (angle < loadmin) angle = loadmin;
if (angle > loadmax) angle = loadmax;
ServoPulseWidthCh1(LoadServo, 600+10*angle);
}
}
void ServoPulseWidthCh1(int PortNum, int NewWidth)
{
//Port Num is between 4-7
switch(PortNum)
{
case 4:
TIM1_TC4 = 0xFFFF - (NewWidth * TckPerMicroS);
break;
case 5:
TIM1_TC5 = 0xFFFF - (NewWidth * TckPerMicroS);
break;
case 6:
TIM1_TC6 = 0xFFFF - (NewWidth * TckPerMicroS);
break;
case 7:
TIM1_TC7 = 0xFFFF - (NewWidth * TckPerMicroS);
break;
}
}