// Initialize Servo Service

// Takes: uint8_t Priority: local priority number

// Returns: True if ES_INIT event is posted
bool ServoSInit(uint8_t Priority){
set local priority
initialize servo module
current state is idle
post es_init event
}
// Set up Servo post service
// Takes: ES_Event ThisEvent: Event to be posted
// Returns: true if event is posted
bool PostServoS(ES_Event ThisEvent) {
post event
}
// Run Servo Service
// Takes: ES_Event ThisEvent: Event to be processed
// Returns: ES_Event ReturnEvent: ES_NO_EVENT if no errors
ES_Event RunServoS(ES_Event ThisEvent) {
next state is current state
if current state is idle
if event is open event
next state is wind
else if event is reset event
call reset function
set reset timer
next state is reset
else if state is wind
if event is wind event
speed equals event param
drive servos forward using wind function
set wind timer
next state is wind
else if event is timeout event
if last direction was forward
drive servos backward using wind function
else if last direction was backward
drive servos forward using wind function
reset wind timer
next state is wind
else if event is grow event
speed is 0
send speed to wind function
next state is grow
else if event is reset event
call reset function
set reset timer
next state is reset
else if next state is grow
if the event is a grow event
if the event param is 0
speed is 0
send speed to wind function
next state is grow
else if the event param is between 1 and 3
plant to grow equals event param
drive the servos using the grow function
next state is grow

else if event is construction event

call construction function
next state is construction
else if event is reset event
call reset function
set reset timer
next state is reset
else if state is construction
if the event is construction timeout
call construction function
next state is construction
else if event is reset event
call reset function
set reset timer
next state is reset
else if state is reset
if event is timeout event
if the reset is not finished
call the reset function
set reset timer
next state is reset
else
next state is idle
next state is idle
current state is next state
}
// Drive servos to simulate wind
// Takes:
uint8_t speed: wind speed
//
uint8_t direction: 1 is forward, 0 is reverse
// Returns: nothing
static void wind(uint8_t speed, uint8_t direction) {
if speed equals 0
set servos to 0
last direction was backwards
else
limit speed
limit maximum angle
if direction is forward
write position based on speed to wind servos
last direction was forward
else if direction is backwards
write position based on a fraction of speed to wind servos (simu
late rocking)
last direction was backwards
}
// Grow the various plants on set
// Takes: uint9_t plant: plant to be grown
// Returns: nothing
static void grow(uint8_t plant) {
if plant is from 1-3
grow requested plant
}
static void construction(void) {
drive construction servos
}
// Reset the servos

// Takes: uint8_t step: 1 or 2 depending on which reset step is required

// Returns: nothing
static void reset(uint8_t step) {
if step is 1
reset speed to 0
reset last direction
send Plant 1 servo to rest
update Reset Counter
else if step is 2
send Plant 2 servo to rest
update the Reset Counter
else if step is 3
send Plant 3 servo to rest
update the Reset Counter
else if step is 4
send Construction servo to rest
update the Reset Counter
else if step is 5
drive wind servos back to rest
update the Reset Counter
else if step is 6
deselect all servos
Reset the Reset Counter
}