judsonupchurch/SET-Control-System
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
ee4bee53a1
SET-Control-System/Code/control_box/control_box.ino
judsonupchurch ae328bbc66 Initial Commit
2024-09-29 21:34:30 -05:00

969 lines
43 KiB
C++
Raw Blame History

// Switch inputs
#define EMERGENCY_SW 13
#define SUPPLY_FILL_SW 11
#define AUTO_FILL_SW 10
#define IGNITER_ARMING_SW 9 // On the designated PCB port it is 10. Using the continuity-check port right now
#define RUN_VENT_SW 7
#define BVAS_ARMING_SW 6
#define BVAS_SW 10 // On the designated PCB port it is 12. Using the auto-fill port right now
#define IGNITER_SW 8
#define SUPPLY_VENT_SW 3
#define FLUID_ARMING_SW 2
// LED Outputs
#define ENGINE_NODE_COMMS_LED 29
#define AB_COMMS_LED 30
#define AUTO_FILL_LED 32
#define SUPPLY_VENT_LED 37
#define SUPPLY_FILL_RELAY_LED 38
#define SUPPLY_FILL_LED 39
#define SUPPLY_VENT_RELAY_LED 40
#define IGNITER_RELAY_LED 41
#define RUN_VENT_LED 42
#define IGNITER_LED 43
#define IGNITER_CONTINUITY_LED 45
#define RUN_VENT_RELAY_LED 44
#define BVAS_RELAY_LED 46
#define BVAS_LED 47
// Arming LEDs
#define FLUID_ARMING_LED 17
#define IGNITER_ARMING_LED 16
#define BVAS_ARMING_LED 15
/*--------------------------------------------------------------------------------------------------------------------*/
/*-------------------------------------------SWITCH INPUTS------------------------------------------------------------*/
/*--------------------------------------------------------------------------------------------------------------------*/
struct switch_inputs_struct {
bool igniter_arming;
bool igniter;
bool fluid_arming;
bool supply_vent;
bool supply_fill;
bool run_vent;
bool bvas_arming;
bool bvas;
bool abort;
};
void read_switch_states(struct switch_inputs_struct *return_struct) {
// Igniter
return_struct->igniter_arming = digitalRead(IGNITER_ARMING_SW);
return_struct->igniter = return_struct->igniter_arming && digitalRead(IGNITER_SW);
// Fluid panel
return_struct->fluid_arming = digitalRead(FLUID_ARMING_SW);
return_struct->supply_vent = return_struct->fluid_arming && digitalRead(SUPPLY_VENT_SW);
return_struct->supply_fill = return_struct->fluid_arming && digitalRead(SUPPLY_FILL_SW);
return_struct->run_vent = return_struct->fluid_arming && digitalRead(RUN_VENT_SW);
// BVAS
return_struct->bvas_arming = digitalRead(BVAS_ARMING_SW);
return_struct->bvas = return_struct->bvas_arming && digitalRead(BVAS_SW);
// Abort
return_struct->abort = digitalRead(EMERGENCY_SW);
}
/*--------------------------------------------------------------------------------------------------------------------*/
/*----------------------------------------------CAN BUS---------------------------------------------------------------*/
/*--------------------------------------------------------------------------------------------------------------------*/
// For the CAN Bus module
#include <SPI.h>
#include <mcp2515.h>
MCP2515 mcp2515(49);
bool comms_with_ab = false;
unsigned long last_ab_can_message_sent;
unsigned long last_ab_can_message_received;
#define AB_CAN_INTERVAL 250 // In ms, how often we should send a CAN message
#define TIME_TO_INIT_PAUSE_STATE 2000 // If this many ms has gone by without a CAN message from CB, go into pause state
unsigned long pause_state_entered_time;
#define TIME_TO_INIT_ABORT_STATE 600000 // 10 minutes. If this many ms has gone by without a CAN message from CB, go into abort state
unsigned long abort_state_entered_time;
void update_can_command_message(struct can_frame *message, const struct switch_inputs_struct *switch_inputs) {
message->can_id = 50;
message->can_dlc = 8;
message->data[0] = switch_inputs->supply_fill;
message->data[1] = switch_inputs->supply_vent;
message->data[2] = switch_inputs->run_vent;
message->data[3] = switch_inputs->bvas;
message->data[4] = switch_inputs->igniter;
message->data[5] = 0;
message->data[6] = 0;
message->data[7] = 0;
}
bool read_from_ab(struct can_frame *ab_response) {
struct can_frame read_msg;
if (mcp2515.readMessage(&read_msg) == MCP2515::ERROR_OK) { // We received a CAN message
//Serial.println(read_msg.can_id);
if (read_msg.can_id == 50) { // AB is acknowledging our command message
last_ab_can_message_received = millis();
ab_response->can_id = read_msg.can_id;
ab_response->can_dlc = read_msg.can_dlc;
for (int i = 0; i < read_msg.can_dlc; i++) {
ab_response->data[i] = read_msg.data[i];
///Serial.print(ab_response->data[i]); ///Serial.print(" ");
}
for (int i = read_msg.can_dlc; i < 8; i++) { // Bytes after what the cb received
ab_response->data[i] = 0;
}
///Serial.println();
}
return true;
} else {
return false;
}
}
/*--------------------------------------------------------------------------------------------------------------------*/
/*------------------------------------------------DISPLAY BOARD-------------------------------------------------------*/
/*--------------------------------------------------------------------------------------------------------------------*/
// LCD Stuff
// For the I2C LCD on the display board
#include <Wire.h>
#include <LiquidCrystal_I2C.h>
LiquidCrystal_I2C lcd_lib(0x27, 20, 4); // This is the object from the library that we use to write to the LCD
// I will create my own LCD class so that we can store more information and functions
class LCD {
private:
String current_text[4] = {"", "", "", ""}; // An array of strings. Each element represents what is currently being shown on the row
String desired_text[4] = {"", "", "", ""}; // What we will want to update the LCD with
bool blank_rows[4] = {true, true, true, true}; // Keep track of any blank lines
public:
LCD() { // Construct the object
}
void clear_lcd() {
// Printing everything as a space is faster than the built in function
for(unsigned int i = 0; i < 4; i++) {
lcd_lib.setCursor(0, i);
lcd_lib.print(" "); // 20 Spaces
current_text[i] = "";
blank_rows[i] = true;
}
lcd_lib.setCursor(0, 0);
}
void clear_lcd_line(int row) {
// Printing everything as a space is faster than the built in function
// If row = 0, that is the first row on the lcd
lcd_lib.setCursor(0, row);
lcd_lib.print(" "); // 20 Spaces
current_text[row] = " ";
lcd_lib.setCursor(0, row);
blank_rows[row] = true;
}
void set_lcd_to_desired_text() {
// Go through each row and if the current_text string differs from the desired then we will write over it
for (unsigned int i = 0; i < 4; i++) {
if (current_text[i] != desired_text[i]) {
String extra_spaces = "";
for (int i = desired_text[i].length(); i < 20; i++) {
extra_spaces += " ";
}
lcd_lib.setCursor(0, i);
//Serial.println("Putting: [" + desired_text[i] + extra_spaces + "] to the LCD on row " + i);
lcd_lib.print(desired_text[i] + extra_spaces);
current_text[i] = desired_text[i];
}
String trimmed = current_text[i];
trimmed.trim();
if (trimmed == "") { // Row is blank
blank_rows[i] = true;
} else { // Row is not blank
blank_rows[i] = false;
}
}
lcd_lib.setCursor(0, 0);
}
int add_string_to_desired_text(String to_add, int row = -1) {
// Inputs a string that wants to be displayed and puts it on the first blank line available
// Returns the row that it was added to
if (to_add == "") {
return -1;
}
int already_on_line = check_if_already_in_desired_text(to_add);
if (already_on_line != -1) { // Already in the desired text
return already_on_line; // Return the line it is on already
}
if (row == -1) { // No row specification
for (unsigned int i = 0; i < 4; i++) { // Go through each available line and see if it is blank
if (blank_rows[i] == true) { // If the line is blank let's use it
desired_text[i] = to_add;
for (unsigned int j = desired_text[i].length(); j < 19; j++) {
desired_text[i] += " ";
}
blank_rows[i] = false; // No longer blank
return i; // Return the line it is now on
}
}
} else {
desired_text[row] = to_add;
for (unsigned int j = desired_text[row].length(); j < 19; j++) {
desired_text[row] += " ";
}
blank_rows[row] = false; // No longer blank
return row;
}
return -1;
}
int check_if_already_in_desired_text(String to_add) {
// Returns the row that the text is already at
for (unsigned int i = 0; i < 4; i++) {
if (to_add == desired_text[i]) {
return i;
}
}
return -1;
}
void clear_desired_text(int row = -1) {
// Resets the desired text to be empty
if (row == -1) {
for (unsigned int i = 0; i < 4; i++) {
desired_text[i] = "";
}
} else {
desired_text[row] = "";
}
}
};
LCD status_lcd; // The LCD that is on the display board
// Output LEDs
// We will control all of the outputs for the relays using classes.
// This is helpful because it is essentially setup where each relay has 3 LEDs and all use the same error formatting depending on the type of control class used: non-latching, latching, latching solenoid
// Because each type of solenoid setup has its own possible errors, we need to account for them differently. Therefore this will also do error message parsing
class NonLatchingRelayDisplays {
private:
unsigned int relay_led_pin; // Represents if the AB saw the on signal and set the pin HIGH
unsigned int state_led_pin; // Represents if the AB actually saw a voltage across the relay and thinks the valve was opened
String name; // So our LCD messages have meaning
String pin_error_message = ""; // If we come across a pin error state, update this string so the LCD can find it and print it
String relay_error_message = ""; // If we come across a relay error state, update this string so the LCD can find it and print it
public:
int pin_error_message_row = -1; // The row that the error message is on the LCD. -1 means it is not on the LCD
int relay_error_message_row = -1; // The row that the error message is on the LCD. -1 means it is not on the LCD
NonLatchingRelayDisplays (unsigned int relay_led_pin, unsigned int state_led_pin, String name) {
this->relay_led_pin = relay_led_pin;
this->state_led_pin = state_led_pin;
this->name = name;
pinMode(relay_led_pin, OUTPUT);
pinMode(state_led_pin, OUTPUT);
digitalWrite(relay_led_pin, LOW);
digitalWrite(state_led_pin, LOW);
}
String get_pin_error_message() {
// Returns what pin error message string we currently have
return pin_error_message;
}
String get_relay_error_message() {
// Returns what relay error message string we currently have
return relay_error_message;
}
void control_leds(unsigned int status) {
// The AB sends back a 3 digit integer encoded with the possible errors/states of the relays
unsigned int ones_place = status % 10;
unsigned int tens_place = (status % 100) / 10;
// Controlling the state_led (ones_place)
if (ones_place == 1 || ones_place == 4) { // 1 is on and no relay error. 4 is on and relay error
digitalWrite(state_led_pin, HIGH);
} else if (ones_place == 0 || ones_place == 3) { // 0 is off and no relay error. 3 is off and relay error
digitalWrite(state_led_pin, LOW);
}
// Controlling the relay_led
if (tens_place == 1 || tens_place == 4) { // 1 is relay should be on and no pin error. 4 is relay should be on and pin error
digitalWrite(relay_led_pin, HIGH);
} else if (tens_place == 0 || tens_place == 3) { // 0 is relay should be off and no pin error. 3 is relay should be off and pin error
digitalWrite(relay_led_pin, LOW);
}
// Setting error messages for the LCD
// Relay errors
if (ones_place == 5 || ones_place == 4) { // The voltage across the relay is not what it should be
relay_error_message = name + "relay FAIL";
} else if (ones_place == 0 || ones_place == 1) { // No relay failure
relay_error_message = "";
}
// Pin errors
if (tens_place == 3 || tens_place == 4) { // There was a pin failure
pin_error_message = name + " pin FAIL";
} else if (tens_place == 0 || tens_place == 1) { // No error
pin_error_message = "";
}
}
};
class LatchingRelayDisplays {
private:
unsigned int relay_led_pin; // Represents if the AB saw the on signal and set the pin HIGH
unsigned int state_led_pin; // Represents if the AB actually saw a voltage across the relay and thinks the valve was opened
String name; // So our LCD messages have meaning
String pin_error_message = ""; // If we come across a pin error state, update this string so the LCD can find it and print it
String relay_error_message = ""; // If we come across a relay error state, update this string so the LCD can find it and print it
public:
int pin_error_message_row = -1; // The row that the error message is on the LCD. -1 means it is not on the LCD
int relay_error_message_row = -1; // The row that the error message is on the LCD. -1 means it is not on the LCD
LatchingRelayDisplays (unsigned int relay_led_pin, unsigned int state_led_pin, String name) {
this->relay_led_pin = relay_led_pin;
this->state_led_pin = state_led_pin;
this->name = name;
pinMode(relay_led_pin, OUTPUT);
pinMode(state_led_pin, OUTPUT);
digitalWrite(relay_led_pin, LOW);
digitalWrite(state_led_pin, LOW);
}
String get_pin_error_message() {
// Returns what pin error message string we currently have
return pin_error_message;
}
String get_relay_error_message() {
// Returns what relay error message string we currently have
return relay_error_message;
}
void control_leds(unsigned int status) {
// The AB sends back a 3 digit integer encoded with the possible errors/states of the relays
unsigned int ones_place = status % 10;
unsigned int tens_place = (status % 100) / 10;
// Controlling the state_led (ones_place)
if (ones_place == 1 || ones_place == 4) { // 1 is on and no relay error. 4 is on and relay error
digitalWrite(state_led_pin, HIGH);
} else if (ones_place == 0 || ones_place == 3) { // 0 is off and no relay error. 3 is off and relay error
digitalWrite(state_led_pin, LOW);
}
// Controlling the relay_led
if (tens_place == 1 || tens_place == 4 || tens_place == 6 || tens_place == 9) { // 1 is relay should be on and no pin error. 4 is relay should be on and setpin error. 6 is relay should be on and resetpin error. 9 is relay should be on and both pin errors
digitalWrite(relay_led_pin, HIGH);
} else if (tens_place == 0 || tens_place == 3 || tens_place == 5 || tens_place == 8) { // 0 is relay should be off and no pin error. 3 is relay should be off and setpin error. 5 is relay should be off and resetpin error. 8 is relay should be off and both pin errors
digitalWrite(relay_led_pin, LOW);
}
// Setting error messages for the LCD
// Relay errors
if (ones_place == 3 || ones_place == 4) { // The voltage across the relay is not what it should be
relay_error_message = name + " relay FAIL";
} else if (ones_place == 0 || ones_place == 1) { // No relay failure
relay_error_message = "";
}
// Pin failure messages
if (tens_place == 3 || tens_place == 4) { // There was only a setpin failure
pin_error_message = name + " R pin FAIL";
} else if (tens_place == 5 || tens_place == 6) { // There was only a resetpin failure
pin_error_message = name + " R pin FAIL";
} else if (tens_place == 8 || tens_place == 9) { // Both pins failed
pin_error_message = name + " R&S pin FAIL";
} else if (tens_place == 0 || tens_place == 1) { // No error
pin_error_message = "";
}
}
};
class LatchingSolenoidDisplays {
private:
unsigned int relay_led_pin; // Represents if the AB saw the on signal and set the pin HIGH
unsigned int state_led_pin; // Represents if the AB actually saw a voltage across the relay and thinks the valve was opened
String name; // So our LCD messages have meaning
String pin_error_message = ""; // If we come across a pin error state, update this string so the LCD can find it and print it
String relay_error_message = ""; // If we come across a relay error state, update this string so the LCD can find it and print it
public:
int pin_error_message_row = -1; // The row that the error message is on the LCD. -1 means it is not on the LCD
int relay_error_message_row = -1; // The row that the error message is on the LCD. -1 means it is not on the LCD
LatchingSolenoidDisplays (unsigned int relay_led_pin, unsigned int state_led_pin, String name) {
this->relay_led_pin = relay_led_pin;
this->state_led_pin = state_led_pin;
this->name = name;
pinMode(relay_led_pin, OUTPUT);
pinMode(state_led_pin, OUTPUT);
digitalWrite(relay_led_pin, LOW);
digitalWrite(state_led_pin, LOW);
}
String get_pin_error_message() {
// Returns what pin error message string we currently have
return pin_error_message;
}
String get_relay_error_message() {
// Returns what relay error message string we currently have
return relay_error_message;
}
void control_leds(unsigned int on_relay_status, unsigned int off_relay_status) {
// The AB sends back a 3 digit integer encoded with the possible errors/states of the relays
unsigned int on_ones_place = on_relay_status % 10;
unsigned int on_tens_place = (on_relay_status % 100) / 10;
unsigned int on_hundreds_place = (on_relay_status % 1000) / 100;
// This is redundant to the on_ones_place. unsigned int off_ones_place = off_relay_status % 10;
unsigned int off_tens_place = (off_relay_status % 100) / 10;
unsigned int off_hundreds_place = (off_relay_status % 1000) / 100;
// Controlling the state_led (ones_place)
if (on_ones_place == 1 || on_ones_place == 4) { // 1 is on and no relay error. 4 is on and relay error
digitalWrite(state_led_pin, HIGH);
} else if (on_ones_place == 0 || on_ones_place == 3) { // 0 is off and no relay error. 3 is off and relay error
digitalWrite(state_led_pin, LOW);
}
// Controlling the relay_led
if (on_tens_place == 1 || on_tens_place == 4) { // 1 is desired to be off and no pin error. 4 is on and relay error
digitalWrite(relay_led_pin, HIGH);
} else if (on_tens_place == 0 || on_tens_place == 3) { // 0 is desired to be off and no pin error. 3 is off and relay error
digitalWrite(relay_led_pin, LOW);
}
// Pin failure message
bool on_pin_fail = on_tens_place == 3 || on_tens_place == 4;
bool off_pin_fail = off_tens_place == 3 || off_tens_place == 4;
if (on_pin_fail && off_pin_fail) { // There was a pin failure for both the ON and OFF relays
pin_error_message = name + " both pin FAIL";
} else if (on_pin_fail) { // Only the ON pin failed
pin_error_message = name + " ON pin FAIL";
} else if (off_pin_fail) { // Only the OFF pin failed
pin_error_message = name + " OFF pin FAIL";
} else { // Neither pin failed
pin_error_message = "";
}
// Relay failure message
bool on_relay_fail = on_hundreds_place == 1 || on_hundreds_place == 2;
bool off_relay_fail = off_hundreds_place == 1 || off_hundreds_place == 2;
if (on_relay_fail && off_relay_fail) { // There was a relay failure for both the ON and OFF relays
relay_error_message = name + " relays FAIL";
} else if (on_relay_fail) { // Only the ON relay failed
relay_error_message = name + " ON relay FAIL";
} else if (off_relay_fail) { // Only the OFF relay failed
relay_error_message = name + " OFF relay FAIL";
} else { // Neither relay failed
relay_error_message = "";
}
}
};
NonLatchingRelayDisplays supply_fill_leds(SUPPLY_FILL_RELAY_LED, SUPPLY_FILL_LED, "Sup Fill"); // Even though it is a servo, it is compatible with this class
LatchingRelayDisplays supply_vent_leds(SUPPLY_VENT_RELAY_LED, SUPPLY_VENT_LED, "Sup Vnt");
LatchingRelayDisplays igniter_leds(IGNITER_RELAY_LED, IGNITER_LED, "Igniter");
LatchingSolenoidDisplays bvas_leds(BVAS_RELAY_LED, BVAS_LED, "BVAS");
LatchingSolenoidDisplays run_vent_leds(RUN_VENT_RELAY_LED, RUN_VENT_LED, "Rn Vnt");
void control_all_leds(const struct can_frame *ab_response, const struct switch_inputs_struct *switch_inputs) {
// Control the arming LEDS
digitalWrite(FLUID_ARMING_LED, switch_inputs->fluid_arming);
digitalWrite(BVAS_ARMING_LED, switch_inputs->bvas_arming);
digitalWrite(IGNITER_ARMING_LED, switch_inputs->igniter_arming);
// COMMS LEDs
digitalWrite(AB_COMMS_LED, comms_with_ab);
// Controlling all LEDs associated with the actuator box
supply_fill_leds.control_leds(ab_response->data[0]);
supply_vent_leds.control_leds(ab_response->data[1]);
run_vent_leds.control_leds(ab_response->data[2], ab_response->data[3]);
bvas_leds.control_leds(ab_response->data[4], ab_response->data[5]);
igniter_leds.control_leds(ab_response->data[6]);
unsigned int igniter_hundreds_place = (ab_response->data[6] % 1000) / 100;
// Specifically the igniter continuity LED
digitalWrite(IGNITER_CONTINUITY_LED, igniter_hundreds_place);
}
void control_ab_related_leds(const struct can_frame *ab_response) {
// Controlling all LEDs associated with the actuator box
supply_fill_leds.control_leds(ab_response->data[0]);
supply_vent_leds.control_leds(ab_response->data[1]);
run_vent_leds.control_leds(ab_response->data[2], ab_response->data[3]);
bvas_leds.control_leds(ab_response->data[4], ab_response->data[5]);
igniter_leds.control_leds(ab_response->data[6]);
unsigned int igniter_hundreds_place = (ab_response->data[6] % 1000) / 100;
// Specifically the igniter continuity LED
digitalWrite(IGNITER_CONTINUITY_LED, igniter_hundreds_place);
}
void control_cb_related_leds(const struct switch_inputs_struct *switch_inputs) {
// Control the arming LEDS
digitalWrite(FLUID_ARMING_LED, switch_inputs->fluid_arming);
digitalWrite(BVAS_ARMING_LED, switch_inputs->bvas_arming);
digitalWrite(IGNITER_ARMING_LED, switch_inputs->igniter_arming);
// COMMS LEDs
digitalWrite(AB_COMMS_LED, comms_with_ab);
}
void write_errors_to_lcd() {
// Loop through all relay "pin_error_message" & "relay_error_message" and write as much as we can.
// Supply fill
if (supply_fill_leds.get_pin_error_message() != "") { // If there actually is an error message
supply_fill_leds.pin_error_message_row = status_lcd.add_string_to_desired_text(supply_fill_leds.get_pin_error_message());
}
if (supply_fill_leds.get_relay_error_message() != "") { // If there actually is an error message
supply_fill_leds.relay_error_message_row = status_lcd.add_string_to_desired_text(supply_fill_leds.get_relay_error_message());
}
// Supply vent
if (supply_vent_leds.get_pin_error_message() != "") { // If there actually is an error message
supply_vent_leds.pin_error_message_row = status_lcd.add_string_to_desired_text(supply_vent_leds.get_pin_error_message());
}
if (supply_vent_leds.get_relay_error_message() != "") { // If there actually is an error message
supply_vent_leds.relay_error_message_row = status_lcd.add_string_to_desired_text(supply_vent_leds.get_relay_error_message());
}
// Igniter
if (igniter_leds.get_pin_error_message() != "") { // If there actually is an error message
igniter_leds.pin_error_message_row = status_lcd.add_string_to_desired_text(igniter_leds.get_pin_error_message());
}
if (igniter_leds.get_relay_error_message() != "") { // If there actually is an error message
igniter_leds.pin_error_message_row = status_lcd.add_string_to_desired_text(igniter_leds.get_relay_error_message());
}
// BVAS
if (bvas_leds.get_pin_error_message() != "") { // If there actually is an error message
bvas_leds.pin_error_message_row = status_lcd.add_string_to_desired_text(bvas_leds.get_pin_error_message());
}
if (bvas_leds.get_relay_error_message() != "") { // If there actually is an error message
bvas_leds.pin_error_message_row = status_lcd.add_string_to_desired_text(bvas_leds.get_relay_error_message());
}
// Run vent
if (run_vent_leds.get_pin_error_message() != "") { // If there actually is an error message
run_vent_leds.pin_error_message_row = status_lcd.add_string_to_desired_text(run_vent_leds.get_pin_error_message());
}
if (run_vent_leds.get_relay_error_message() != "") { // If there actually is an error message
run_vent_leds.pin_error_message_row = status_lcd.add_string_to_desired_text(run_vent_leds.get_relay_error_message());
}
status_lcd.set_lcd_to_desired_text(); // Actually update the screen
}
void write_igniter_countup() {
// TODO. In the bottom right of the LCD, write 3 places (00.0) of time for a timer after ignition
}
bool establish_can_with_ab() {
// Establishes CAN with the Actuator Box.
comms_with_ab = false;
struct can_frame ab_command;
ab_command.can_id = 5;
ab_command.can_dlc = 8;
ab_command.data[0] = 1;
ab_command.data[1] = 1;
ab_command.data[2] = 1;
ab_command.data[3] = 1;
ab_command.data[4] = 1;
ab_command.data[5] = 1;
ab_command.data[6] = 1;
ab_command.data[7] = 1;
mcp2515.sendMessage(&ab_command);
unsigned long last_ping_sent = millis();
while(comms_with_ab == false) {
struct can_frame ab_response;
if (mcp2515.readMessage(&ab_response) == MCP2515::ERROR_OK) { // We received a CAN message
///Serial.println("Received CAN from AB");
if (ab_response.can_id == 5 && ab_response.data[0] == 2) { // AB is acknowledging our ping
///Serial.println("AB was responding to our ping");
comms_with_ab = true;
last_ab_can_message_received = millis();
ab_command.data[0] = 3;
ab_command.data[1] = 3;
ab_command.data[2] = 3;
ab_command.data[3] = 3;
ab_command.data[4] = 3;
ab_command.data[5] = 3;
ab_command.data[6] = 3;
ab_command.data[7] = 3;
delay(15); // I like waiting for the line to stabilize and give time
///Serial.println("Telling AB we heard it");
///Serial.println("2-Way CAN established");
mcp2515.sendMessage(&ab_command); // Tell AB we heard it
}
} else {
if (millis() - last_ping_sent >= AB_CAN_INTERVAL) { // Wait some time before pinging again
///Serial.println("Sent ping to AB");
mcp2515.sendMessage(&ab_command);
last_ping_sent = millis();
}
}
}
return true;
}
/*--------------------------------------------------------------------------------------------------------------------*/
/*-------------------------------------EMERGENCY PROCEDURE/STATE MANAGEMENT-------------------------------------------*/
/*--------------------------------------------------------------------------------------------------------------------*/
// Global variables to keep track of what the actuator box has told us is the state of the system
struct ab_states_struct {
bool igniter = false;
bool bvas = false;
bool supply_fill = false;
bool supply_vent = false;
bool run_vent = false;
};
ab_states_struct ab_states;
void update_system_states(const struct can_frame *ab_response) {
unsigned int ones_place; // Used for parsing the data
// Supply fill
ones_place = (ab_response->data[0] % 10);
if (ones_place == 1) { // Supply fill is open
ab_states.supply_fill = true;
} else {
ab_states.supply_fill = false;
}
// Supply vent
ones_place = (ab_response->data[1] % 10);
if (ones_place == 1 || ones_place == 4) { // Supply fill is open
ab_states.supply_vent = true;
} else {
ab_states.supply_vent = false;
}
// Run tank vent
ones_place = (ab_response->data[2] % 10);
if (ones_place == 1 || ones_place == 4) { // Run tank vent is open
ab_states.run_vent = true;
} else {
ab_states.run_vent = false;
}
// BVAS
ones_place = (ab_response->data[4] % 10);
if (ones_place == 1 || ones_place == 4) { // BVAS is open
ab_states.bvas = true;
} else {
ab_states.bvas = false;
}
// Igniter
ones_place = (ab_response->data[6] % 10);
if (ones_place == 1 || ones_place == 4) { // Supply fill is open
ab_states.igniter = true;
} else {
ab_states.igniter = false;
}
}
void abort_loop() {
// When the control presses the abort switch.
// Igniter off, close supply fill, open run vent, open supply vent, open bvas
const unsigned long abort_initiated_time = millis(); // This is the time we first turned on the abort button. Needed to make sure we give supply fill adequate time to close
switch_inputs_struct switch_inputs;
can_frame command_message;
command_message.can_id = 5;
command_message.can_dlc = 8;
command_message.data[0] = 0; // Close the supply fill
command_message.data[4] = 0; // Turn off the igniter
command_message.data[5] = 9; // Tell the AB we are aborting
command_message.data[6] = 9; // Tell the AB we are aborting
command_message.data[7] = 9; // Tell the AB we are aborting
can_frame ab_response;
// Loop until the abort switch is turned off
bool supply_fill_open = ab_states.supply_fill; // Wanted so we actually wait for supply fill to close
while (1) {
if (supply_fill_open == true) { // If the supply fill is open, we should close it before opening the vents
command_message.data[1] = 3; // Tell AB to keep the supply vent state
command_message.data[2] = 3; // Tell AB to keep the run vent state
command_message.data[3] = 3; // Tell AB to keep the BVAS state
if (millis() - abort_initiated_time >= 1000) { // Has been enough time for the supply fill to close and for us to vent the lines
supply_fill_open = false; // It has now been closed
}
} else { // Supply fill valve is no longer open
command_message.data[1] = 1; // Open the supply vent
command_message.data[2] = 1; // Open the run tank vent
command_message.data[3] = 3; // Tell AB to keep the BVAS state
}
// We will send our message now
if (millis() - last_ab_can_message_sent >= (AB_CAN_INTERVAL)) { // Only send at most every whatever time specified
print_can_frame(command_message);
mcp2515.sendMessage(&command_message);
last_ab_can_message_sent = millis();
}
if (read_from_ab(&ab_response)) { // Get the AB response if there is one. If not, the we keep the same response from previous
update_system_states(&ab_response); // Update the control box's tracking of states based upon the AB's response
control_ab_related_leds(&ab_response);
}
// Control all of the LEDs on the display board
// control_all_leds(&ab_response, &switch_inputs);
read_switch_states(&switch_inputs);
control_cb_related_leds(&switch_inputs);
comms_with_ab = (millis() - last_ab_can_message_received >= TIME_TO_INIT_PAUSE_STATE) ? false : true;
// Control the LCD on the display board
write_errors_to_lcd(); // FIXME?
if (switch_inputs.abort == false) { // Only leave if the emergency switch is turned off
break;
}
}
}
bool ab_entered_pause_state() {
// Returns true if enough time has passed that we believe the AB would have entered its pause state
if (millis() - last_ab_can_message_received > TIME_TO_INIT_PAUSE_STATE) {
return true;
} else {
return false;
}
}
bool ab_entered_abort_state() {
// Returns true if enough time has passed that we believe the AB would have entered its abort state
if (millis() - last_ab_can_message_received > TIME_TO_INIT_ABORT_STATE) {
return true;
} else {
return false;
}
}
bool reestablish_can_with_ab() {
// Reestablishes CAN with the Actuator Box.
comms_with_ab = false;
struct can_frame ab_command;
ab_command.can_id = 5;
ab_command.can_dlc = 8;
ab_command.data[0] = 1;
ab_command.data[1] = 1;
ab_command.data[2] = 1;
ab_command.data[3] = 1;
ab_command.data[4] = 1;
ab_command.data[5] = 1;
ab_command.data[6] = 1;
ab_command.data[7] = 1;
///Serial.println("Sent ping to AB");
mcp2515.sendMessage(&ab_command);
unsigned long last_ping_sent = millis();
struct can_frame ab_response;
while (millis() - last_ping_sent < AB_CAN_INTERVAL) {
if (mcp2515.readMessage(&ab_response) == MCP2515::ERROR_OK) { // We received a CAN message
//Serial.println("Received ackowledgement from AB");
if (ab_response.can_id == 5 && ab_response.data[0] == 2) { // AB is acknowledging our ping
comms_with_ab = true;
last_ab_can_message_received = millis();
ab_command.data[0] = 3;
ab_command.data[1] = 3;
ab_command.data[2] = 3;
ab_command.data[3] = 3;
ab_command.data[4] = 3;
ab_command.data[5] = 3;
ab_command.data[6] = 3;
ab_command.data[7] = 3;
delay(50); // I like waiting for the line to stabilize and give time
//Serial.print("Sent AB message with ID: "); ///Serial.print(ab_command.can_id); ///Serial.print(" And data[0]: "); ///Serial.println(ab_command.data[0]);
mcp2515.sendMessage(&ab_command); // Tell AB we heard it
return true;
}
}
}
return false; // No comms yet
}
/*--------------------------------------------------------------------------------------------------------------------*/
/*----------------------------------------------------SETUP-----------------------------------------------------------*/
/*--------------------------------------------------------------------------------------------------------------------*/
void setup() {
Serial.begin(115200);
pinMode(EMERGENCY_SW, INPUT);
pinMode(SUPPLY_FILL_SW, INPUT);
pinMode(AUTO_FILL_SW, INPUT);
pinMode(IGNITER_ARMING_SW, INPUT);
pinMode(RUN_VENT_SW, INPUT);
pinMode(BVAS_ARMING_SW, INPUT);
pinMode(BVAS_SW, INPUT);
pinMode(IGNITER_SW, INPUT);
pinMode(SUPPLY_VENT_SW, INPUT);
pinMode(FLUID_ARMING_SW, INPUT);
pinMode(FLUID_ARMING_LED, OUTPUT);
pinMode(IGNITER_ARMING_LED, OUTPUT);
pinMode(BVAS_ARMING_LED, OUTPUT);
pinMode(ENGINE_NODE_COMMS_LED, OUTPUT);
pinMode(AB_COMMS_LED, OUTPUT);
pinMode(AUTO_FILL_LED, OUTPUT);
pinMode(IGNITER_CONTINUITY_LED, OUTPUT);
lcd_lib.init();
lcd_lib.backlight();
status_lcd.clear_desired_text();
status_lcd.clear_lcd();
status_lcd.add_string_to_desired_text("Connecting to AB...", 0);
status_lcd.set_lcd_to_desired_text();
mcp2515.reset();
mcp2515.setBitrate(CAN_125KBPS);
mcp2515.setNormalMode();
// Initializes and establishes CAN comms to AB
///Serial.println("Connecting to AB");
establish_can_with_ab();
status_lcd.clear_lcd();
status_lcd.clear_desired_text();
}
/*--------------------------------------------------------------------------------------------------------------------*/
/*-----------------------------------------------------LOOP-----------------------------------------------------------*/
/*--------------------------------------------------------------------------------------------------------------------*/
void loop() {
static switch_inputs_struct switch_inputs;
static can_frame command_message;
static can_frame ab_response;
// Check for any lost comms and try to predict what is going on with the actuator box
if (ab_entered_pause_state() == true) { // We think the AB went into a pause state because we haven't seen comms in a bit
///Serial.println("AB entered pause state");
comms_with_ab = false;
pause_state_entered_time = millis();
digitalWrite(AB_COMMS_LED, LOW);
status_lcd.clear_lcd();
status_lcd.clear_desired_text();
status_lcd.clear_desired_text(0);
status_lcd.add_string_to_desired_text("Bad Comms|AB PAUSED", 0);
status_lcd.clear_desired_text(1);
status_lcd.add_string_to_desired_text("Supply fill, vent,", 1);
status_lcd.clear_desired_text(2);
status_lcd.add_string_to_desired_text("Run vent closed", 2);
status_lcd.set_lcd_to_desired_text(); // Actually update the screen
unsigned long last_updated_time_for_pause_lcd = 0;
while (ab_entered_abort_state() == false) { // Loop for as long as we can until we think the AB went into the abort state
if (reestablish_can_with_ab()) { // If we managed to reestablish comms
///Serial.println("Reestablished CAN with AB");
digitalWrite(AB_COMMS_LED, HIGH);
break; // We reestablished so resume normal comms
}
if (millis() - last_updated_time_for_pause_lcd >= 250) {
// LCD message counting time since lost comms
String foo = String((TIME_TO_INIT_ABORT_STATE - (millis() - pause_state_entered_time)) / 1000) + "." + String(((TIME_TO_INIT_ABORT_STATE - (millis() - pause_state_entered_time)) / 100)%10) + "s until ABORT";
status_lcd.clear_desired_text(3);
status_lcd.add_string_to_desired_text(foo, 3);
status_lcd.set_lcd_to_desired_text(); // Actually update the screen
last_updated_time_for_pause_lcd = millis();
}
read_switch_states(&switch_inputs);
control_cb_related_leds(&switch_inputs);
}
// At this point we the AB either aborted or we reestablished comms
if (ab_entered_abort_state() == true) {
//Serial.println("AB ABORTED");
status_lcd.clear_lcd();
status_lcd.clear_desired_text(0);
status_lcd.add_string_to_desired_text("AB ABORTED", 0);
status_lcd.set_lcd_to_desired_text(); // Actually update the screen
while (1) {
read_switch_states(&switch_inputs);
control_cb_related_leds(&switch_inputs);
}
}
// If we are at this point then we reestablished comms
status_lcd.clear_lcd();
status_lcd.clear_desired_text();
}
// Lets get the most current state of the switches
read_switch_states(&switch_inputs);
// Check to see if we need to be in the abort priority loop because of the switch
if (switch_inputs.abort == true) {
status_lcd.clear_desired_text(); // Reset all to blank
status_lcd.clear_lcd();
status_lcd.clear_desired_text(0);
status_lcd.add_string_to_desired_text("ABORT INITIATED", 0);
status_lcd.set_lcd_to_desired_text(); // Actually update the screen
///Serial.println("ABORT LOOP Entered");
abort_loop(); // Auto-loops itself until the abort switch is turned off
///Serial.println("Left abort loop");
status_lcd.clear_lcd();
status_lcd.clear_desired_text();
// Let's get the most current state of the switches in case they were changed while we were aborting
read_switch_states(&switch_inputs);
}
// Update the command CAN message and send it
update_can_command_message(&command_message, &switch_inputs);
if (millis() - last_ab_can_message_sent >= AB_CAN_INTERVAL) { // Only send at most every whatever time specified
mcp2515.sendMessage(&command_message);
last_ab_can_message_sent = millis();
}
if (read_from_ab(&ab_response)) { // Get the AB response if there is one. If not, the we keep the same response from previous
update_system_states(&ab_response); // Update the control box's tracking of states based upon the AB's response
control_ab_related_leds(&ab_response);
}
// Control all of the LEDs on the display board
// control_all_leds(&ab_response, &switch_inputs);
control_cb_related_leds(&switch_inputs);
// Control the LCD on the display board
write_errors_to_lcd();
}
void print_can_frame(struct can_frame& can_message) {
Serial.println("CAN FRAME:");
Serial.print("CAN ID"); Serial.println(can_message.can_id);
Serial.print("CAN DLC"); Serial.println(can_message.can_dlc);
Serial.print("DATA 0 - Supply Fill: "); Serial.println(can_message.data[0]);
Serial.print("DATA 1 - Supply Vent: "); Serial.println(can_message.data[1]);
Serial.print("DATA 2 - Run Tank Vent: "); Serial.println(can_message.data[2]);
Serial.print("DATA 3 - BVAS: "); Serial.println(can_message.data[3]);
Serial.print("DATA 4 - Igniter: "); Serial.println(can_message.data[4]);
Serial.print("DATA 5: "); Serial.println(can_message.data[5]);
Serial.print("DATA 6: "); Serial.println(can_message.data[6]);
Serial.print("DATA 7: "); Serial.println(can_message.data[7]);
Serial.println("-------------------------------------------------\n");
}
Reference in New Issue View Git Blame Copy Permalink