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

Module
BallShooting.c
Revision
2.0.1
Description
Control of flywheel motors and servo to let balls thru
****************************************************************************/
/*----------------------------- Include Files -----------------------------*/
// Basic includes for a program using the Events and Services Framework
#include "ES_Configure.h"
#include "ES_Framework.h"
#include "ES_Types.h"
#include "ES_Events.h"
#include "ES_Timers.h"
/* 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 <stdint.h>
#include "inc/hw_memmap.h"
#include "inc/hw_gpio.h"
#include "inc/hw_pwm.h"
#include "inc/hw_sysctl.h"
#include "inc/hw_types.h"
#include "bitdefs.h"
#include "driverlib/gpio.h"
#include "inc/hw_gpio.h"
#include "BallShooting.h"
#include "DriveMotorService.h"
#include "MasterMachine.h"
/*----------------------------- Module Defines ----------------------------*/
// define constants for the states for this machine
// and any other local defines
#define PWMTicksPerMS 40000/32
#define PeriodInMS 20
#define BitsPerNibble 4
#define ALL_BITS (0xff<<2)
#define LOW_POSITION 10
#define HI_POSITION 7
#define SERVO_TIME 175
#define AIM_SPEED 30
#define SHOT_TIME 1000
been released
#define FLYWHEEL_TIME 2450

// Low position of servo

// High position of servo
// How long to lift the servo
// How fast to rotate
// How long to wait after ball has
// How long to spin up the flywheels

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

/* prototypes for private functions for this machine, things like during
functions, entry & exit functions.They should be functions relevant to the
behavior of this state machine
*/
static ES_Event DuringAim( ES_Event Event);
static ES_Event DuringFire( ES_Event Event);
static void SetFlywheelSpeed(uint8_t NewDuty);
static void SetServo(uint8_t NewDuty);
/*---------------------------- Module Variables ---------------------------*/
// everybody needs a state variable, you may need others as well

static BallShootingState_t CurrentState;

/*------------------------------ Module Code ------------------------------*/
bool InitBallShooting( void )
{
// start by enabling the clock to the PWM Module (PWM0)
HWREG(SYSCTL_RCGCPWM) |= SYSCTL_RCGCPWM_R0;
// enable the clock to Port B
HWREG(SYSCTL_RCGCGPIO) |= SYSCTL_RCGCGPIO_R1;
// Select the PWM clock as System Clock/32
HWREG(SYSCTL_RCC) = (HWREG(SYSCTL_RCC) & ~SYSCTL_RCC_PWMDIV_M) |
(SYSCTL_RCC_USEPWMDIV | SYSCTL_RCC_PWMDIV_32);
// make sure that the PWM module clock has gotten going
while ((HWREG(SYSCTL_PRPWM) & SYSCTL_PRPWM_R0) != SYSCTL_PRPWM_R0)
;
// disable the PWM while initializing
HWREG( PWM0_BASE+PWM_O_1_CTL ) = 0;
// program generator A to go to 0 at rising compare A, 1 on falling compare A
HWREG( PWM0_BASE+PWM_O_1_GENA) =
(PWM_1_GENA_ACTCMPAU_ZERO | PWM_1_GENA_ACTCMPAD_ONE );
// Set the PWM period. Since we are counting both up & down, we initialize
// the load register to 1/2 the desired total period
HWREG( PWM0_BASE+PWM_O_1_LOAD) = ((PeriodInMS * PWMTicksPerMS)-1)>>1;
// Set the initial Duty cycle on A to 50% by programming the compare value
// to 1/2 the period to count up (or down)
HWREG( PWM0_BASE+PWM_O_1_CMPA) = ((PeriodInMS * PWMTicksPerMS)-1)>>2;
// set changes to the PWM output Enables to be locally syncronized to a
// zero count
HWREG(PWM0_BASE+PWM_O_ENUPD) = (HWREG(PWM0_BASE+PWM_O_ENUPD) &
~(PWM_ENUPD_ENUPD2_M | PWM_ENUPD_ENUPD3_M)) |
(PWM_ENUPD_ENUPD2_LSYNC | PWM_ENUPD_ENUPD3_LSYNC);
// enable the PWM outputs
HWREG( PWM0_BASE+PWM_O_ENABLE) |= (PWM_ENABLE_PWM2EN);// | PWM_ENABLE_PWM3EN);
// now configure the Port B pins to be PWM outputs
// start by selecting the alternate function for PB4 & 5
HWREG(GPIO_PORTB_BASE+GPIO_O_AFSEL) |= (BIT4HI);
// now choose to map PWM to those pins, this is a mux value of 4 that we
// want to use for specifying the function on bits 4 & 5
HWREG(GPIO_PORTB_BASE+GPIO_O_PCTL) =
(HWREG(GPIO_PORTB_BASE+GPIO_O_PCTL) & 0xfff0ffff) + (4<<(4*BitsPerNibble));
// Enable pins 4 & 5 on Port B for digital I/O
HWREG(GPIO_PORTB_BASE+GPIO_O_DEN) |= (BIT4HI | BIT5HI);
// make pins 4 & 5 on Port B into outputs
HWREG(GPIO_PORTB_BASE+GPIO_O_DIR) |= (BIT4HI |BIT5HI);
// set the up/down count mode and enable the PWM generator
HWREG(PWM0_BASE+ PWM_O_1_CTL) |= (PWM_1_CTL_MODE | PWM_1_CTL_ENABLE);
//initialize flywheels to zero rpm
SetFlywheelSpeed(0);

//initialize servo position

SetServo(LOW_POSITION);

printf("BallShootingInit Complete\r\n");
return true;

/****************************************************************************
Function
RunBallShooting
Parameters
ES_Event: the event to process
Returns
ES_Event: an event to return
Description
add your description here
****************************************************************************/
ES_Event RunBallShooting( ES_Event CurrentEvent )
{
bool MakeTransition = false;/* are we making a state transition? */
BallShootingState_t NextState = CurrentState;
ES_Event EntryEventKind = { ES_ENTRY, 0 };// default to normal entry to new
state
ES_Event ReturnEvent = CurrentEvent; // assume we are not consuming event
switch ( CurrentState )
{
case Aim :
CurrentEvent = DuringAim(CurrentEvent);
//process any events
if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
{
switch (CurrentEvent.EventType)
{
case EV_AIM_READY :
//stop motors
StopMotors();
the next state will be
skip changing MakeTransition
that we are taking a transition
with history change kind of entry

NextState = Fire;//Decide what

// for internal transitions,
MakeTransition = true; //mark
// if transitioning to a state
EntryEventKind.EventType =

ES_ENTRY;
this event for the upper

// optionally, consume or re-map

// level state machine
ReturnEvent.EventType =

ES_NO_EVENT;

}
}
break;

break;
default: break;
// repeat cases as required for relevant events

case Fire :
CurrentEvent = DuringFire(CurrentEvent);
//process any events
if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
{
if( CurrentEvent.EventType == ES_TIMEOUT &&
CurrentEvent.EventParam == FlywheelTimer ) {
//raise servo to allow ball to come
through
SetServo(HI_POSITION);
//start servo timer
ES_Timer_InitTimer(ServoTimer,
SERVO_TIME);
the next state will be
skip changing MakeTransition
that we are taking a transition
with history change kind of entry

NextState = Fire;//Decide what

// for internal transitions,
MakeTransition = false; //mark
// if transitioning to a state
EntryEventKind.EventType =

ES_ENTRY;
this event for the upper

// optionally, consume or re-map

// level state machine
ReturnEvent.EventType =

ES_NO_EVENT;

// repeat cases as required for relevant events

}

if( CurrentEvent.EventType == ES_TIMEOUT &&

CurrentEvent.EventParam == ServoTimer ) {
//put servo back to original
position
SetServo(LOW_POSITION);
//start shot timer
ES_Timer_InitTimer(ShotTimer,
SHOT_TIME);
the next state will be
skip changing MakeTransition
that we are taking a transition
with history change kind of entry
ES_ENTRY;
this event for the upper

NextState = Fire;//Decide what

// for internal transitions,
MakeTransition = false; //mark
// if transitioning to a state
EntryEventKind.EventType =
// optionally, consume or re-map
// level state machine
ReturnEvent.EventType =

ES_NO_EVENT;
relevant events

// repeat cases as required for

}
if( CurrentEvent.EventType == ES_TIMEOUT && CurrentEvent.EventParam
== ShotTimer )

//Post EV_SHOT_FIRED to go back into

GoingToPosition2AfterBall

ES_Event PostEvent =

{EV_SHOT_FIRED, 0};

PostMaster(PostEvent);

skip changing MakeTransition

NextState = Fire;
// for internal transitions,
MakeTransition = false; //mark

that we are taking a transition

// if transitioning to a state

with history change kind of entry

EntryEventKind.EventType =

ES_ENTRY;

// optionally, consume or re-map

this event for the upper

// level state machine

ReturnEvent.EventType =
ES_NO_EVENT;

// repeat cases as required for

relevant events
}
}
break;
default: break;
}
//
If we are making a state transition
if (MakeTransition == true)
{
//
Execute exit function for current state
CurrentEvent.EventType = ES_EXIT;
RunBallShooting(CurrentEvent);
CurrentState = NextState; //Modify state variable
//
Execute entry function for new state
// this defaults to ES_ENTRY
RunBallShooting(EntryEventKind);

}
return(ReturnEvent);

}
/****************************************************************************
Function
StartBallShooting
Parameters
None
Returns
None
Description
Does any required initialization for this state machine
Notes
****************************************************************************/
void StartBallShooting ( ES_Event CurrentEvent )
{
// start in aim state
if ( ES_ENTRY_HISTORY != CurrentEvent.EventType )
{
CurrentState = Aim;

}
// call the entry function (if any) for the ENTRY_STATE
RunBallShooting(CurrentEvent);
}
/****************************************************************************
Function
QueryBallShootingState
Parameters
None
Returns
TemplateState_t The current state of the Template state machine
Description
returns the current state of the Template state machine
****************************************************************************/
BallShootingState_t QueryBallShootingState ( void )
{
return(CurrentState);
}
/***************************************************************************
private functions
***************************************************************************/
static ES_Event DuringAim( ES_Event Event)
{
ES_Event 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) )
{
//rotate to find IR beacon
printf("STATE: Aiming to IR target\r\n");
//set wheels to rotate at AIM_SPEED
NewRPMCommandLeft(AIM_SPEED);
NewRPMCommandRight(-AIM_SPEED);
}
else if ( Event.EventType == ES_EXIT )
{
// do nothing
}else
// do the 'during' function for this state
{
// do nothing
}
// return either Event, if you don't want to allow the lower level machine
// to remap the current event, or ReturnEvent if you do want to allow it.
return(ReturnEvent);
}
static ES_Event DuringFire( ES_Event Event)
{
ES_Event 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) )
printf("STATE: Firing\r\n");
// ramp up flywheel motors
SetFlywheelSpeed(1);
//start flywheel timer, allow the motors to reach speed
ES_Timer_InitTimer(FlywheelTimer, FLYWHEEL_TIME);

}
else if ( Event.EventType == ES_EXIT )
{
printf("Done shooting\r\n");
//stop flywheel motors
SetFlywheelSpeed(0);
}else
// do the 'during' function for this state
{
// do nothing
}
// return either Event, if you don't want to allow the lower level machine
// to remap the current event, or ReturnEvent if you do want to allow it.
return(ReturnEvent);
}
//Helper functions for motor controls
/*
SetFlywheel speed turns flywheels off if passed 0, on if passed anything else
*/
static void SetFlywheelSpeed(uint8_t NewDuty)
{
//turn flywheels off
if(NewDuty == 0)
HWREG(GPIO_PORTB_BASE + (GPIO_O_DATA + ALL_BITS)) &= BIT5LO;
//turn flywheels on
else
HWREG(GPIO_PORTB_BASE + (GPIO_O_DATA + ALL_BITS)) |= BIT5HI;
return;
}
/*
Set position for servo motor
10 is about the lowest position, 5 is about the highest position
*/
static void SetServo(uint8_t position)
{
//set servo position
HWREG( PWM0_BASE+PWM_O_1_CMPA) = position * ((PeriodInMS * PWMTicksPerMS)1)/200;
}