/**************************************************************************** Module DOG_TX pseudo code Description This is the top service for DOG which acts as a controller of the DOG as well as its transmitter module /*----------------------------- Module Defines ----------------------------*/ // these times assume a 1.000mS/tick timing #define ONE_SEC 1000 #define HALF_SEC (ONE_SEC/2) #define TWO_SEC (ONE_SEC*2) #define FIVE_SEC (ONE_SEC*5) #define LOST_COMM_TIME ONE_SEC #define DIRECTION_CHANGE_TIME 100 // packet byte definition #define START_BYTE 0x7E #define API_TX_ID 0x01 #define OPTION_BYTE 0x00 #define MSB_LEN 0x00 #define MAX_XBEE_PACKET_SIZE 9 #define PAIR_RESPONSE_DATA_LEN 1 #define CHECKSUM_OFFSET 0xFF #define XBEE_OVERHEAD_LEN 9 #define DATA_OVERHEAD_LEN 5 #define LARGEST_FRAME_DATA 100 #define ACCEL_X_INDEX 0 #define ACCEL_Y_INDEX 1 #define ACCEL_Z_INDEX 2 #define GYRO_X_INDEX 3 #define GYRO_Y_INDEX 4 #define GYRO_Z_INDEX 5 #define MSB_MASK 0xFF00 #define LSB_MASK 0x00FF #define BRAKE_MASK 0x02 #define SPECIAL_FUNC_MASK 0x01 #define BRAKE_SERVO_CHANNEL 2 #define UNBRAKE_ANGLE 65 #define BRAKE_ANGLE 90 #define FORWARD_DIR 1 #define BACKWARD_DIR 2 #define AD_CHANNEL_NUM 1 /*---------------------------- Module Functions ---------------------------*/ static bool TransmitPacket() static void ClearDataPacket(void) static void WriteToUART(void) static void ConstructPacket(void) static void TurnFanOn(void) static void TurnFanOff(void) static void SetFarmerAddress(uint16_t input) static void ResetFarmerAddress(void) static void Send_PairResponse(void) static void Send_StatusMessage(void) static void Send_EncryptionReset(void) static void ExecuteCommand(uint32_t cmd) static void ConStructStatusMessage(void) static void Init_Port(void) static void StopMotor_Left(void) static void SetMotor_Left(float input) static void StopMotor_Right(void) static void SetMotor_Right(float input) static void SetPowerRelay(bool input) static void SetLeftFan_Forward() static void SetRightFan_Forward() static void SetLeftFan_Backward() static void SetRightFan_Backward() static void Calculate_Motor_Speed(uint8_t cmd1, uint8_t cmd2) static void SetBrakeOn(void) static void SetBrakeOff(void) static void SetSpecialFnOn(void) static void SetSpecialFnOff(void) static float MapToPeriod(float input) static void ClearRXDataPacket(void) static void ClearRXDataPacket(void) static void ClearRXBuffer(void) static void CopyToBuffer(void) static void ReadDogTag(void) static void SwitchSong(void) static void playUnpairedSound(void) static void playPairedSound(void) /*---------------------------- Module Variables ---------------------------*/ init priority variable and state variables init arrays for storing data and Xbee packets init variables for check sum, frame id, byte remaining, RX addresses init variable to keep track of the fan state init MSB/LSBs for all the accel and gyro axes locations init index variable to 0 init variables for length of status message init paired farmer address init variables for accel and gyro values to 0 init const variables for bounds and offsets of the control value init const multipliers for alpha and beta init variables for control offset and stop tolerance init variables for scaling R/L fan power levels RX side parameters: init variables for packet length, check sum, check sum received flag init variables for RX message MSB/LSB lengths init arrays for buffering and storing RX data packet init index variable init bool flag for direction change init variables to store current and previous fan rotation directions for R/L init variable to store current command /*------------------------------ Module Code ------------------------------*/ /**************************************************************************** Function InitDogTX Parameters uint8_t : the priorty of this service Returns bool, false if error in initialization, true otherwise Description Saves away the priority, and does any other required initialization for this service ****************************************************************************/ bool InitDogTX ( uint8_t Priority ) { /******************************************** Initialization Code *******************************************/ set up I/O lines for debugging start with the lines low initialize current state to waiting to pair Initialize packet lengths to 0 Initialize UART for Xbee and talking to TREAT Initialize Servo (call InitServo) Init PF0 and one shot timer for ESC control (call Init_Port) Initialize one shot interrupts for ESC left and right Initialize A/D Channel Release brake (call SetBrakeOff) enable NVIC and enable global interrupt turn power relay off turn fan off initially (call TurnFanOff) play the unpaired sound (call playUnpairedSound) print "Finished initializing Dog TX Service" post the initial transition event ES_INIT } /**************************************************************************** Function PostDogTX Parameters EF_Event ThisEvent ,the event to post to the queue Returns bool false if the Enqueue operation failed, true otherwise Description Posts an event to this state machine's queue ****************************************************************************/ bool PostDogTX( ES_Event ThisEvent ) { post ThisEvent using MyPriority } /**************************************************************************** Function RunDogTX Parameters ES_Event : the event to process Returns ES_Event, ES_NO_EVENT if no error ES_ERROR otherwise Description add your description here ****************************************************************************/ ES_Event RunDogTX( ES_Event ThisEvent ) { init ReturnEvent and set type to no event set NextState to CurrentState switch CurrentState if current state is wait to pair if receive pair request message from the FARMER reset farmer address (call ResetFarmerAddress) set farmer address (call SetFarmerAddress with ThisEvent param) Transmit pair ack packet (call Send_PairResponse) else if receive ack for pair response message start 1s incoming transmission timer turn on Treat fan (call TurnFanOn) play paired sound (call playPairedSound) set next state to paired wait for key else if receive nack for pair response message resend pair response message (call Send_PairResponse) break if current state is paired wait for key if don't hear back from Farmer with in 1 second turn off Fan (call TurnFanOff) set next state to wait to pair state Disable Lost Communication timer if receive encryption key message (encryption keys are stored in DOG_RX...) restart lost comm timer set RX_state to be in control mode (all received data will first be decrypted) set next state to paired call playPairedSound() switch song (call SwitchSong) break if current state is paired if 1s timeout set RX state to not be in control mode, rx packet will not be decrypted turn off Fan set next state to wait to pair state switch to unpaired sounds reset encryption index key Stop R/L motors Disable Lost Communication timer if this is direction change timer timeout set change direction variable execute current command else if received control message restart lost comm timer Set current command (call GetControlMessage) excecute current command transmit the status message else if received invalid control message restart lost comm timer transmit the reset encryption index request (call ResetEncryptKeyIndex) send encryption key reset message to farmer Stop R/L motors else if receive ack for the control message restart lost comm timer when getting ack else if receive nack for the control message restart lost comm timer when getting nack send control packet (call Send_StatusMessage) break default : break update current state to next state return ReturnEvent } /*************************************************************************** private functions ***************************************************************************/ static void StopMotor_Left(){ print "Stop motor" set current timeout to STOP_CTRL_VALUE for left motor } static void SetMotor_Left(float input){ print "Set pulse period to: " and the input set current timeout for left motor to the input (scaled) } static void StopMotor_Right(){ print "Stop motor" set current timeout to STOP_CTRL_VALUE for right motor } static void SetMotor_Right(float input){ print "Set pulse period to: " and the input set current timeout for right motor to the input (scaled) } static void SetPowerRelay(bool input){ if input true print "Turn on Power Relay" set relay control pin else print "Turn off Power Relay" clear relay control pin } static void SetLeftFan_Backward(){ print "Switch Left Fan Relay: Backward" clear relay control pin } static void SetLeftFan_Forward(){ print "Switch Left Fan Relay: Forward" set relay control pin } static void SetRightFan_Backward(){ print "Switch Right Fan Relay: Backward" clear relay control pin } static void SetRightFan_Forward(){ print "Switch Right Fan Relay: Forward" set relay control pin } static void SetBrakeOn(){ print "Brake On" move servo to braking angle (call moveToAngle) } static void SetBrakeOff(){ print "Brake Off" move servo to unbrake angle (call moveToAngle) } static void SetSpecialFnOn(){ print "If you read this, you are special." } static void SetSpecialFnOff(){ print "If you read this, you are also special." } static void SwitchSong(){ set song control pin print a message to delay a little clear song control pin } static void playUnpairedSound(){ clear song control pin } static void playPairedSound(){ set song control pin } /* * This function writes 0x01 to UART5 to turn fan on */ static void TurnFanOn(){ print "Turn fan on" write 0x01 to PE5 UART to turn on fan write fan state data to UART } /* * This function write 0x00 to UART5 to turn fan off */ static void TurnFanOff(){ print "Turn fan off" write 0x00 to PE5 UART to turn off fan write fan state data to UART } /* * This function clears farmer address */ static void ResetFarmerAddress(){ clear paired_farmer_address } /* * This function sets farmer address */ static void SetFarmerAddress(uint16_t input){ set paired_farmer_address } /* * This function executes the control command sent by farmer */ static void ExecuteCommand(uint32_t cmd){ print "Execute command: " and the command, cmd init forward and backward variable with cmd values init left and right variable with cmd values init digital (brake/special func) variable with cmd values print "Control 1 is: " and the f/b command variable first calculate the left and right power value for the motors set left fan direction and magnitude set right fan direction and magnitude if new direction is different than the previous direction stop R/L motors start direction change timer clear change direction complete flag update previous R/L fan directions to current directions if brake bit true set brake else clear brake if special function bit true set special function on else set special function off static void Calculate_Motor_Speed(uint8_t cmd1, uint8_t cmd2){ print what the commands are if the control value cmd1 is in the stop window, set it to 127, the ctrl offset if cmd1 is 127 and cmd2 is 127 set R/L power levels to 0 else calculate and set the left and right power levels print out the power levels } static float MapToPeriod(float input){ map/scale input to a period for motor control return it } /* * This function construct a pair response message */ static void Send_PairResponse(){ first clear the data packet set DataPacket_Length to PAIR_RESPONSE_DATA_LEN (2 bytes) set first byte to PAIR_ACK set frame id for pair response construct packet call transmit function to transmit data packet } /* * This function construct a status xbee message */ static void Send_StatusMessage(){ first clear the data packet set DataPacket_Length to STATUS_MESSAGE_LEN (2 bytes) call ConStructStatusMessage set Destination address (LSB/MSB_RX_ADDRESS) increment frame id by 1 every time we send status construct packet call transmit function to transmit data } /* * This function construct an encryption reset packet */ static void Send_EncryptionReset(){ first clear the data packet set DataPacket_Length to ENCR_RESET_LEN (2 bytes) set first byte to ENCR_RESET set Destination address (LSB/MSB_RX_ADDRESS) set frame id construct packet call transmit function to transmit data } /* * This function construct a status data packet */ static void ConStructStatusMessage(){ set accel and gryo value variables according to the IMU --> (using GetIMUData) add STATUS and accel/gyro values to the data packet } /* * This function construct a packet to be transmitted with option of * broadcasting to all listening devices */ static void ConstructPacket(){ add to Xbee packet: -- START_BYTE -- MSB_LEN -- data packet length + overhead (start increment checksum here) -- API_TX_ID -- frame id -- MSB/LSB RX addresses -- option byte -- contents of the data packet --check sum update Xbee packet length } /* * This function clears the content of data packet array */ static void ClearDataPacket(){ for each element of data packet set all index of data packet to 0 } /* * This function transmits packet by starting writing to UART pin out * We need to call ConstructPacket before calling this function */ static bool TransmitPacket(){ set variable for remaining number of bytes to transmit to xbee packet length reset index to 0 if UART Transmit FIFO empty bit is set (we can send) write data to UART (call WriteToUART) enable TX interrupt return true (success) else return false (failure) } /* * This function copy data from xbeepacket array to the UART pin out */ static void WriteToUART(){ write XbeePacket[index] to UARTDR decrement bytes remaining variable increment index } static void Init_Port(){ Initialize Port F Initialize PF0 for left fan ESC Control Initialize PF1 for left fan direction relay Initialize PF2 for right fan ESC Control Initialize PF3 for right fan direction relay Initialize PF4 for Power Relay } /* * This function is the ISR for the UART1 module */ void DOG_UART1_ISR(){ if UART1 interrupt flag for receiver is set clear RX interrupt flag RX State Machine in ISR: set next state to current state switch current state if current state is WaitFor7E (wait for start delimiter bytes) if receive incoming start byte first clear current data packet reset Checksums, received index to 0 set next state to wait for msb break if current state is wait for MSB save msb length of the received packet set next state to wait for lsb break if current state is wait for LSB save lsb length of the received packet form data packet length from the received msb and lsb set next state to get data packet break if current state is wait for get data packet put received data into DataPacket array add the data to check sum increment the (received) index compare index to the received index, if equal go to check sum state break if current state is get check sum set check sum to the byte received if it equals our calculated check sum call CopyToBuffer post received byte to FARMER_RX service set next state to WaitFor7E again break default: break set current state to next state else if UART1 interrupt flag for transmitter is set if FIFO empty clear TX interrupt flag write data to UART if number of bytes remaining is zero, disable interrupt on TX } /* * This function clears data packet content */ static void ClearRXDataPacket(){ for each element in packet set =0 } static void ClearRXBuffer(){ for each element in buffer set =0 } static void CopyToBuffer(){ for each element i set RX_Buffer_Array[i] = RX_DataPacket[i] } uint8_t GetRXBuffer(int index){ return element of buffer array at [index] } /*------------------------------- Footnotes -------------------------------*/ /*------------------------------ End of file ------------------------------*/