Motorservice

/****************************************************************************
Module
Motor.c
Revision
1.0.1
Description
This is a template file for implementing flat state machines under the
Gen2 Events and Services Framework.
Notes
History
When Who What/Why
-------------- --- --------
01/15/12 11:12 jec revisions for Gen2 framework
11/07/11 11:26 jec made the queue static
10/30/11 17:59 jec fixed references to CurrentEvent in RunTemplateSM()
10/23/11 18:20 jec began conversion from SMTemplate.c (02/20/07 rev)
****************************************************************************/
/*----------------------------- 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
*/
// the common headers for C99 types
#include <stdint.h>
#include <stdbool.h>
#include <stdio.h>
#include "ES_Port.h"
#include "termio.h"
#include "Beacon.h"
#include "ES_Configure.h"
#include "ES_Framework.h"
#include "PWMService.h"
#include "MotorService.h"
#include "ADMulti.h"
#include "ISR.h"
#include "Unloading.h"
#include "MasterHSM.h"
// the headers to access the GPIO subsystem
#include "inc/hw_memmap.h"
#include "inc/hw_types.h"
#include "inc/hw_gpio.h"
#include "inc/hw_sysctl.h"
// the headers to access the TivaWare Library

#include "driverlib/sysctl.h"
#include "driverlib/pin_map.h"
#include "driverlib/gpio.h"
#include "driverlib/timer.h"
#include "driverlib/interrupt.h"
#include "BITDEFS.H"
/*----------------------------- Module Defines ----------------------------*/
#define Motor_Timer 2
#define NINETY 1860
#define FFIVE 930
#define THRESHOLD 4000
#define BAND 500
#define UTURNTIMER 3000
/*---------------------------- Module Functions ---------------------------*/
/* prototypes for private functions for this service.They should be functions
relevant to the behavior of this service
*/
/*---------------------------- Module Variables ---------------------------*/

// with the introduction of Gen2, we need a module level Priority variable
static uint8_t MyPriority;
static uint8_t NumOfADC = 3;//Num of PWM outputs
static uint16_t StepTimer = 0, StepTime = 100;
static uint16_t ADCTimer = 1, ADCTime = 2;
static uint16_t Interval;
static uint32_t Pot;
static uint32_t Speed;
static uint32_t TargetSpeed;
static MotorState_t CurrentState;
static uint32_t FRONTLS, BACKLS;
static uint32_t LastFLS, LastBLS;
static uint32_t CurrentTape[3] = { 0, 0, 0 };
static uint32_t Floor[3];
static uint8_t Beacon = 0; //Beacon Flag
static uint8_t TapeLeft = 0, TapeMiddle = 0, TapeRight = 0; //Tape Flag
static uint16_t StartTime;
static uint16_t StopTime;
static uint8_t TapeFlag = 0;
static uint8_t MasterTapeFlag = 0;
static uint8_t Period = 0;
static uint8_t BeaconFlag = 0;
static uint8_t LeftSpeed = 0, RightSpeed = 0;
static uint32_t debounce = 0;
/*------------------------------ Module Code ------------------------------*/

/****************************************************************************
Function
InitMotor
Parameters
uint8_t : the priorty of this service
Returns
bool, false if error in initialization, true otherwise
Description
Saves away the priority, Sets up the initial transition and does any
other required initialization for this state machine
Notes
Author
J. Edward Carryer, 10/23/11, 18:55
****************************************************************************/
bool InitMotor(uint8_t Priority)
{
ES_Event_t ThisEvent;
MyPriority = Priority;
// put us into the Initial PseudoState
ADC_MultiInit(NumOfADC);//Initialize PE0 as Analog
// enable the clock to Port B
ADC_MultiRead(Floor);
//Initialization for Limit Switches
HWREG(SYSCTL_RCGCGPIO) |= SYSCTL_RCGCGPIO_R0;
while ((HWREG(SYSCTL_PRGPIO) & SYSCTL_PRGPIO_R0) != SYSCTL_PRGPIO_R0)
{}
//Assigning Digital status to PORTD

HWREG(GPIO_PORTA_BASE + GPIO_O_DEN) |= (BIT6HI | BIT7HI);
//Making PORTD0 to D3 as Inputs
HWREG(GPIO_PORTA_BASE + GPIO_O_DIR) &= (BIT6LO & BIT7LO);
//Initialization for Motor Direction Pins

HWREG(SYSCTL_RCGCGPIO) |= SYSCTL_RCGCGPIO_R1;
//Wait for the GPIO port to be ready
while ((HWREG(SYSCTL_PRGPIO) & SYSCTL_PRGPIO_R1) != SYSCTL_PRGPIO_R1)
{}
// Enable pins 0, 4 & 5 on Port B for digital I/O - for the other side of Motors
HWREG(GPIO_PORTB_BASE + GPIO_O_DEN) |= (BIT0HI | BIT1HI);
// make pins 4 & 5 on Port B into outputs
HWREG(GPIO_PORTB_BASE + GPIO_O_DIR) |= (BIT0HI | BIT1HI);
//Pull the two lines low
HWREG(GPIO_PORTB_BASE + (GPIO_O_DATA + ALL_BITS)) &= BIT0LO;
CurrentState = InitPState;
// post the initial transition event
//InitPWMService();
SetDutyPB4(LeftSpeed);
SetDutyPB5(RightSpeed);
ThisEvent.EventType = ES_INIT;
if (ES_PostToService(MyPriority, ThisEvent) == true)
{
return true;
}
else
{
return false;
}
}
/****************************************************************************
Function
PostMotor
Parameters
EF_Event_t ThisEvent , the event to post to the queue
Returns
boolean False if the Enqueue operation failed, True otherwise
Description
Posts an event to this state machine's queue
Notes
Author
****************************************************************************/
bool PostMotor(ES_Event_t ThisEvent)
{
return ES_PostToService(MyPriority, ThisEvent);
}
/****************************************************************************
Function
RunMotor
Parameters
ES_Event_t : the event to process
Returns
ES_Event_t, ES_NO_EVENT if no error ES_ERROR otherwise
Description
add your description here
Notes
uses nested switch/case to implement the machine.
Author
****************************************************************************/
ES_Event_t RunMotor(ES_Event_t ThisEvent)
{
ES_Event_t ReturnEvent;
ReturnEvent.EventType = ES_NO_EVENT; // assume no errors
ES_Event_t NextEvent;
//printf("\n\rTape Value is %d",CurrentTape);
// printf("\n\rFloor Value is %d",Floor);
switch (CurrentState)
{
case InitPState: // If current state is initial Psedudo State
{
if (ThisEvent.EventType == ES_INIT) // only respond to ES_Init
{
// this is where you would put any actions associated with the
// transition from the initial pseudo-state into the actual
// initial state
// now put the machine into the actual initial state

CurrentState = Waiting;
}
}
break;
case Waiting: // If current state is state one

{
switch (ThisEvent.EventType)
{
case ES_START:
{
CurrentState = InGame;
} break;
case ES_GAMEOVER:
{ //Stop the Motors
LeftSpeed = 0;
RightSpeed = 0;
printf("\n\Game Over is received****************************");
CurrentState = GameOver;
} break;
default:
{}
break;
}
} break;
case InGame:
{
printf("\n\Motor is IN GAME STATE*********************");
{
case ES_BACKCRASH:
{ //Stop the MOtors
LeftSpeed = 0;
RightSpeed = 0;
printf("\n\Motor is sensing backcrash*********************");
TapeLeft = TapeMiddle = TapeRight = 0;
} break;
case ES_FULLFRONT:
{
//SetSpeed of Both motors to be same
LeftSpeed = 35;
RightSpeed = 35;
printf("\n\Motor is FULLFRONT*********************");
} break;
case ES_FULLBACK:
{
//SetSpeed of Both Motors to be same
//Pull the two lines high for reverse
LeftSpeed = 10;
RightSpeed = 10;
HWREG(GPIO_PORTB_BASE + (GPIO_O_DATA + ALL_BITS)) |= BIT0HI;
printf("\n\Motor is FULLBACK*********************");
} break;
case ES_HOLD:
{ //Stop the motors
LeftSpeed = 0;
RightSpeed = 0;
printf("\n\Motor is HOLDING*********************");
} break;
case ES_GETTAPE:
{ //Make the motors move forward
LeftSpeed = 35;
RightSpeed = 35;
CurrentState = GetTapeMonster;
printf("\n\Motor is GETTING TAPE*********************");
MasterTapeFlag = 1;
// ES_Timer_InitTimer(Motor_Timer, 1000);
} break;
case ES_CCW:
{ //Set speed on one motor and make the other motor stop
LeftSpeed = 0;
RightSpeed = 35;
//CurrentState = FollowLine;
printf("\n\Motor is in ES_CCW*********************");
} break;
case ES_CW:
LeftSpeed = 35;
RightSpeed = 0;
// CurrentState = FollowLine;
printf("\n\Motor is in ES_CW*********************");
} break;
case ES_GETOPPONENTBEACON:
//SetSpeed one motor to rotate
LeftSpeed = 35;
RightSpeed = 0;
printf("\n\Motor is GETTIGN BEACON*********************");
} break;
case ES_GAMEOVER:
{ //Stop the Motors
LeftSpeed = 0;
RightSpeed = 0;
printf("\n\Motor is GAMEOVER*********************");
} break;
default:
{}
break;
}
} break;
case GetTapeMonster:
{
printf("\n\Motor is GETTAPEMONSTER STATE*********************");
if (ThisEvent.EventType == ES_TAPE)
{
MasterTapeFlag = 1;
}
// if ((ThisEvent.EventType==ES_TIMEOUT), (ThisEvent.EventParam ==
Motor_Timer))
{
// CurrentState=FollowLine;
}
/*if ((ThisEvent.EventType == ES_TIMEOUT) && (ThisEvent.EventParam ==
Motor_Timer) && (TapeFlag == 1))
{ //SetSpeed on Motors for Reverse
}*/
{
case ES_GETOPPONENTBEACON:
//SetSpeed one motor to rotate
LeftSpeed = 35;
RightSpeed = 0;
printf("\n\Motor is GETTIGN BEACON*********************");
} break;
case ES_GETTAPE:
LeftSpeed = 35;
RightSpeed = 35;
CurrentState = GetTapeMonster;
printf("\n\Motor is GETTING TAPE*********************");
MasterTapeFlag = 1;
// ES_Timer_InitTimer(Motor_Timer, 1000);
} break;
case ES_FULLFRONT:
LeftSpeed = 35;
RightSpeed = 35;
printf("\n\rMotor is FULLFRONT*********************");
} break;
/*case ES_OPPONENTBEACONDETECTED:
{
LeftSpeed=0;
RightSpeed=0;
CurrentState=FollowLine;
if(abs(Period-600)<20)
SetFrequency(500);
else if (abs(Period-500)<20)
SetFrequency(600);
printf("\n\rMotor is detecting Beacon*********************");
TapeLeft=TapeMiddle=TapeRight = 1;
} break;*/
case ES_TAPE_MIDDLE:
{ //Make the motors move backwards
LeftSpeed = 35;
RightSpeed = 35;
CurrentState = FollowLine;
printf("\n\Motor is sensing TAPE MIDDLE*********************");
TapeMiddle = 0;
TapeLeft = 1;
TapeRight = 1;
} break;
case ES_FULLBACK:
{ //Make the motors move bakcwards
LeftSpeed = 10;
RightSpeed = 10;
printf("\n\rMotor is FULLBACK*********************");
} break;
case ES_HOLD:
{ //Stop teh MOtors
LeftSpeed = 0;
RightSpeed = 0;
printf("\n\rMotor is HOLDING*********************");
} break;
case ES_FOLLOWLINE_CW_R:
{ //Set speed on one motor and make the other motor stop, in reverse direction
LeftSpeed = 00;
RightSpeed = 20;
BeaconFlag = 1;
//ES_Timer_InitTimer(Motor_Timer, 1000);
printf("\n\Motor is Following Line CW*********************");
TapeMiddle = 1;
} break;
case ES_FOLLOWLINE_CCW_R:
{ //Set speed on one motor and make the other motor stop, in reverse direction
//SetSpeed on other Motor for Reverse, Other Motor to Zero
LeftSpeed = 20;
RightSpeed = 0;
BeaconFlag = 1;
//ES_Timer_InitTimer(Motor_Timer, 1000);
printf("\n\Motor is Following line CCW*********************");
TapeMiddle = 1;
} break;
case ES_GAMEOVER:
{ //Stop the MOtors
LeftSpeed = 0;
RightSpeed = 0;
printf("\n\Motor is GAMEOVER*********************");
} break;
default:
{}
break;
}
} break;
case FollowLine:
{
/* if(LeftSpeed==50&&RightSpeed==25)
{
LeftSpeed=40;
RightSpeed=40;
}
else if(LeftSpeed==25&&RightSpeed==50)
{
LeftSpeed=40;
RightSpeed=40;
}*/
printf("\n\Motor is FOLLOWLINE STATE*********************");

/* if ((ThisEvent.EventType==ES_TIMEOUT), (ThisEvent.EventParam ==
Motor_Timer))
{ LeftSpeed=50;
RightSpeed=0;
CurrentState = Uturn;
printf("\n\Motor is ready to UTURN*********************");
ES_Timer_InitTimer(Motor_Timer, UTURNTIMER);
}*/
{
case ES_TAPE_MIDDLE:
{ //Set speed on motors to go reverse
LeftSpeed = 20;
RightSpeed = 20;
printf("\n\Motor is sensing TAPE MIDDLE*********************");
TapeMiddle = 0;
TapeLeft = 1;
TapeRight = 1;
} break;
case ES_FULLFRONT:
{ //Set speed on motors to go front
LeftSpeed = 35;
RightSpeed = 35;
printf("\n\rMotor is FULLFRONT*********************");
} break;
case ES_FULLBACK:
{ //Set Speed on MOtors to go back
LeftSpeed = 10;
RightSpeed = 10;
printf("\n\rMotor is FULLBACK*********************");
} break;
case ES_TAPE_LEFT:
{ //Set speed on one motor to go reverse, and other motor to zero
/*if(LeftSpeed>3)
LeftSpeed=LeftSpeed=LeftSpeed-3;
else
LeftSpeed=0;*/
LeftSpeed = 0;
RightSpeed = 15;
printf("\n\Motor is Senbsing TAPE LEFT*********************");
TapeLeft = 0;
TapeMiddle = 1;
TapeRight = 1;
} break;
case ES_TAPE_RIGHT:
{ //Set speed on one motor to go reverse, and other motor to zero
LeftSpeed = 15;
RightSpeed = 0;
/*if(RightSpeed>3)
RightSpeed=RightSpeed+3;
else
RightSpeed=0;*/
printf("\n\Motor is TAPE RIGHT*********************");
TapeRight = 0;
TapeLeft = 1;
TapeMiddle = 1;
} break;
/* case ES_UTURN:
{//Make motors to go back
LeftSpeed=50;
RightSpeed=50;
ES_Timer_InitTimer(Motor_Timer, 1000);
printf("\n\Motor is going to UTURN*********************");
//CurrentState = Uturn;
} break;*/
case ES_HOLD:
{ //Stop the motors
LeftSpeed = 0;
RightSpeed = 0;
printf("\n\rHolding");
} break;
case ES_BACKCRASH:
{ //Stop the motors
LeftSpeed = 0;
RightSpeed = 0;
printf("\n\Motor is sensing backcrash*********************");
} break;
case ES_GAMEOVER:
{ //STop the motors
LeftSpeed = 0;
RightSpeed = 0;
printf("\n\Game Over is Received");
} break;
default:
{}
break;
}
} break;
/* case Uturn:
{
printf("\n\Motor is in UTURN*********************");
if ((ThisEvent.EventType == ES_TIMEOUT) && (ThisEvent.EventParam ==
Motor_Timer))
{
NextEvent.EventType = ES_UTURNCOMPLETE;
PostMainMasterSM(NextEvent);
printf("\n\Motor UTURBN COMPLETE*********************");
}
else if (ThisEvent.EventType == ES_FOLLOWLINE)
{ //Set motors to go back
LeftSpeed=50;
RightSpeed=50;
printf("\n\Motor is FOLLOWING LINE*********************");
}
} break;*/
case GameOver:
{
printf("\n\rMotor State is GameOver");
} break;
default:
{}
break;
}
return ReturnEvent;
}
/****************************************************************************
Function
QueryTemplateSM
Parameters
None
Returns
TemplateState_t The current state of the Template state machine
Description
returns the current state of the Template state machine
Notes
Author
****************************************************************************/
/***************************************************************************
private functions
***************************************************************************/
bool Check4FrontWall(void)
{
bool ReturnVal = false;
ES_Event_t BeaconEvent;
ES_Event_t MasterEvent;
FRONTLS = HWREG(GPIO_PORTA_BASE + (GPIO_O_DATA + ALL_BITS)) & BIT6HI;
if (FRONTLS != LastFLS)
{ //Check if button is down
if (FRONTLS == 0)
{
printf("\n\rFrontCrash Here------");
ThisEvent.EventType = ES_FRONTCRASH;
// printf("\n\rValue of Current IR is %d and Last IR is %d",CurrentIR,
LastIR);
PostMainMasterSM(ThisEvent);
ReturnVal = true;
}
}
LastFLS = FRONTLS;
return ReturnVal;
}
bool Check4RearWall(void)
{
BACKLS = HWREG(GPIO_PORTA_BASE + (GPIO_O_DATA + ALL_BITS)) & BIT7HI;
//Check if Button state is equal to last button state
if (BACKLS != LastBLS)
{ //Check if button is down
if (BACKLS == 0)
{
printf("\n\rBackCrashHere------");
ThisEvent.EventType = ES_BACKCRASH;
// printf("\n\rValue of Current IR is %d and Last IR is %d",CurrentIR,
LastIR);
PostMainMasterSM(ThisEvent);
PostMotor(ThisEvent);
ReturnVal = true;
}
}
LastBLS = BACKLS;
return ReturnVal;
}
bool Check4Tape(void)
{
ADC_MultiRead(CurrentTape);
//printf("\n\rTape Value is %d %d
%d",CurrentTape[0],CurrentTape[1],CurrentTape[2]);
if ((CurrentTape[0] > THRESHOLD) && (abs(CurrentTape[0] - Floor[0]) > BAND))
{
if (TapeLeft == 1)
{
ThisEvent.EventType = ES_TAPE_LEFT;
ReturnVal = true;
}
}
{
if (TapeMiddle == 1)
{
ThisEvent.EventType = ES_TAPE_MIDDLE;
ReturnVal = true;
}
}
{
if (TapeRight == 1)
{
ThisEvent.EventType = ES_TAPE_RIGHT;
ReturnVal = true;
}
}
if (BeaconFlag == 1)
{
Period = QueryPeriodW3TA() * 25 / 1000;
if ((abs(Period - 600) < 20) || (abs(Period - 500) < 20))
{
BeaconEvent.EventType = ES_OPPONENTBEACONDETECTED;
BeaconEvent.EventParam = Period;
PostMotor(BeaconEvent);
BeaconFlag = 0;
}
}
if (MasterTapeFlag == 1)
{
if ((((CurrentTape[0] > THRESHOLD) && (abs(CurrentTape[0] - Floor[0]) > BAND))
|| ((CurrentTape[1] > THRESHOLD) && (abs(CurrentTape[1] - Floor[1]) > BAND)) ||
((CurrentTape[2] > THRESHOLD) && (abs(CurrentTape[2] - Floor[2]) > BAND))))
{
debounce = debounce++;
if (debounce > 1000)
{
MasterEvent.EventType = ES_TAPE;
PostMainMasterSM(MasterEvent);
MasterTapeFlag = 0;
ReturnVal = true;
printf("Tape Detected");
debounce = 0;
}
}
else
{
debounce = 0;
}
}
return ReturnVal;
}

Motorservice

Hochgeladen von

Dokumentinformationen

Originaltitel

Copyright

Verfügbare Formate

Dieses Dokument teilen

Dokument teilen oder einbetten

Freigabeoptionen

Stufen Sie dieses Dokument als nützlich ein?

Sind diese Inhalte unangemessen?

Copyright:

Verfügbare Formate

Motorservice

Hochgeladen von

Copyright:

Verfügbare Formate

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

// the headers to access the TivaWare Library

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

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

//Assigning Digital status to PORTD

//Initialization for Motor Direction Pins

// now put the machine into the actual initial state

case Waiting: // If current state is state one

printf("\n\Motor is FOLLOWLINE STATE*********************");

FRONTLS = HWREG(GPIO_PORTA_BASE + (GPIO_O_DATA + ALL_BITS)) & BIT6HI;

Das könnte Ihnen auch gefallen