1 Controlservice: Pseudo-Code For The Controlservice Module

1 ControlService
************************************************************************************
Pseudo-code for the ControlService module
This is a service to implement a control law on yaw rate for the thrust fans, as
well as respond to digital bit commands in the control byte from the controller.
It responds to a brake command and a peripheral command.
InitControlService
Takes a priority number, returns true
// Initialize deferral queue

// Initialize the MyPriority variable with the passed in parameter.
// Initialize thrust periodic timer (wide timer 2 timer A)
// Set PWM channels to 0
// Set CurrentState to be InitDogRXState
// Post Event ES_Init to queue
End of InitMotorCommandService
PostControlService
Create PostControlService
RunControlService
The parameter it takes in will be ThisEvent and the parameter field of
ThisEvent will be the time that the event occurred. Returns ES_NO_Event.
// Create an event to return

// Define any function-level variables
// Set NextState to CurrentState
// Based on the state of the CurrentState, choose one of the following blocks of
code
// If case is InitControl
// If event is ES_Init
// Initialization was successful. Set NextState to ReadyForControl
// Endif
// If case is ReadyForControl
// If print timer expires, print values from ISR to screen
// Else if event is EV_Brake
// Post an event to PIC_UARTService to turn off lift fan
// Else if event is EV_NoBrake
// Post an event to PIC_UARTService to turn on lift fan
// If event is EV_StartPeripheral
// Initialize all periperal counters and flags to 0
// Start peripheral timer
// If peripheral timer expires
// Call on thunderbolt peripheral function
// If this event is EV_BattleStart
// Initialize DogTag variable
// Get which dog tag is connected using GetDogTag function
// Depending on DogTag, choose which pokemon to light up:
// If DogTag is 1
// Write values to the shift register to turn Charmander and Pikachu on
// If DogTag is 2
// Write values to the shift register to turn Bulbasaur and Pikachu on
// If DogTag is 3
// Write values to the shift register to turn Squirtle and Pikachu on
// Endif
// Endif
// Endif
// Set CurrentState to NextState
// Return ES_NO_EVENT
End of RunControlService
InitThrustPeriodicTimer
Initializes the periodic timer used for the control law on yaw rate, which updates
the duty cycles for the thrust fans upon every expiration.
// Start by enabling clock to wide timer 2 timer A

// Kill a few cycles to let the clock get going
1
// Make sure that timer A is disabled
// Set it up in 32bit wide( individ, not concatenated)
// Set up timer A in periodic mode
// Set timeout to SAMPLE_TIME
// Dont enable a local timeout interrupt yet
// Enable timer A wide timer 2 in NVIC (bit2 of EN3, corresponds to bit 98 overall)
// Set priority of interrupt as 1, which is lower than the automatic 0 priority of
other interrupts
// Make sure interrupts are enabled globally
// Kick timer off
End of InitThrustPeriodicTimer
Thunderbolt
Sets the color of the LED ribbons so as to simulate Pikachu shocking the other
pokemon with a lightning attack
// Define local function level variables

// Get DogTag from GetDogTag function in main DogSM
// If FlashCounter less than NUM_FLASHES (5)
// Increment FlashCounter
// If FlashFlag is true
// Shock current dog tag pokemon by turning its LED yellow
// Set FlashFlag as false so that we turn lights off next time
// Else
// Set pokemon back to its original color and shock remaining two pokemon
// Set FlashFlag as true so that we turn lights on next time
// Endif
// Restart peripheral timer
// Else
// Turn all pokemon off except the one corresponding to current DogTag and Pikachu
// Endif
End of Thunderbolt
ThrustPeriodicTimerISR
Responds to the thrust fan periodic timers expiration by setting the thrust fan
duty cycle based on the FwdReverseByte and LeftRightByte from the controllers
last control message as well as the current yaw rate.
// Define necessary function-level variables

// Clear source of interrupt
// Get current control bytes from receive service
// Based off of forward/reverse byte value, create a speed set point
// and then determine directionality of fans:
// If FwdReverseByte greater than 127 + a dead band
// Set fan direction as forward
// Create a speed set point based on byte value
// Else if FwdReverseByte less than 127 - a dead band
// Set fan direciton as backward
// Create a speed set point based on byte value
// Else we want pure yaw
// Set one fan as forward and one fan as reverse based on LeftRightByte
// Set speed set point as 0
// Get current IMU data using getter function and add in an offset
// Set desired yaw rate using LeftRightByte
// Calculate error as desired minus actual yaw rate.
// Execute closed-loop PD control law to determine the differences in duty cycle
// needed on the left and right fans in order to produce the correct yaw
// Calculate finalized fan duty cycles based on speed set point and yaw
// Clamp values so they are never too high or too low
// Set fan duty cycles using corresponding PWM channels
// Store current error as last error
End of ThrustPeriodicTimerISR
End of ControlService

1 Controlservice: Pseudo-Code For The Controlservice Module

Uploaded by

Copyright:

Available Formats

1 Controlservice: Pseudo-Code For The Controlservice Module

Uploaded by

Document Information

Original Title

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

1 Controlservice: Pseudo-Code For The Controlservice Module

Uploaded by

Copyright:

Available Formats

1 ControlService

// Initialize deferral queue

// Create an event to return

// Start by enabling clock to wide timer 2 timer A

// Define local function level variables

// Define necessary function-level variables

You might also like