/****************************************************************************

Module
DriveMotorService.c
Revision
1.0.1
Description
This drives the Drive Motor for ME218B Project
****************************************************************************/
/*----------------------------- Include Files -----------------------------*/
/* include header files for this state machine as well as any machines at the
next lower level in the hierarchy that are sub-machines to this machine
*/
#include "ES_Configure.h"
#include "ES_Framework.h"
#include "DriveMotorService.h"
#include "PWMDrive.h"
#include "InputCapture.h"
#include "DeadReckoning.h"
// H files for HWREG()
#include <stdint.h>
#include <stdbool.h>
#include <stdio.h>
#include "inc/hw_types.h"
#include "inc/hw_memmap.h"
#include "inc/hw_gpio.h"
#include "inc/hw_sysctl.h"
#include "driverlib/sysctl.h"
#include "driverlib/gpio.h"
#include "driverlib/interrupt.h"
#include "utils/uartstdio.h"
#include <math.h>
// H files for interupts
#include "inc/hw_timer.h"
#include "inc/hw_nvic.h"
#include "bitdefs.h"
#include "Location.h"
#define ALL_BITS (0xff<<2)
#define HI_LIMIT 100
over-driven
#define LO_LIMIT 25
#define RPM_LIMIT 70
#define SUM_ERROR_LIMIT 2000
#define RampSumError 3500

// Doesn't allow motors to be

//Previous Values: 2800, 3000

/*----------------------------- Module Defines ----------------------------*/

#define EncoderNum 100
#define WheelRad 5
inches

// Number of Encoder Edges

// Wheel Radius in

// Prop. Gain Changes Depending on State, so this value is defined in

ControlLoop
//#define pGainL 1.10
// Proportional Gain for
Control Loop for testing
#define iGainL 0.03
// Integral Gain for
Control Loop

// // Prop. Gain Changes Depending on State, so this value is defined in

ControlLoop
//#define pGainR 1.1
// Proportional Gain for
Control Loop for testing
#define iGainR 0.03
// Integral Gain for
Control Loop
#define TicksPerMS 40000
40Mhz clock

// 40,000 ticks per mS assumes a

#define UpdateTime 100

updates

// Update time for Period

/*---------------------------- Module Functions ---------------------------*/

/* prototypes for private functions for this service.They should be functions
relevant to the behavior of this service
*/
void writeMotorDirection(int Dir);
// wries Motor
Direction
void InitControlLoop(void);
//
initialization for ControlLoop
/*---------------------------- Module Variables ---------------------------*/
// with the introduction of Gen2, we need a module level Priority variable
//static uint8_t MyPriority;
static int RPMComL = 0;
// holds RPM Command for Left
Wheel
static int RPMComR = 0;
// holds RPM Command for Right
Wheel
static int curRPMLeft;
// holds current RPM for Left
Wheel
static int curRPMRight;
// holds current RPM for Right
Wheel
static int SumErrorL = 0.0;
// error integrator
static int SumErrorR = 0.0;
// error integrator
/*------------------------------ Module Code ------------------------------*/
/****************************************************************************
Function
InitDriveMotorService
Parameters
uint8_t : the priorty of this service
Returns
bool, false if error in initialization, true otherwise
Description
Saves away the priority, and does any
other required initialization for this service
Notes
Author
****************************************************************************/
bool InitDriveMotorService ( void )
{
// ES_Event ThisEvent;
// only needed in Test Harness
//

MyPriority = Priority;

// only needed in Test Harness

// Initialize PWM for PB6 & PB7; Set wheels to have 0 Speed
InitPWMDrive();
// Initialize PWM

for Drive Motors

SetPWMDriveLeft(0);
// Set PWM Left to 0
SetPWMDriveRight(0);
// Set PWM Right to 0
InitControlLoop();
// Initialize
Control Loop
InitDeadReckoning();
// Initialize Dead
Reckoning Mod
printf("InitDriveMotorService Complete\r\n");
ES_Timer_InitTimer(DriveMotorUpdateTimer,UpdateTime);
// Start
UpdateTimer
RPMComL = 0;
RPMComR = 0;

// Set RPMComL to 0
// Set RPMComR to 0

return true;

/****************************************************************************
Function
RunDriveMotorService
Parameters
ES_Event : the event to process
Returns
ES_Event, ES_NO_EVENT if no error ES_ERROR otherwise
Description
Run function for DriveMotorService
Only needed in Test Harness
****************************************************************************/
ES_Event RunDriveMotorService( ES_Event ThisEvent )
{
ES_Event ReturnEvent;
ReturnEvent.EventType = ES_NO_EVENT; // assume no errors
if (ThisEvent.EventType == ES_TIMEOUT && ThisEvent.EventParam
==DriveMotorUpdateTimer) // Update Timer Expires
{
// restart Update Timer
ES_Timer_InitTimer(DriveMotorUpdateTimer,UpdateTime);
}
// Keyboard debugger
if (ThisEvent.EventType == ES_NEW_KEY)
{
// Add 20 to RPMComR if L key is pressed
if (ThisEvent.EventParam == 'L')
{
int newRPMComL = RPMComL + 20;
// Reset RPMCom if greater than 50
if (newRPMComL > 50)
newRPMComL = -50;
// Set RPMComL
RPMComL = newRPMComL;
}
// Add 20 to RPMComR if R key is pressed
if (ThisEvent.EventParam == 'R')
{
int newRPMComR = RPMComR + 20;
// Reset RPMCom if greater than 50

if (newRPMComR > 50)

newRPMComR = -50;
// Set RPMComR
RPMComR = newRPMComR;
}

return ReturnEvent;
}
/***************************************************************************
public functions
***************************************************************************/
// Sets New Command RPM for Left Motor
void NewRPMCommandLeft (int NewRPM)
{
// Set RPM_LIMIT
if(NewRPM > RPM_LIMIT) {
NewRPM = RPM_LIMIT;
}
// Set Negative RPM_LIMIT is NewRPM is negative
else if(NewRPM < -RPM_LIMIT) {
NewRPM = -RPM_LIMIT;
}
// Set new Command
RPMComL = NewRPM;
return;
}
// Sets New Command RPM for Right Motor
void NewRPMCommandRight (int NewRPM)
{
// Set RPM_LIMIT
if(NewRPM > RPM_LIMIT) {
NewRPM = RPM_LIMIT;
}
// Set Negative RPM_LIMIT is NewRPM is negative
else if(NewRPM < -RPM_LIMIT) {
NewRPM = -RPM_LIMIT;
}
// Set new Command
RPMComR = NewRPM;
return;
}
// Getter Function for other modules
int GetRPMComL( void ) {
return RPMComL;
}
// Getter Function for other modules
int GetRPMComR( void ) {
return RPMComR;
}
// function to stop motors
void StopMotors(void) {
// Set RPMComL & RPMComR to 0
NewRPMCommandLeft(0);
NewRPMCommandRight(0);
// Set PWM for Left & Right to 0
SetPWMDriveLeft(0);

SetPWMDriveRight(0);

// Function to reset SumError for both wheels

void ResetSumError(void) {
// set Sum Error to 0
SumErrorL = 0;
SumErrorR = 0;
// set PWM to 0
SetPWMDriveRight(0);
SetPWMDriveLeft(0);
}
/***************************************************************************
private functions
***************************************************************************/
// Function to Initialize Control loop timers
// This uses Wide Timer 1A
void InitControlLoop(void)
{
volatile uint32_t Dummy; // use volatile to avoid over-optimization
// start by enabling the clock to the timer (Wide Timer 1)
HWREG(SYSCTL_RCGCWTIMER) |= SYSCTL_RCGCWTIMER_R1;
// kill a few cycles to let the clock get going
Dummy = HWREG(SYSCTL_RCGCGPIO);
// make sure that timer (Timer A&B) is disabled before configuring
HWREG(WTIMER1_BASE+TIMER_O_CTL) &= ~TIMER_CTL_TAEN;
// set it up in 32bit wide (individual, not concatenated) mode
// the constant name derives from the 16/32 bit timer, but this is a 32/64
// bit timer so we are setting the 32 bit mode
HWREG(WTIMER1_BASE+TIMER_O_CFG) = TIMER_CFG_16_BIT;
// set up timer A in periodic mode so that it repeats the time-outs
HWREG(WTIMER1_BASE+TIMER_O_TAMR) =
(HWREG(WTIMER1_BASE+TIMER_O_TAMR)& ~TIMER_TAMR_TAMR_M)|
TIMER_TAMR_TAMR_PERIOD;
// set timeout to 3mS
HWREG(WTIMER1_BASE+TIMER_O_TAILR) = TicksPerMS * 3;
// enable a local timeout interrupt
HWREG(WTIMER1_BASE+TIMER_O_IMR) |= TIMER_IMR_TATOIM;
// enable the Timer A in Wide Timer 1 interrupt in the NVIC
// it is interrupt number 96 so apppears in EN3 at bit 0
HWREG(NVIC_EN3) = BIT0HI;
// make sure interrupts are enabled globally
__enable_irq();
// now kick the timer off by enabling it and enabling the timer to
// stall while stopped by the debugger
HWREG(WTIMER1_BASE+TIMER_O_CTL) |= (TIMER_CTL_TAEN | TIMER_CTL_TASTALL);
printf("InitControlLoop for Motors Complete\r\n");
return;
}

//// ISR to output new Duty Cycle

void ControlLaw(void)
{
// Declare Control Variables for Left Wheel
static int RPMErrorL;
static int ReqDutyL;

// make static for speed

// make static for speed

// Declare Control Variables for Right Wheel

static
static
static
static

int RPMErrorR;
int ReqDutyR;
float pGainL;
float pGainR;

//
//
//
//

make static for speed

make static for speed
prop. gain for left wheel
prop. gain for right wheel

// Change gains for GoingToPosition7 State

if(QueryLocationSM() == GoingToPosition7) {
pGainL = 1.5;
pGainR = 1.5;
}
// Set Default gains
else
{
pGainL = 0.2;
pGainR = 0.2;
}
// start by clearing the source of the interrupt
HWREG(WTIMER1_BASE+TIMER_O_ICR) = TIMER_ICR_TATOCINT;
//--------------------------------------Left
Motor-------------------------------------curRPMLeft = GetRPMLeft();
current RPM from InputCapture Mod

Error

// Calculate Error
// if Command is negative, set RPMCom negative
if(RPMComL < 0) {
RPMErrorL = -RPMComL - curRPMLeft;
}
// if Command is positive, keep RPMCom positive
else {
RPMErrorL = RPMComL - curRPMLeft;

// store

// Calculate RPM

}
SumErrorL += RPMErrorL;
integrator term

// sum for

// Reset Sum Error if GoingToPosition7 & SumErrorLimit is Reached

if( (abs(SumErrorL) > SUM_ERROR_LIMIT) &&
QueryLocationSM() != GoingToPosition7 )
{
SumErrorL = 0;
// reset SumErrorL
}
ReqDutyL = (pGainL * RPMErrorL)+(iGainL * SumErrorL);
Requested Duty, PI controller
// If RPMComL is positive
if(RPMComL >= 0) { //RPMComL
// Set Direction pin LO

// calculate

HWREG(GPIO_PORTF_BASE + (GPIO_O_DATA + ALL_BITS)) &= BIT3LO;

// Keep ReqDutyL below HI_LIMIT
if (ReqDutyL > HI_LIMIT)
{
ReqDutyL = HI_LIMIT;
SumErrorL -= RPMErrorL;
// anti-windup
//printf("rpm error left: %d, sum error: %d, rpm:
%d\r\n",RPMErrorL,SumErrorL, curRPMLeft);
}
// Keep ReqDutyL non-negative
else if (ReqDutyL < 0)
{
ReqDutyL = 0;
SumErrorL -= RPMErrorL;
// anti-windup
}
} else
// if RPMComL is negative
{
// Set Direction Pin HI
HWREG(GPIO_PORTF_BASE + (GPIO_O_DATA + ALL_BITS)) |= BIT3HI;
// Keep ReqDutyL below HI_LIMIT
if (ReqDutyL > HI_LIMIT)
{
ReqDutyL = HI_LIMIT;
SumErrorL -= RPMErrorL;
// anti-windup
}
// Keep ReqDutyL non-negative
else if (ReqDutyL < 0)
{
ReqDutyL = 0;
SumErrorL -= RPMErrorL;
// anti-windup
}
}
SetPWMDriveLeft(ReqDutyL);
//
output Requested Duty Cycle
//--------------------------------------Right
Motor-------------------------------------curRPMRight = GetRPMRight();
// store current RPM from
InputCapture Mod
// Calculate Error
// if Command is negative, set RPMCom negative
if(RPMComR < 0) {
RPMErrorR = -RPMComR - curRPMRight;
} else
// if Command is negative, set RPMComR positive
{
RPMErrorR = RPMComR - curRPMRight;
//
Calculate RPM Error
}
SumErrorR += RPMErrorR;
sum for integrator term
//printf("sum error right: %d \r\n", SumErrorR);
// if state is GoingToPosition7 && SUM_ERROR_LIMIT is reached
if( (abs(SumErrorR) > SUM_ERROR_LIMIT)
&& QueryLocationSM() != GoingToPosition7 )
{
SumErrorR = 0;
// reset SumErrorR

//

}
// If RPMComL is positive
ReqDutyR = (pGainR * RPMErrorR)+(iGainR * SumErrorR);
Requested Duty, PI controller

// calculate

// If PWM is saturated, we only give commands DutyCycle: 0-100%

if(RPMComR >= 0) { //RPMComR
// Set Direction Pin LO
HWREG(GPIO_PORTF_BASE + (GPIO_O_DATA + ALL_BITS)) &= BIT4LO;
// Keep ReqDutyR below HI_LIMIT
if (ReqDutyR > HI_LIMIT)
{
ReqDutyR = HI_LIMIT;
SumErrorR -= RPMErrorR;
// anti-windup
//printf("rpm error right: %d, sum error: %d, rpm:
%d\r\n",RPMErrorR,SumErrorR, curRPMRight);
}
else // Keep ReqDutR non-negative
if (ReqDutyR < 0)
{
ReqDutyR = 0;
SumErrorR -= RPMErrorR;
// anti-windup
}
} else {
// Set Direction Pin HI
HWREG(GPIO_PORTF_BASE + (GPIO_O_DATA + ALL_BITS)) |= BIT4HI;
// Keep ReqDutyR below HI_LIMIT
if (ReqDutyR > HI_LIMIT)
{
ReqDutyR = HI_LIMIT;
SumErrorR -= RPMErrorR;
// anti-windup
}
else // Keep ReqDutR non-negative
if (ReqDutyR < 0)
{
ReqDutyR = 0;
SumErrorR -= RPMErrorR;
// anti-windup
}
}
SetPWMDriveRight(ReqDutyR);
}

return;

/*------------------------------- Footnotes -------------------------------*/

/*------------------------------ End of file ------------------------------*/