Scribd
Upload
170 views8 pages

Linefollowingsm

This C module contains functions for line following control. It initializes line following interrupts and AD conversion, defines constants for line sensing thresholds, and contains functions for enabling/disabling line following control and checking line sensor readings to detect when the line is on or aligned. The main function runs a line following state machine that controls motor duty cycles based on sensor readings to follow the line.

Uploaded by

api-398062839
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as PDF, TXT or read online on Scribd
170 views8 pages

Linefollowingsm

This C module contains functions for line following control. It initializes line following interrupts and AD conversion, defines constants for line sensing thresholds, and contains functions for enabling/disabling line following control and checking line sensor readings to detect when the line is on or aligned. The main function runs a line following state machine that controls motor duty cycles based on sensor readings to follow the line.

Uploaded by

api-398062839
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as PDF, TXT or read online on Scribd
You are on page 1/ 8

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

Module
LineFollowingSM.c

Description
This module contains the line following functions.

****************************************************************************/
/*----------------------------- Include Files -----------------------------*/
/* include header files for the framework and this service
*/
#include "ES_Configure.h"
#include "ES_Framework.h"
#include "ES_DeferRecall.h"

#include "inc/hw_memmap.h"
#include "inc/hw_types.h"
#include "inc/hw_gpio.h"
#include "inc/hw_sysctl.h"
#include "inc/hw_ssi.h"
#include "driverlib/sysctl.h"
#include "driverlib/pin_map.h" // Define PART_TM4C123GH6PM in project
#include "driverlib/gpio.h"
#include "termio.h"
#include "hw_nvic.h"
#include "hw_pwm.h"
#include "hw_timer.h"
#include "hw_ssi.h"

#include "MotorDrive.h"
#include "ADMulti.h"
#include "LineFollowingSM.h"
#include "MasterSM.h"
#include "math.h"
#include "MotorEncoders.h"

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


#define TicksPerMS 40000
#define NOMINAL_DC 43 //60

#define LINE_ON_THRESHOLD 3500 //2.4


#define LINE_ALIGNED_THRESHOLD 3000 //2.4 // line sensing threshold [V]
#define LINE_ALIGNED_DIFF_THRESHOLD 100 //line sensing diff threshold [ADC ticks]
#define COUNTS_PER_VOLT 1241.0

#define SAMPLE_TIME 10
#define DUTY_ADD_TOL 5

// Motor rotation direction looking from wheel side


#define CW 1
#define CCW 0

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


void InitLineFollowingIntResponse(void);

static void EnableLineFollowing(void);


static void DisableLineFollowing(void);

static ES_Event_t DuringFollowingTheLine(ES_Event_t Event);

/*---------------------------- Module Variables ---------------------------*/


static LineFollowingState_t CurrentState;
// Target magnetic field detection difference (TargetDelta) is 0.0
static float TargetDelta = 0.0;
static float LastDelta;
static float Delta;
// PI gains
//static const float iGain = 0.0;
//static const float pGain = 0.07;
//static const float dGain = .7;
//// term for moving average
//static const float BETA = 0.7;

// PI gains
static const float iGain = 0.0;
static const float pGain = 0.08;
static const float dGain = 0.5;
// term for moving average
static const float BETA = 0.7;

// PI terms
static float IntegralTerm = 0.0;
static float DerivativeTerm = 0.0;
static float DutyAdd = 0.0;
// nominal Duty Cycle of the motors at 30% for line following
static float LMotorDC = NOMINAL_DC;
static float RMotorDC = NOMINAL_DC;

// Event Checker variables


static uint32_t CurrentSensorReading_Left;
static uint32_t CurrentSensorReading_Right;
static uint32_t LastSensorReading_Left = 0;
static uint32_t LastSensorReading_Right = 0;
static uint32_t CurrentSensorReading_Diff;
static bool LCR_CONTROL = false;

static bool AllowLineOn = true;


static bool AllowLineAlign = false;

// Get AD readings of the sensors


static uint32_t LineSensors[2];

/*------------------------------ Module Code ------------------------------*/


/****************************************************************************
Function
InitLineFollowingSM

Parameters
uint8_t: the priorty of this service

Returns
boolean: True if no error, False otherwise

Description
Saves away the priority, and starts
the top level state machine. Hardware init for LineFollowingSM
Notes

Author
S. Park
****************************************************************************/
void InitLineFollowingSM(void)
{
//Initialize the line following control
InitLineFollowingIntResponse();
// Initialize the AD ports (PE0 and PE1)
ADC_MultiInit(2);
}

/****************************************************************************
Function
RunLineFollowingSM

Parameters
ES_Event_t ThisEvent, event to process

Returns
ES_Event_t ThisEvent, event to return
Description
Posts an event to the queue of the SM
Notes

Author
S. Park
****************************************************************************/
ES_Event_t RunLineFollowingSM(ES_Event_t CurrentEvent)
{
bool MakeTransition = false;/* are we making a state transition?
*/
LineFollowingState_t NextState = CurrentState;
ES_Event_t EntryEventKind = { ES_ENTRY, 0 }; // default to normal entry
to new state
ES_Event_t ReturnEvent = { ES_NO_EVENT, 0 }; // assume no error

switch (CurrentState)
{
case FollowingTheLine: // If current state is TurningLeft
{ // Execute During function for TurningLeft
ReturnEvent = CurrentEvent = DuringFollowingTheLine(CurrentEvent);
//process any events
if (CurrentEvent.EventType != ES_NO_EVENT) //If an event is active
{
//If both bumpers are hit
if ((CurrentEvent.EventType == LEFT_BUMPER_HIT) || (CurrentEvent.EventType
== RIGHT_BUMPER_HIT))
{
// Reload station reached, stop motors
StopDrive();
// Transition to Line Following Complete
ReturnEvent.EventType = LINE_FOLLOW_COMPLETE;
}
}
}
break;
}

// If we are making a state transition


if (MakeTransition == true)
{
// Execute exit function for current state
CurrentEvent.EventType = ES_EXIT;
RunLineFollowingSM(CurrentEvent);

CurrentState = NextState; //Modify state variable

// Execute entry function for new state


// this defaults to ES_ENTRY
RunLineFollowingSM(EntryEventKind);
}

// in the absence of an error the top level state machine should


// always return ES_NO_EVENT, which we initialized at the top of func
return ReturnEvent;
}

/****************************************************************************
Function
StartLineFollowingSM

Parameters
ES_Event_t CurrentEvent

Returns
nothing

Description
Does any required initialization for this state machine
Notes

Author
S. Park
****************************************************************************/
void StartLineFollowingSM(ES_Event_t CurrentEvent)
{
// Start with motors stopped
StopDrive();
EnableLineFollowing();

//Initialize to Waiting2LineFollow
CurrentState = FollowingTheLine;

// Run the line following service


RunLineFollowingSM(CurrentEvent);

return;
}

/***************************************************************************
private functions
***************************************************************************/

static ES_Event_t DuringFollowingTheLine(ES_Event_t Event)


{
//During function for stopped
ES_Event_t ReturnEvent = Event; // assme no re-mapping or comsumption

// process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events


if ((Event.EventType == ES_ENTRY) ||
(Event.EventType == ES_ENTRY_HISTORY))
{
// Drive Forward using Line Following control
EnableLineFollowing();
}
else if (Event.EventType == ES_EXIT)
{
// Stop Line Following control
DisableLineFollowing();
}
else
{
// No standard during actions
}
// return Event without remapping
return ReturnEvent;
}

static void EnableLineFollowing(void)


{
//Reset line following control variables
LastDelta = 0.0;
IntegralTerm = 0.0;
DutyAdd = 0.0;
LMotorDC = NOMINAL_DC;
RMotorDC = NOMINAL_DC;

//Enable timer interrupt and encoder timeouts


SetDriving(true);
LCR_CONTROL = true;
}

static void DisableLineFollowing(void)


{
//Disable timer interrupt
//HWREG(NVIC_DIS3) = BIT2HI;
LCR_CONTROL = false;
SetDriving(false);
}

bool Check4LineOn(void)
{
// ES_Event_t LineEvent;
// // Get AD readings of the sensors
//
// CurrentSensorReading_Left = LineSensors[0];
// CurrentSensorReading_Right = LineSensors[1];

// printf("%u\t%u\n\r",LineSensors[0],LineSensors[1]);
// if ((CurrentSensorReading_Left != LastSensorReading_Left)
// || (CurrentSensorReading_Right != LastSensorReading_Right))
// {
// if (AllowLineOn)
// {
// // check if either sensor is above threshold
// if ((CurrentSensorReading_Left > LINE_ON_THRESHOLD*COUNTS_PER_VOLT)
// || (CurrentSensorReading_Right > LINE_ON_THRESHOLD*COUNTS_PER_VOLT))
// {
// AllowLineOn = false;
// printf("found line \r\n");
// LineEvent.EventType = LINE_ON;
// PostMasterSM(LineEvent);
// return true;
// }
// }
// }
//
//
// LastSensorReading_Left = CurrentSensorReading_Left;
// LastSensorReading_Right = CurrentSensorReading_Right;

return false;
}

bool Check4LineAlign(void)
{
// ES_Event_t LineEvent;
// // Get AD readings of the sensors
//
// //TODO: UNCOMMENT IF WE WANT
//
// CurrentSensorReading_Left = LineSensors[0];
// CurrentSensorReading_Right = LineSensors[1];
// CurrentSensorReading_Diff = abs(CurrentSensorReading_Left -
CurrentSensorReading_Right);
// //printf("%u\t%u\t%u\r\n",CurrentSensorReading_Left,
CurrentSensorReading_Right, CurrentSensorReading_Diff);
//
// if (AllowLineAlign)
// {
// if ((CurrentSensorReading_Left != LastSensorReading_Left)
// || (CurrentSensorReading_Right != LastSensorReading_Right))
// {
// // took out && (CurrentSensorReading_Diff<LINE_ALIGNED_DIFF_THRESHOLD)
// // check if either sensor is above 1.9V and values are equal
// if (
// ((CurrentSensorReading_Left > LINE_ALIGNED_THRESHOLD*COUNTS_PER_VOLT)
// || (CurrentSensorReading_Right >
LINE_ALIGNED_THRESHOLD*COUNTS_PER_VOLT))
// )
// {
// AllowLineAlign = false;
// printf("LINE_ALIGNED\r\n");
// LineEvent.EventType = LINE_ALIGNED;
// PostMasterSM(LineEvent);
// return true;
// }
// }
// }

// LastSensorReading_Left = CurrentSensorReading_Left;
// LastSensorReading_Right = CurrentSensorReading_Right;

return false;
}

void InitLineFollowingIntResponse(void)
{
// enable clock to the timer (Wide Timer 2)
HWREG(SYSCTL_RCGCWTIMER) |= SYSCTL_RCGCWTIMER_R2;
// kill a few cycles
while ((HWREG(SYSCTL_PRWTIMER) & SYSCTL_PRWTIMER_R2) != SYSCTL_PRWTIMER_R2)
{}

// disable Timer A before configuring


HWREG(WTIMER2_BASE + TIMER_O_CTL) &= ~TIMER_CTL_TAEN;

// set up in 32bit wide (individual, not concatenated) mode


HWREG(WTIMER2_BASE + TIMER_O_CFG) = TIMER_CFG_16_BIT;

// set up timer A in periodic mode


HWREG(WTIMER2_BASE + TIMER_O_TAMR) =
(HWREG(WTIMER2_BASE + TIMER_O_TAMR) & ~TIMER_TAMR_TAMR_M) |
TIMER_TAMR_TAMR_PERIOD;

// set timeout to 2ms


HWREG(WTIMER2_BASE + TIMER_O_TAILR) = TicksPerMS * SAMPLE_TIME;

// enable a local timeout interrupt


HWREG(WTIMER2_BASE + TIMER_O_IMR) |= TIMER_IMR_TATOIM;

// enable the Timer B in Wide Timer 2 interrupt in the NVIC


//HWREG(NVIC_EN3) = BIT2HI;
//HWREG(NVIC_PRI24) |= BIT22HI;
HWREG(NVIC_EN3) |= BIT2HI;

// make sure interrupts are enabled globally


__enable_irq();

//enable timer and to stall while stopped by debugger


HWREG(WTIMER2_BASE + TIMER_O_CTL) |= (TIMER_CTL_TAEN | TIMER_CTL_TASTALL);
}

float clamp(float val, float clampL, float clampH)


{
if (val > clampH)
{
return clampH;
}
else if (val < clampL)
{
return clampL;
}
return val;
}

void LineFollowingIntResponse(void)
{
ES_Event_t LineEvent;
ADC_MultiRead(LineSensors);

// clear the source of the interrupt


HWREG(WTIMER2_BASE + TIMER_O_ICR) = TIMER_ICR_TATOCINT;

if (AllowLineOn)
{
// check if either sensor is above threshold
if ((LineSensors[0] > LINE_ON_THRESHOLD)
|| (LineSensors[1] > LINE_ON_THRESHOLD))
{
AllowLineOn = false;
//printf("found line \r\n");
LineEvent.EventType = LINE_ON;
PostMasterSM(LineEvent);
}
}
if (AllowLineAlign)
{
if ((LineSensors[0] > LINE_ALIGNED_THRESHOLD))
{
AllowLineAlign = false;
//printf("found line \r\n");
LineEvent.EventType = LINE_ALIGNED;
PostMasterSM(LineEvent);
}
}

if (LCR_CONTROL)
{
//printf("following line \r\n");
// calculate the sensor reading difference (Right - Left)
Delta = ((float)LineSensors[1]) - (float)(LineSensors[0]);
//printf("%d\t%d\t%f\n\r",LineSensors[0],LineSensors[1],Delta);
//printf("%d\n\r", LineSensors[0]);
// calculate the integral term & clamp between 0 and nominal DC
IntegralTerm += iGain * Delta;
//caculate with moving average (filters the value)
DerivativeTerm = BETA * DerivativeTerm + (1 - BETA) * (Delta - LastDelta);
IntegralTerm = clamp(IntegralTerm, -50, 50);

// calculate the control effort & clamp between 0 and nominal DC


DutyAdd = pGain * Delta + IntegralTerm + dGain * DerivativeTerm;
DutyAdd = clamp(DutyAdd, -50, 50);
//printf("%f\r\n",DutyAdd);
//printf("%f\t%f\t", Delta,IntegralTerm);

// set new duty cycle to slow down motor


// Right side is closer, slow down right wheel
if (DutyAdd >= 0.0)
{
RMotorDC = NOMINAL_DC;
LMotorDC = NOMINAL_DC + DutyAdd;
SetRightDrive((int)RMotorDC, CW);
SetLeftDrive((int)LMotorDC, CCW);
}
// Left side is closer, slow down left wheel
else if (DutyAdd < 0.0)
{
RMotorDC = NOMINAL_DC - DutyAdd;
LMotorDC = NOMINAL_DC;
SetRightDrive((int)RMotorDC, CW);
SetLeftDrive((int)LMotorDC, CCW);
}
// update Delta
LastDelta = Delta;
}
}

//function to enable LineChecking


void EnableLineOn(void)
{
AllowLineOn = true;
}

//fucntion to enable LineAlign


void EnableLineAlign(void)
{
AllowLineAlign = true;
}

You might also like