MotorDrive.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 ----------------------------*/
//Only for use on Port U on E128
#define angscale 60 //For twitching towards the target angle on turns
#define exactscale 100 //For exact turns
/*********Notes**********/
// PWM applied to MOTOR_1A and MOTOR_2A
// MOTOR_1B=0 and MOTOR_2B=0, Forward
// MOTOR_1B=0 and MOTOR_2B=1, CW
// MOTOR_1B=1 and MOTOR_2B=0, CCW
// MOTOR_1B=1 and MOTOR_2B=1, Reve
/*---------------------------- Module Functions ---------------------------*/
/*---------------------------- Module Variables ---------------------------*/
static int turntime;
/* PWM CLOCK IS SET AT /64 /8, SO IT IS 24MHz/64/8 = 46.875 kHz */
/* PWM PER = 250, ~5.33 ms */
/* 2ms = 90 degrees (check this)
1ms = 45 degrees */
void Stop(void )
{
printf("Stop\r\n");
//Active breaking for Motor 1
PTU &= ~(MOTOR_1B | MOTOR_2B);
//Set PWM polarity
PWMPOL |= (_S12_PPOL2 | _S12_PPOL3);
SetDuty(0, MOTOR_1A);
SetDuty(0, MOTOR_2A);
}
void RotateCCW(int angle, int exact)
{
printf("CCW %d\r\n",angle);
//Need to add timing aspect
PTU |= MOTOR_1B;
PTU &= ~(MOTOR_2B);
//Set PWM polarity
PWMPOL |= (_S12_PPOL2 | _S12_PPOL3);
if (exact == 0)
{
turntime = (angle*7); //set how long to turn (where 90% turn ~2ms)
} else
{
turntime = (angle*9);
}
if (turntime == 0) turntime = 1;
printf("turntime %d\r\n", turntime);
ES_Timer_SetTimer(1,turntime);
ES_Timer_StartTimer(1);
//Speed
SetDuty(60, MOTOR_1A);
SetDuty(40, MOTOR_2A);
}
void RotateCW(int angle, int exact)
{
printf("CW %d\r\n", angle);
//Set direction of wheels
PTU &= ~(MOTOR_1B);
PTU |= MOTOR_2B;
//Set PWM polarity
PWMPOL |= (_S12_PPOL2 | _S12_PPOL3);
if (exact == 0)
{
turntime = (angle*7); //set how long to turn (where 90% turn ~2ms)
} else
{
turntime = (angle*9);
}
if (turntime == 0) turntime = 1;
printf("turntime %d\r\n", turntime);
ES_Timer_SetTimer(1,turntime);
ES_Timer_StartTimer(1);
//Speed
SetDuty(40, MOTOR_1A);
SetDuty(60, MOTOR_2A);
}
void RotateCWInf(void)
{
//Set direction of wheels
PTU &= ~(MOTOR_1B);
PTU |= MOTOR_2B;
//Set PWM polarity
PWMPOL |= (_S12_PPOL2 | _S12_PPOL3);
SetDuty(40, MOTOR_1A);
SetDuty(60, MOTOR_2A);
}
void DriveReverse(void )
{
printf("Drive Reverse\r\n");
//Drive Reverse
PTU |= (MOTOR_1B | MOTOR_2B);
//PTU &= ~(MOTOR_1B | MOTOR_2B);
//Set PWM polarity
PWMPOL |= (_S12_PPOL2 | _S12_PPOL3);
//Drive at 40% Duty Cycle
SetDuty(60, MOTOR_1A);
SetDuty(60, MOTOR_2A);
}
void DriveForward(void )
{
printf("Drive Forward\r\n");
//Drive Forward
PTU &= ~(MOTOR_1B | MOTOR_2B);
//PTU |= (MOTOR_1B | MOTOR_2B);
//Set PWM polarity
PWMPOL |= (_S12_PPOL2 | _S12_PPOL3);
//Drive at 60% Duty Cycle
SetDuty(60, MOTOR_1A);
SetDuty(60, MOTOR_2A);
}
/*static void AlignBeacon(void )
{
requestAlign();
printf("Align Beacon\r\n");
//Rotate CW
PTU &= ~(MOTOR_1B);
PTU |= MOTOR_2B;
//Set PWM polarity
PWMPOL &= ~(_S12_PPOL1);
PWMPOL |= _S12_PPOL4;
//Depends on how fast we need to turn it (need to tune speed)
SetDuty1(70);
SetDuty4(70);
}
static void DriveUntilTape(void )
{
printf("Drive Until Tape\r\n");
requestTape();
//Drive Forward
PTU &= ~(MOTOR_1B);
PTU &= ~(MOTOR_2B);
PWMPOL &= ~(_S12_PPOL1);
PWMPOL &= ~(_S12_PPOL4);
//Drive at 100% Duty Cycle
SetDuty(100,MOTOR_1A);
SetDuty(100,MOTOR_2A);
}*/
void SetDuty(double DC, char channel)
{
//Guards to make sure PWM doesn't go too high and give motors 14-16V
if(channel == MOTOR_1A)
{
if (PTU & MOTOR_1B)
{
if (DC < 30)
{
DC = 30;
}
}
} else if (channel == MOTOR_2A)
{
if (PTU & MOTOR_2B)
{
if (DC < 30)
{
DC = 30;
}
}
}
if(channel == 1)
{
if (DC > 40)
{
DC = 40;
}
}
if(DC > 70)
{
DC = 70;
}
switch(channel)
{
case 1:
PWMDTY1 = (int)((250*DC)/100);
break;
case 2:
//printf("??");
PWMDTY2 = (int)((250*DC)/100);
break;
case 3:
//printf("???\r\n");
PWMDTY3 = (int)((250*DC)/100);
break;
case 4:
PWMDTY4 = (int)((250*DC)/100);
break;
case 5:
PWMDTY5 = (int)((250*DC)/100);
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 ----------------------------*/
//Only for use on Port U on E128
#define angscale 60 //For twitching towards the target angle on turns
#define exactscale 100 //For exact turns
/*********Notes**********/
// PWM applied to MOTOR_1A and MOTOR_2A
// MOTOR_1B=0 and MOTOR_2B=0, Forward
// MOTOR_1B=0 and MOTOR_2B=1, CW
// MOTOR_1B=1 and MOTOR_2B=0, CCW
// MOTOR_1B=1 and MOTOR_2B=1, Reve
/*---------------------------- Module Functions ---------------------------*/
/*---------------------------- Module Variables ---------------------------*/
static int turntime;
/* PWM CLOCK IS SET AT /64 /8, SO IT IS 24MHz/64/8 = 46.875 kHz */
/* PWM PER = 250, ~5.33 ms */
/* 2ms = 90 degrees (check this)
1ms = 45 degrees */
void Stop(void )
{
printf("Stop\r\n");
//Active breaking for Motor 1
PTU &= ~(MOTOR_1B | MOTOR_2B);
//Set PWM polarity
PWMPOL |= (_S12_PPOL2 | _S12_PPOL3);
SetDuty(0, MOTOR_1A);
SetDuty(0, MOTOR_2A);
}
void RotateCCW(int angle, int exact)
{
printf("CCW %d\r\n",angle);
//Need to add timing aspect
PTU |= MOTOR_1B;
PTU &= ~(MOTOR_2B);
//Set PWM polarity
PWMPOL |= (_S12_PPOL2 | _S12_PPOL3);
if (exact == 0)
{
turntime = (angle*7); //set how long to turn (where 90% turn ~2ms)
} else
{
turntime = (angle*9);
}
if (turntime == 0) turntime = 1;
printf("turntime %d\r\n", turntime);
ES_Timer_SetTimer(1,turntime);
ES_Timer_StartTimer(1);
//Speed
SetDuty(60, MOTOR_1A);
SetDuty(40, MOTOR_2A);
}
void RotateCW(int angle, int exact)
{
printf("CW %d\r\n", angle);
//Set direction of wheels
PTU &= ~(MOTOR_1B);
PTU |= MOTOR_2B;
//Set PWM polarity
PWMPOL |= (_S12_PPOL2 | _S12_PPOL3);
if (exact == 0)
{
turntime = (angle*7); //set how long to turn (where 90% turn ~2ms)
} else
{
turntime = (angle*9);
}
if (turntime == 0) turntime = 1;
printf("turntime %d\r\n", turntime);
ES_Timer_SetTimer(1,turntime);
ES_Timer_StartTimer(1);
//Speed
SetDuty(40, MOTOR_1A);
SetDuty(60, MOTOR_2A);
}
void RotateCWInf(void)
{
//Set direction of wheels
PTU &= ~(MOTOR_1B);
PTU |= MOTOR_2B;
//Set PWM polarity
PWMPOL |= (_S12_PPOL2 | _S12_PPOL3);
SetDuty(40, MOTOR_1A);
SetDuty(60, MOTOR_2A);
}
void DriveReverse(void )
{
printf("Drive Reverse\r\n");
//Drive Reverse
PTU |= (MOTOR_1B | MOTOR_2B);
//PTU &= ~(MOTOR_1B | MOTOR_2B);
//Set PWM polarity
PWMPOL |= (_S12_PPOL2 | _S12_PPOL3);
//Drive at 40% Duty Cycle
SetDuty(60, MOTOR_1A);
SetDuty(60, MOTOR_2A);
}
void DriveForward(void )
{
printf("Drive Forward\r\n");
//Drive Forward
PTU &= ~(MOTOR_1B | MOTOR_2B);
//PTU |= (MOTOR_1B | MOTOR_2B);
//Set PWM polarity
PWMPOL |= (_S12_PPOL2 | _S12_PPOL3);
//Drive at 60% Duty Cycle
SetDuty(60, MOTOR_1A);
SetDuty(60, MOTOR_2A);
}
/*static void AlignBeacon(void )
{
requestAlign();
printf("Align Beacon\r\n");
//Rotate CW
PTU &= ~(MOTOR_1B);
PTU |= MOTOR_2B;
//Set PWM polarity
PWMPOL &= ~(_S12_PPOL1);
PWMPOL |= _S12_PPOL4;
//Depends on how fast we need to turn it (need to tune speed)
SetDuty1(70);
SetDuty4(70);
}
static void DriveUntilTape(void )
{
printf("Drive Until Tape\r\n");
requestTape();
//Drive Forward
PTU &= ~(MOTOR_1B);
PTU &= ~(MOTOR_2B);
PWMPOL &= ~(_S12_PPOL1);
PWMPOL &= ~(_S12_PPOL4);
//Drive at 100% Duty Cycle
SetDuty(100,MOTOR_1A);
SetDuty(100,MOTOR_2A);
}*/
void SetDuty(double DC, char channel)
{
//Guards to make sure PWM doesn't go too high and give motors 14-16V
if(channel == MOTOR_1A)
{
if (PTU & MOTOR_1B)
{
if (DC < 30)
{
DC = 30;
}
}
} else if (channel == MOTOR_2A)
{
if (PTU & MOTOR_2B)
{
if (DC < 30)
{
DC = 30;
}
}
}
if(channel == 1)
{
if (DC > 40)
{
DC = 40;
}
}
if(DC > 70)
{
DC = 70;
}
switch(channel)
{
case 1:
PWMDTY1 = (int)((250*DC)/100);
break;
case 2:
//printf("??");
PWMDTY2 = (int)((250*DC)/100);
break;
case 3:
//printf("???\r\n");
PWMDTY3 = (int)((250*DC)/100);
break;
case 4:
PWMDTY4 = (int)((250*DC)/100);
break;
case 5:
PWMDTY5 = (int)((250*DC)/100);
break;
}
}