841 lines
39 KiB
C++
841 lines
39 KiB
C++
// Digital write pins to turn on the relays and readback if the Arduino pin is actually going HIGH or LOW
|
|
#define IGNITER_CMD_ON 26
|
|
#define IGNITER_CMD_ON_READBACK 27
|
|
#define IGNITER_CMD_OFF 28
|
|
#define IGNITER_CMD_OFF_READBACK 29
|
|
#define SUPPLY_FILL_CMD 30 // Not needed, changed to servo
|
|
#define SUPPLY_FILL_CMD_READBACK 31 // Not needed, changed to servo
|
|
#define SUPPLY_VENT_CMD_ON 32
|
|
#define SUPPLY_VENT_CMD_ON_READBACK 33
|
|
#define SUPPLY_VENT_CMD_OFF 34
|
|
#define SUPPLY_VENT_CMD_OFF_READBACK 35
|
|
#define RUN_VENT_CMD_ON 36
|
|
#define RUN_VENT_CMD_ON_READBACK 37
|
|
#define RUN_VENT_CMD_OFF 38
|
|
#define RUN_VENT_CMD_OFF_READBACK 39
|
|
#define SPARE1_CMD_ON 40
|
|
#define SPARE1_CMD_ON_READBACK 41
|
|
#define SPARE1_CMD_OFF 42
|
|
#define SPARE1_CMD_OFF_READBACK 43
|
|
#define SPARE2_CMD_ON 44
|
|
#define SPARE2_CMD_ON_READBACK 45
|
|
#define SPARE2_CMD_OFF 46
|
|
#define SPARE2_CMD_OFF_READBACK 47
|
|
#define SPARE3_CMD 48
|
|
#define SPARE3_CMD_READBACK 49
|
|
|
|
// Relay voltage readbacks
|
|
#define IGNITER_CONTINUITY_CHECK A0
|
|
#define IGNITER_VOLTAGE_CHECK A1
|
|
#define SUPPLY_FILL_VOLTAGE_CHECK A2 // Not needed, changed to servo
|
|
#define SUPPLY_VENT_VOLTAGE_CHECK A3
|
|
#define RUN_VENT_ON_VOLTAGE_CHECK A6
|
|
#define RUN_VENT_OFF_VOLTAGE_CHECK A12
|
|
#define SPARE1_VOLTAGE_CHECK A13
|
|
#define SPARE2_VOLTAGE_CHECK A14
|
|
#define SPARE3_VOLTAGE_CHECK A15
|
|
|
|
// Just for the BVAS control while we wait for the plumbing board and its systems to be integrated
|
|
#define BVAS_CMD_ON 30
|
|
#define BVAS_CMD_ON_READBACK 31
|
|
#define BVAS_ON_VOLTAGE_CHECK A2
|
|
#define BVAS_CMD_OFF 48
|
|
#define BVAS_CMD_OFF_READBACK 49
|
|
#define BVAS_OFF_VOLTAGE_CHECK A15
|
|
|
|
#define ENABLE_BVAS_SERVO true // This tells the actuator box to not use the pneumatic BVAS and to instead use the servo bvas
|
|
#define ENABLE_BVAS_DELAY false // We we want to have a delay from receiving MDOT command to actually opening the valve
|
|
#define BVAS_DELAY 7000 // Milleseconds to delay for BVAS
|
|
|
|
/*--------------------------------------------------------------------------------------------------------------------*/
|
|
/*--------------------------------------------------ETHERNET COMMS----------------------------------------------------*/
|
|
/*--------------------------------------------------------------------------------------------------------------------*/
|
|
#define ENABLE_ETHERNET_COMMS false // Whether or not we are using ethernet comms at all or not
|
|
|
|
#if (ENABLE_ETHERNET_COMMS == true)
|
|
#define ETHERNET_CS_PIN 25 // CS pin on PCB to the ethernet module
|
|
|
|
#ifndef EthernetUDPWrapper_h
|
|
#include "EthernetUDPWrapper.h"
|
|
#endif
|
|
|
|
unsigned int port = 5000;
|
|
IPAddress ip(192, 168, 0, 115); // IP address of the Actuator Box. Set the router to bind to this IP address. Also update DAQ GUI code to match. IP address can change to whatever is needed
|
|
|
|
EthernetUDPWrapper UDPWrapper(ETHERNET_CS_PIN, port, ip);
|
|
#endif
|
|
|
|
|
|
/*--------------------------------------------------------------------------------------------------------------------*/
|
|
/*--------------------------------------------------RELAY CONTROL-----------------------------------------------------*/
|
|
/*--------------------------------------------------------------------------------------------------------------------*/
|
|
// Used for latching relay control
|
|
#ifndef LatchingRelay_h
|
|
#include "LatchingRelay.h"
|
|
#endif
|
|
|
|
// Used for non-latching relay control
|
|
#ifndef NonLatchingRelay_h
|
|
#include "NonLatchingRelay.h"
|
|
#endif
|
|
|
|
// Used for solenoids that are latching (run-vent, BVAS)
|
|
#ifndef LatchingSolenoid_h
|
|
#include "LatchingSolenoid.h"
|
|
#endif
|
|
|
|
// Defining relay objects
|
|
LatchingRelay igniter_relay("Igniter Relay", IGNITER_CMD_ON, IGNITER_CMD_ON_READBACK, IGNITER_CMD_OFF, IGNITER_CMD_OFF_READBACK, IGNITER_VOLTAGE_CHECK);
|
|
LatchingRelay supply_vent_relay("Supply Vent Relay", SUPPLY_VENT_CMD_ON, SUPPLY_VENT_CMD_ON_READBACK, SUPPLY_VENT_CMD_OFF, SUPPLY_VENT_CMD_OFF_READBACK, SUPPLY_VENT_VOLTAGE_CHECK);
|
|
LatchingRelay spare1_relay(SPARE1_CMD_ON, SPARE1_CMD_ON_READBACK, SPARE1_CMD_OFF, SPARE1_CMD_OFF_READBACK, SPARE1_VOLTAGE_CHECK);
|
|
LatchingRelay spare2_relay(SPARE2_CMD_ON, SPARE2_CMD_ON_READBACK, SPARE2_CMD_OFF, SPARE2_CMD_OFF_READBACK, SPARE2_VOLTAGE_CHECK);
|
|
|
|
LatchingSolenoid run_vent_solenoid("Run Vent Solenoid", RUN_VENT_CMD_ON, RUN_VENT_CMD_ON_READBACK, RUN_VENT_CMD_OFF, RUN_VENT_CMD_OFF_READBACK, RUN_VENT_ON_VOLTAGE_CHECK, RUN_VENT_OFF_VOLTAGE_CHECK);
|
|
|
|
#if (ENABLE_BVAS_SERVO == false)
|
|
LatchingSolenoid bvas_solenoid("BVAS Solenoid", BVAS_CMD_ON, BVAS_CMD_ON_READBACK, BVAS_CMD_OFF, BVAS_CMD_OFF_READBACK, BVAS_ON_VOLTAGE_CHECK, BVAS_OFF_VOLTAGE_CHECK);
|
|
#endif
|
|
|
|
bool igniter_continuity_check(unsigned int pin) {
|
|
unsigned int raw_read = analogRead(pin); // maps (0V, 5V) to (0, 1023)
|
|
float voltage = raw_read*(5.0/1024); // Voltage at the analog pin
|
|
|
|
if (voltage < 0.75) { // Igniter is connected
|
|
return true;
|
|
} else {
|
|
return false;
|
|
}
|
|
}
|
|
|
|
|
|
/*--------------------------------------------------------------------------------------------------------------------*/
|
|
/*--------------------------------------------------SERVO-------------------------------------------------------------*/
|
|
/*--------------------------------------------------------------------------------------------------------------------*/
|
|
|
|
// Servo stuff
|
|
#ifndef Servo_h
|
|
#include <Servo.h>
|
|
#endif
|
|
#define SUPPLY_FILL_SERVO_PIN 11 // Use the unused connectors
|
|
Servo supply_fill_servo; // 90 is closed, 0 is open
|
|
bool supply_fill_servo_state = false; // False for closed, true for open
|
|
|
|
#if (ENABLE_BVAS_SERVO == true)
|
|
// Below is everything for the servo main oxidizer valve
|
|
#define BVAS_SERVO_PIN 9 // Use the unused connectors
|
|
#define BVAS_SERVO_FULL_CLOSED 0 // Angle to write when full closed
|
|
#define BVAS_SERVO_FULL_OPEN 90 // Angle to write when full open. Currently 120 because of 48:32 gear reduction
|
|
|
|
Servo bvas_servo;
|
|
int bvas_servo_angle; // 0 is closed, 90 is full open
|
|
bool bvas_servo_desired_state = false; // False for closed, true for open any angle
|
|
bool bvas_servo_actual_state = false; // False for closed, true for open any angle
|
|
|
|
const unsigned int amplitude = 20; // 20 degree amplitude
|
|
const double angular_frequency = 2*PI / 3000.0; // Angular frequency such that period = 3000ms
|
|
const unsigned int vertical_offset = 35; // Vertical offset to sin function
|
|
const unsigned int phase_shift = PI; // How much to shift the sin function
|
|
|
|
unsigned int calc_bvas_servo_angle(unsigned long time_since_mdot) {
|
|
return amplitude * cos(angular_frequency * time_since_mdot - phase_shift) + vertical_offset;
|
|
}
|
|
|
|
void bvas_servo_set_desired_state(unsigned char new_desired_state) {
|
|
if (new_desired_state == 0) { // Should be closed
|
|
bvas_servo_desired_state = false;
|
|
} else if (new_desired_state == 3) { // Keep last desired state
|
|
bvas_servo_desired_state = bvas_servo_desired_state;
|
|
} else if (new_desired_state == 1) { // Open
|
|
bvas_servo_desired_state = true;
|
|
}
|
|
}
|
|
|
|
void control_bvas_servo() {
|
|
static unsigned long mdot_time;
|
|
if (bvas_servo_desired_state == false) {
|
|
bvas_servo.write(BVAS_SERVO_FULL_CLOSED); // Close
|
|
bvas_servo_actual_state = false;
|
|
} else if (bvas_servo_desired_state == true && bvas_servo_actual_state == false) { // This is the first time we are seeing the "open" command which means begin mdot
|
|
mdot_time = millis();
|
|
bvas_servo_actual_state = true;
|
|
|
|
unsigned int desired_degrees_open = calc_bvas_servo_angle(0);
|
|
bvas_servo_angle = map(desired_degrees_open, 0, 90, BVAS_SERVO_FULL_CLOSED, BVAS_SERVO_FULL_OPEN);
|
|
bvas_servo.write(bvas_servo_angle);
|
|
} else if (bvas_servo_desired_state == true && bvas_servo_actual_state == true) { // Already begun mdot, let's continue mdotting
|
|
unsigned long time_since_mdot = millis() - mdot_time;
|
|
unsigned int desired_degrees_open = calc_bvas_servo_angle(time_since_mdot);
|
|
bvas_servo_angle = map(desired_degrees_open, 0, 90, BVAS_SERVO_FULL_CLOSED, BVAS_SERVO_FULL_OPEN);
|
|
bvas_servo.write(bvas_servo_angle);
|
|
bvas_servo_actual_state = true;
|
|
}
|
|
}
|
|
|
|
// #define NUM_BVAS_SERVO_ANGLE_VALUES 5 // Tells how many angle values we will provide for the bvas servo
|
|
// /*2D array with the 1st row being the times after mdot in milliseconds
|
|
// and the 2nd row being the desired angle openings of the servo
|
|
// ***please note, this is using 0 as full closed and 90 as full open. Do not do any conversion here if the
|
|
// servo is oriented opposite. That conversion is taken care of if you set the above definitions correctly. Thanks. - Judson*/
|
|
// const unsigned int bvas_servo_times_with_desired_angles[2][NUM_BVAS_SERVO_ANGLE_VALUES] = {
|
|
// /*Times in (ms)*/ {0, 4000, 7000, 8000, 9000,},
|
|
// /*Angle in degrees*/ {0, 10, 90, 70, 90,}
|
|
// };
|
|
|
|
// void bvas_servo_set_desired_state(unsigned char new_desired_state) {
|
|
// if (new_desired_state == 0) { // Should be closed
|
|
// bvas_servo_desired_state = false;
|
|
// } else if (new_desired_state == 3) { // Keep last desired state
|
|
// bvas_servo_desired_state = bvas_servo_desired_state;
|
|
// } else if (new_desired_state == 1) { // Open
|
|
// bvas_servo_desired_state = true;
|
|
// }
|
|
// }
|
|
|
|
// void control_bvas_servo() {
|
|
// static unsigned long mdot_time;
|
|
// if (bvas_servo_desired_state == false) {
|
|
// bvas_servo.write(BVAS_SERVO_FULL_CLOSED); // Close
|
|
// bvas_servo_actual_state = false;
|
|
// } else if (bvas_servo_desired_state == true && bvas_servo_actual_state == false) { // This is the first time we are seeing the "open" command which means begin mdot
|
|
// mdot_time = millis();
|
|
// bvas_servo_actual_state = true;
|
|
// bvas_servo_angle = map(bvas_servo_times_with_desired_angles[1][0], 0, 90, BVAS_SERVO_FULL_CLOSED, BVAS_SERVO_FULL_OPEN);
|
|
// //bvas_servo_angle = bvas_servo_times_with_desired_angles[1][0];
|
|
// bvas_servo.write(bvas_servo_angle);
|
|
// } else if (bvas_servo_desired_state == true && bvas_servo_actual_state == true) { // Already begun mdot, let's continue mdotting
|
|
// unsigned int time_index_to_use = 0;
|
|
// for (unsigned int i = 0; i < NUM_BVAS_SERVO_ANGLE_VALUES; i++) {
|
|
// unsigned long time_since_mdot = millis() - mdot_time;
|
|
// if (time_since_mdot >= bvas_servo_times_with_desired_angles[0][i]) { // We have at least gotten to this time range
|
|
// time_index_to_use = i;
|
|
// } else { // We have not gotten to this new time value so continue using the last index
|
|
// break;
|
|
// }
|
|
// }
|
|
// bvas_servo_angle = map(bvas_servo_times_with_desired_angles[1][time_index_to_use], 0, 90, BVAS_SERVO_FULL_CLOSED, BVAS_SERVO_FULL_OPEN);
|
|
// //bvas_servo_angle = bvas_servo_times_with_desired_angles[1][time_index_to_use];
|
|
// bvas_servo.write(bvas_servo_angle);
|
|
// bvas_servo_actual_state = true;
|
|
// }
|
|
// }
|
|
#endif
|
|
|
|
|
|
|
|
/*--------------------------------------------------------------------------------------------------------------------*/
|
|
/*------------------------------------------------CAN BUS-------------------------------------------------------------*/
|
|
/*--------------------------------------------------------------------------------------------------------------------*/
|
|
|
|
// CAN Stuff
|
|
#ifndef SPI_h
|
|
#include <SPI.h>
|
|
#endif
|
|
|
|
#ifndef mcp2515_h
|
|
#include <mcp2515.h>
|
|
#endif
|
|
|
|
//#include "CANComms.h"
|
|
|
|
#define CAN_CS 53
|
|
MCP2515 mcp2515(CAN_CS); // CAN CS pin is 53
|
|
struct can_frame received_msg; // The struct we will constantly be using to receive from the CB
|
|
struct can_frame acknowledge_msg; // The struct we will constantly be updating so we can send it as acknowledgement to the CB
|
|
bool comms_with_cb = false;
|
|
unsigned long last_cb_message_time;
|
|
#define TIME_BETWEEN_MESSAGES 100 // The ms between CAN reads/writes
|
|
|
|
#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 // 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;
|
|
unsigned long time_for_supply_fill_to_close = 1500; // Give time for the supply fill to close
|
|
|
|
bool initialize_can_with_CB() {
|
|
// Used to initialize CAN bus wth the CB. Only returns true once it has succesfully received, acknowledged, and received an acknowledgment from CB
|
|
// Returning false can't really happen because it loops until 2-way communication is established
|
|
|
|
unsigned long last_read_time = millis();
|
|
|
|
while (true) {
|
|
if (millis() - last_read_time > TIME_BETWEEN_MESSAGES) {
|
|
last_read_time = millis();
|
|
if (mcp2515.readMessage(&received_msg) == MCP2515::ERROR_OK) { // We have an incoming message
|
|
//Serial.println("CAN message received");
|
|
if (received_msg.can_id == 5) { // CB is trying to ping the AB
|
|
unsigned long received_ping_time = millis();
|
|
if (received_msg.data[0] == 1) { // CB is doing its first ping to the AB
|
|
//Serial.println("Received ping from CB");
|
|
last_cb_message_time = millis();
|
|
acknowledge_msg.can_id = 5;
|
|
acknowledge_msg.can_dlc = 8;
|
|
acknowledge_msg.data[0] = 2; acknowledge_msg.data[1] = 2; acknowledge_msg.data[2] = 2; acknowledge_msg.data[3] = 2; acknowledge_msg.data[4] = 2; acknowledge_msg.data[5] = 2; acknowledge_msg.data[6] = 2; acknowledge_msg.data[7] = 2;
|
|
delay(100);
|
|
//Serial.println("Sending acknowledgement");
|
|
mcp2515.sendMessage(&acknowledge_msg); // Send acknowledgement to CB
|
|
while (millis() - received_ping_time < 1000) { // Now we wait for the message back from the CB to confirm 2-way communication
|
|
if (millis() - last_read_time > TIME_BETWEEN_MESSAGES) {
|
|
last_read_time = millis();
|
|
if (mcp2515.readMessage(&received_msg) == MCP2515::ERROR_OK) { // We have an incoming message
|
|
//Serial.println("Received another CAN message");
|
|
if (received_msg.can_id == 5 && received_msg.data[0] == 3) { // CB is acknowledging the acknowledgement from AB
|
|
//Serial.println("2-Way CAN established");
|
|
last_cb_message_time = millis();
|
|
comms_with_cb = true;
|
|
return true;
|
|
} else {
|
|
break;
|
|
}
|
|
} else {
|
|
//Serial.println("No second CAN message received");
|
|
}
|
|
}
|
|
|
|
}
|
|
}
|
|
}
|
|
} else {
|
|
//Serial.println("No CAN message received");
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
bool reinitialize_can_with_CB() {
|
|
// Used to initialize CAN bus wth the CB. Only returns true once it has succesfully received, acknowledged, and received an acknowledgment from CB
|
|
// Returning false can't really happen because it loops until 2-way communication is established
|
|
|
|
static unsigned long last_read_time = millis();
|
|
|
|
if (millis() - last_read_time > TIME_BETWEEN_MESSAGES) {
|
|
last_read_time = millis();
|
|
if (mcp2515.readMessage(&received_msg) == MCP2515::ERROR_OK) { // We have an incoming message
|
|
Serial.println("CAN message received");
|
|
if (received_msg.can_id == 5) { // CB is trying to ping the AB
|
|
unsigned long received_ping_time = millis();
|
|
if (received_msg.data[0] == 1) { // CB is doing its first ping to the AB
|
|
Serial.println("Received ping from CB");
|
|
last_cb_message_time = millis();
|
|
acknowledge_msg.can_id = 5;
|
|
acknowledge_msg.can_dlc = 8;
|
|
acknowledge_msg.data[0] = 2; acknowledge_msg.data[1] = 2; acknowledge_msg.data[2] = 2; acknowledge_msg.data[3] = 2; acknowledge_msg.data[4] = 2; acknowledge_msg.data[5] = 2; acknowledge_msg.data[6] = 2; acknowledge_msg.data[7] = 2;
|
|
delay(100);
|
|
Serial.println("Sending acknowledgement");
|
|
mcp2515.sendMessage(&acknowledge_msg); // Send acknowledgement to CB
|
|
while (millis() - received_ping_time < 1000) { // Now we wait for the message back from the CB to confirm 2-way communication
|
|
if (millis() - last_read_time > TIME_BETWEEN_MESSAGES) {
|
|
last_read_time = millis();
|
|
if (mcp2515.readMessage(&received_msg) == MCP2515::ERROR_OK) { // We have an incoming message
|
|
Serial.println("Received another CAN message");
|
|
if (received_msg.can_id == 5 && received_msg.data[0] == 3) { // CB is acknowledging the acknowledgement from AB
|
|
Serial.println("2-Way CAN established");
|
|
last_cb_message_time = millis();
|
|
comms_with_cb = true;
|
|
return true;
|
|
}
|
|
} else {
|
|
Serial.println("No CAN message received");
|
|
}
|
|
}
|
|
|
|
}
|
|
}
|
|
}
|
|
} else {
|
|
Serial.println("No CAN message received");
|
|
}
|
|
}
|
|
return false;
|
|
}
|
|
|
|
bool need_to_enter_pause_state() {
|
|
// Returns true if enough time has passed that we believe we have lost comms with the CB and need to pause
|
|
if (millis() - last_cb_message_time > TIME_TO_INIT_PAUSE_STATE) {
|
|
return true;
|
|
} else {
|
|
return false;
|
|
}
|
|
}
|
|
|
|
bool need_to_enter_abort_state() {
|
|
// Returns true if enough time has passed that we believe we have lost comms with the CB and need to abort
|
|
if (millis() - last_cb_message_time > TIME_TO_INIT_ABORT_STATE) {
|
|
return true;
|
|
} else {
|
|
return false;
|
|
}
|
|
}
|
|
|
|
void lost_comms_procedural_loop() {
|
|
// Calling this function immediately tells us we need to pause the system
|
|
// We need 2 different pauses:
|
|
// 1. Before we open the BVAS for mdot: igniter off, bvas closed, close supply fill, close run vent, close supply vent
|
|
// 2. During / after mdot: igniter off, close supply fill, close run vent, close supply vent, leave bvas open
|
|
|
|
Serial.println("Entered pause state");
|
|
igniter_relay.set_desired_state(false); // Make sure the igniter is off
|
|
supply_fill_servo.write(90); // Close the supply fill valve
|
|
supply_fill_servo_state = false;
|
|
supply_vent_relay.set_desired_state(false); // Close the supply vent
|
|
run_vent_solenoid.set_desired_solenoid_state(false); // Close the run vent
|
|
// BVAS is kept in the same position
|
|
|
|
// The above is putting us in the "pause" state
|
|
|
|
while (comms_with_cb == false) { // While no comms, this is the "pause state"
|
|
if (reinitialize_can_with_CB() == false) { // Comms not regained
|
|
if (need_to_enter_abort_state()) { // It has been enough time to where we need to abort due to not getting comms back
|
|
Serial.println("Entered abort state");
|
|
abort_state_entered_time = millis();
|
|
|
|
while (1) { // We need to abort. No need to do anything else anymore
|
|
run_vent_solenoid.set_desired_solenoid_state(true); // Open the run vent
|
|
supply_vent_relay.set_desired_state(true); // Open the supply vent
|
|
// Dylan Mahoney said to leave BVAS as is
|
|
while (1) {
|
|
control_relays_and_servos();
|
|
}
|
|
}
|
|
}
|
|
// Message is available over ethernet (probably the daq requesting for data)
|
|
#if (ENABLE_ETHERNET_COMMS == true)
|
|
if (UDPWrapper.is_message_available()) {
|
|
char* incoming_msg = UDPWrapper.get_incoming_packet();
|
|
|
|
if (strcmp("daq_request", incoming_msg) == 0) {
|
|
char* outgoing_msg = format_daq_response();
|
|
|
|
UDPWrapper.send_response(outgoing_msg, 9);
|
|
delete outgoing_msg;
|
|
}
|
|
}
|
|
#endif
|
|
|
|
} else { // We did get our comms back
|
|
comms_with_cb = true;
|
|
//Serial.println("Re initialized CAN with CB");
|
|
return; // We got comms back, are in the "pause" state, and can go back to being controlled by the control box
|
|
}
|
|
control_relays_and_servos();
|
|
}
|
|
}
|
|
|
|
/*--------------------------------------------------------------------------------------------------------------------*/
|
|
/*------------------------------------------------SETUP AND LOOP------------------------------------------------------*/
|
|
/*--------------------------------------------------------------------------------------------------------------------*/
|
|
|
|
void setup() {
|
|
#if (ENABLE_ETHERNET_COMMS == false)
|
|
pinMode(25, OUTPUT); // Ethernet SPI module CS pin
|
|
digitalWrite(25, HIGH); // Disable Ethernet SPI module
|
|
#endif
|
|
|
|
Serial.begin(115200);
|
|
|
|
supply_vent_relay.set_desired_state(0);
|
|
run_vent_solenoid.set_desired_solenoid_state(0);
|
|
igniter_relay.set_desired_state(0);
|
|
|
|
#if (ENABLE_BVAS_SERVO == false)
|
|
bvas_solenoid.set_desired_solenoid_state(0);
|
|
#else
|
|
bvas_servo.attach(BVAS_SERVO_PIN);
|
|
bvas_servo.write(BVAS_SERVO_FULL_CLOSED); // Close the valve
|
|
#endif
|
|
|
|
supply_fill_servo.attach(SUPPLY_FILL_SERVO_PIN);
|
|
supply_fill_servo.write(90); // Close the valve
|
|
unsigned long start_time = millis();
|
|
while (millis() - start_time < 2500) { // Give time to make sure relays are off and whatnot
|
|
control_relays_and_servos();
|
|
}
|
|
|
|
//Ethernet stuff
|
|
#if (ENABLE_ETHERNET_COMMS == true)
|
|
if (UDPWrapper.begin() == false) {
|
|
//Serial.println("Failed to start UDPWrapper");
|
|
}
|
|
#endif
|
|
|
|
mcp2515.reset();
|
|
mcp2515.setBitrate(CAN_125KBPS);
|
|
mcp2515.setNormalMode();
|
|
|
|
//Serial.println("Intitializing CAN with CB");
|
|
initialize_can_with_CB();
|
|
// CAN is now established with the Control Box
|
|
//Serial.println("CAN Initialized with CB");
|
|
}
|
|
|
|
/*--------------------------------------------------------------------------------------------------------------------*/
|
|
/*------------------------------------------------LOOP FUNCTIONS------------------------------------------------------*/
|
|
/*--------------------------------------------------------------------------------------------------------------------*/
|
|
void set_desired_relay_states() {
|
|
// Uses the received CAN message to update the desired states of the valves/relays
|
|
|
|
// Supply Fill is conveniently absent because it is controlled in the control_relays_and_servos() loop
|
|
supply_vent_relay.set_desired_state(received_msg.data[1]); // 0 for closed, 1 for open, 3 for keep last desired state
|
|
run_vent_solenoid.set_desired_solenoid_state(received_msg.data[2]); // 0 for closed, 1 for open, 3 for keep last desired state
|
|
igniter_relay.set_desired_state(received_msg.data[4]); // 0 for closed, 1 for open, 3 for keep last desired state
|
|
|
|
#if (ENABLE_BVAS_SERVO == false)
|
|
#if (ENABLE_BVAS_DELAY == true)
|
|
static bool first_mdot_command_received = false;
|
|
static unsigned long first_mdot_time;
|
|
if (received_msg.data[3] == 1U) { // MDOT command is being sent
|
|
if (first_mdot_command_received == false) {
|
|
first_mdot_time = millis();
|
|
first_mdot_command_received = true;
|
|
}
|
|
if (millis() - first_mdot_time >= BVAS_DELAY) {
|
|
bvas_solenoid.set_desired_solenoid_state(1U); // 0 for closed, 1 for open, 3 for keep last desired state
|
|
} else {
|
|
bvas_solenoid.set_desired_solenoid_state(0U); // 0 for closed, 1 for open, 3 for keep last desired state
|
|
}
|
|
} else if (received_msg.data[3] == 0U) { // We definitely want it closed
|
|
bvas_solenoid.set_desired_solenoid_state(0U); // 0 for closed, 1 for open, 3 for keep last desired state
|
|
first_mdot_command_received = false;
|
|
}
|
|
#else
|
|
bvas_solenoid.set_desired_solenoid_state(received_msg.data[3]); // 0 for closed, 1 for open, 3 for keep last desired state
|
|
#endif
|
|
#else
|
|
#if (ENABLE_BVAS_DELAY == true)
|
|
bvas_servo_set_desired_state(received_msg.data[3]); // 0 for closed, 1 for continue mdot process, 3 for keep last desired state
|
|
#else
|
|
bvas_servo_set_desired_state(received_msg.data[3]); // 0 for closed, 1 for continue mdot process, 3 for keep last desired state
|
|
#endif
|
|
#endif
|
|
|
|
}
|
|
|
|
void control_relays_and_servos() {
|
|
// Controls relays and servos
|
|
// Automatically checks for pin errors, relay voltages/errors, and timing events
|
|
|
|
// Supply Fill
|
|
if (received_msg.data[0] == 0) { // Supply fill valve should be closed
|
|
supply_fill_servo.write(90); // Close the valve
|
|
supply_fill_servo_state = false;
|
|
} else if (received_msg.data[0] == 1) { // Supply fill valve should be open
|
|
supply_fill_servo.write(0); // Open the valve
|
|
supply_fill_servo_state = true;
|
|
} else if (received_msg.data[0] == 3) { // The servo needs to keep whatever its last state was
|
|
if (supply_fill_servo_state == true) {
|
|
supply_fill_servo.write(0); // Open the valve
|
|
supply_fill_servo_state = true;
|
|
} else {
|
|
supply_fill_servo.write(90); // Close the valve
|
|
supply_fill_servo_state = true;
|
|
}
|
|
}
|
|
|
|
supply_vent_relay.control_relay();
|
|
run_vent_solenoid.control_solenoid();
|
|
igniter_relay.control_relay();
|
|
|
|
#if (ENABLE_BVAS_SERVO == false)
|
|
bvas_solenoid.control_solenoid();
|
|
#else
|
|
control_bvas_servo();
|
|
#endif
|
|
|
|
|
|
}
|
|
|
|
void format_can_response() {
|
|
// Format acknowledgement CAN message to show errors
|
|
// Reset message bytes
|
|
acknowledge_msg.can_id = 50;
|
|
acknowledge_msg.can_dlc = 8;
|
|
acknowledge_msg.data[0] = 0;
|
|
acknowledge_msg.data[1] = 0;
|
|
acknowledge_msg.data[2] = 0;
|
|
acknowledge_msg.data[3] = 0;
|
|
acknowledge_msg.data[4] = 0;
|
|
acknowledge_msg.data[5] = 0;
|
|
acknowledge_msg.data[6] = 0;
|
|
acknowledge_msg.data[7] = 0;
|
|
|
|
// Supply Fill. 0 = closed, 1 = open
|
|
if (supply_fill_servo_state == false) { // Supposed to be closed
|
|
acknowledge_msg.data[0] = 0;
|
|
} else { // Supposed to be open
|
|
acknowledge_msg.data[0] = 11;
|
|
}
|
|
|
|
// Supply Vent. One's place for voltage. Ten's place for what it's telling relay to do
|
|
if (supply_vent_relay.get_desired_state() == false) { // Supposed to be closed
|
|
acknowledge_msg.data[1] += 0;
|
|
} else { // Supposed to be open
|
|
acknowledge_msg.data[1] += 10;
|
|
}
|
|
if (supply_vent_relay.get_setpin_working() == false) { // Setpin not working
|
|
acknowledge_msg.data[1] += 30;
|
|
}
|
|
if (supply_vent_relay.get_resetpin_working() == false) { // Resetpin not working
|
|
acknowledge_msg.data[1] += 50;
|
|
}
|
|
if (supply_vent_relay.get_actual_state() == false) { // There is NOT a voltage across the relay
|
|
acknowledge_msg.data[1] += 0;
|
|
} else { // There is a voltage across the relay
|
|
acknowledge_msg.data[1] += 1;
|
|
}
|
|
if (supply_vent_relay.get_relay_working() == false) { // Voltage disagrees with desired state
|
|
acknowledge_msg.data[1] += 3;
|
|
} else {
|
|
acknowledge_msg.data[1] += 0; // Voltage agrees with desired state
|
|
}
|
|
|
|
// Run Tank Vent OFF
|
|
if (run_vent_solenoid.get_desired_state() == false) { // Solenoid is desired to be off
|
|
acknowledge_msg.data[2] += 0;
|
|
} else { // Relay is supposed to be on
|
|
acknowledge_msg.data[2] += 10;
|
|
}
|
|
if (run_vent_solenoid.get_offpin_working() == false) { // The pin is not working
|
|
acknowledge_msg.data[2] += 30;
|
|
}
|
|
if (run_vent_solenoid.get_expected_state() == false) { // The solenoid is thought to be off
|
|
acknowledge_msg.data[2] += 0;
|
|
} else { // The solenoid is thought to be on
|
|
acknowledge_msg.data[2] += 1;
|
|
}
|
|
if (run_vent_solenoid.get_expected_state() != run_vent_solenoid.get_desired_state()) {
|
|
acknowledge_msg.data[2] += 3;
|
|
}
|
|
|
|
// Run Tank Vent ON
|
|
if (run_vent_solenoid.get_desired_state() == false) { // Solenoid is desired to be off
|
|
acknowledge_msg.data[3] += 0;
|
|
} else { // Relay is supposed to be on
|
|
acknowledge_msg.data[3] += 10;
|
|
}
|
|
if (run_vent_solenoid.get_onpin_working() == false) { // The pin is not working
|
|
acknowledge_msg.data[3] += 30;
|
|
}
|
|
if (run_vent_solenoid.get_expected_state() == false) { // The solenoid is thought to be off
|
|
acknowledge_msg.data[3] += 0;
|
|
} else { // The solenoid is thought to be on
|
|
acknowledge_msg.data[3] += 1;
|
|
}
|
|
if (run_vent_solenoid.get_expected_state() != run_vent_solenoid.get_desired_state()) {
|
|
acknowledge_msg.data[3] += 3;
|
|
}
|
|
|
|
|
|
#if (ENABLE_BVAS_SERVO == false)
|
|
// BVAS OFF
|
|
if (bvas_solenoid.get_desired_state() == false) { // Solenoid is desired to be off
|
|
acknowledge_msg.data[4] += 0;
|
|
} else { // Relay is supposed to be on
|
|
acknowledge_msg.data[4] += 10;
|
|
}
|
|
if (bvas_solenoid.get_offpin_working() == false) { // The pin is not working
|
|
acknowledge_msg.data[4] += 30;
|
|
}
|
|
if (bvas_solenoid.get_expected_state() == false) { // The solenoid is thought to be off
|
|
acknowledge_msg.data[4] += 0;
|
|
} else { // The solenoid is thought to be on
|
|
acknowledge_msg.data[4] += 1;
|
|
}
|
|
if (bvas_solenoid.get_expected_state() != bvas_solenoid.get_desired_state()) {
|
|
acknowledge_msg.data[4] += 3;
|
|
}
|
|
|
|
// BVAS ON
|
|
if (bvas_solenoid.get_desired_state() == false) { // Solenoid is desired to be off
|
|
acknowledge_msg.data[5] += 0;
|
|
} else { // Relay is supposed to be on
|
|
acknowledge_msg.data[5] += 10;
|
|
}
|
|
if (bvas_solenoid.get_onpin_working() == false) { // The pin is not working
|
|
acknowledge_msg.data[5] += 30;
|
|
}
|
|
if (bvas_solenoid.get_expected_state() == false) { // The solenoid is thought to be off
|
|
acknowledge_msg.data[5] += 0;
|
|
} else { // The solenoid is thought to be on
|
|
acknowledge_msg.data[5] += 1;
|
|
}
|
|
if (bvas_solenoid.get_expected_state() != bvas_solenoid.get_desired_state()) {
|
|
acknowledge_msg.data[5] += 3;
|
|
}
|
|
#else // Using the bvas servo
|
|
if (bvas_servo_actual_state == false) { // Should be closed
|
|
acknowledge_msg.data[4] = 0;
|
|
acknowledge_msg.data[5] = 0;
|
|
} else {
|
|
acknowledge_msg.data[4] = 11;
|
|
acknowledge_msg.data[5] = 11;
|
|
}
|
|
#endif
|
|
|
|
// Igniter
|
|
if (igniter_relay.get_desired_state() == false) { // Supposed to be off
|
|
acknowledge_msg.data[6] += 0;
|
|
} else { // Supposed to be open
|
|
acknowledge_msg.data[6] += 10;
|
|
}
|
|
if (igniter_relay.get_setpin_working() == false) { // Setpin not working
|
|
acknowledge_msg.data[6] += 30;
|
|
}
|
|
if (igniter_relay.get_resetpin_working() == false) { // Resetpin not working
|
|
acknowledge_msg.data[6] += 50;
|
|
}
|
|
if (igniter_relay.get_actual_state() == false) { // There is NOT a voltage across the relay
|
|
acknowledge_msg.data[6] += 0;
|
|
} else { // There is a voltage across the relay
|
|
acknowledge_msg.data[6] += 1;
|
|
}
|
|
if (igniter_relay.get_relay_working() == false) { // Voltage disagrees with desired state
|
|
acknowledge_msg.data[6] += 3;
|
|
} else {
|
|
acknowledge_msg.data[6] += 0; // Voltage agrees with desired state
|
|
}
|
|
if (igniter_continuity_check(IGNITER_CONTINUITY_CHECK) == false) { // No continuity detected
|
|
acknowledge_msg.data[6] += 0;
|
|
} else { // We have igniter continuity
|
|
acknowledge_msg.data[6] += 100;
|
|
}
|
|
}
|
|
|
|
// Format the UDP message to be sent to the daq when it wants it
|
|
char* format_daq_response() {
|
|
char *outgoing_daq_msg = new char(9);
|
|
outgoing_daq_msg[0] = '0';
|
|
outgoing_daq_msg[1] = '0';
|
|
outgoing_daq_msg[2] = '0';
|
|
outgoing_daq_msg[3] = '0';
|
|
outgoing_daq_msg[4] = '0';
|
|
outgoing_daq_msg[5] = '0';
|
|
outgoing_daq_msg[6] = '0';
|
|
outgoing_daq_msg[7] = '0';
|
|
outgoing_daq_msg[8] = '0';
|
|
outgoing_daq_msg[9] = '0';
|
|
|
|
|
|
// Supply Fill. 0 = closed, 1 = open
|
|
if (supply_fill_servo_state == false) { // Supposed to be closed. '00' is closed
|
|
outgoing_daq_msg[0] = '0';
|
|
outgoing_daq_msg[1] = '0';
|
|
} else { // Supposed to be open. '90' is open
|
|
outgoing_daq_msg[0] = '9';
|
|
outgoing_daq_msg[1] = '0';
|
|
}
|
|
|
|
if (supply_vent_relay.get_actual_state() == false) { // There is NOT a voltage across the relay
|
|
outgoing_daq_msg[2] = '0';
|
|
outgoing_daq_msg[3] = '0';
|
|
} else { // There is a voltage across the relay
|
|
outgoing_daq_msg[2] = '9';
|
|
outgoing_daq_msg[3] = '0';
|
|
}
|
|
|
|
if (run_vent_solenoid.get_expected_state() == false) {
|
|
outgoing_daq_msg[4] = '0';
|
|
outgoing_daq_msg[5] = '0';
|
|
} else { // There is a voltage across the relay
|
|
outgoing_daq_msg[4] = '9';
|
|
outgoing_daq_msg[5] = '0';
|
|
}
|
|
|
|
#if (ENABLE_BVAS_SERVO == false)
|
|
if (bvas_solenoid.get_expected_state() == false) { // The solenoid is thought to be off
|
|
outgoing_daq_msg[6] = '0';
|
|
outgoing_daq_msg[7] = '0';
|
|
} else { // The solenoid is thought to be on
|
|
outgoing_daq_msg[6] = '9';
|
|
outgoing_daq_msg[7] = '0';
|
|
}
|
|
#else // Using the bvas servo
|
|
if (bvas_servo_actual_state == false) { // Should be closed
|
|
outgoing_daq_msg[6] = '0';
|
|
outgoing_daq_msg[7] = '0';
|
|
} else {
|
|
itoa(bvas_servo_angle, outgoing_daq_msg+6, 10);
|
|
}
|
|
#endif
|
|
|
|
if (igniter_relay.get_actual_state() == false) { // There is NOT a voltage across the relay
|
|
outgoing_daq_msg[8] = '0';
|
|
} else { // There is a voltage across the relay
|
|
outgoing_daq_msg[8] = '1';
|
|
}
|
|
|
|
return outgoing_daq_msg;
|
|
}
|
|
|
|
void manual_abort_loop() {
|
|
// This loops when the CB told the AB that it is manually aborting
|
|
// Primary concern is if the CB sends a manual abort command then loses comms, we want to prevent the AB from going into a "pause state" and ruining the abort sequence
|
|
unsigned int last_read_time = millis();
|
|
while(1) {
|
|
if (millis() - last_read_time > TIME_BETWEEN_MESSAGES) {
|
|
if (mcp2515.readMessage(&received_msg) == MCP2515::ERROR_OK) {
|
|
last_cb_message_time = millis();
|
|
set_desired_relay_states();
|
|
control_relays_and_servos();
|
|
|
|
format_can_response();
|
|
mcp2515.sendMessage(&acknowledge_msg);
|
|
if (received_msg.can_id != 5) { // No longer in manual abort, only BREAK condition
|
|
return;
|
|
}
|
|
}
|
|
}
|
|
// Still trying to update the relays every loop
|
|
control_relays_and_servos();
|
|
|
|
if (need_to_enter_pause_state()) {
|
|
comms_with_cb = false; // We lost comms with the control box
|
|
reinitialize_can_with_CB();
|
|
}
|
|
}
|
|
}
|
|
|
|
void loop() {
|
|
/*--------------------------------------------------------------------------------------------------------------------*/
|
|
|
|
// Check for possible pause state scenarios and abort state scenarios due to lost comms
|
|
if (need_to_enter_pause_state()) {
|
|
//Serial.println("Lost Comms, entering PAUSE state");
|
|
comms_with_cb = false; // We lost comms with the control box
|
|
pause_state_entered_time = millis();
|
|
lost_comms_procedural_loop(); // Loops until either comms are regained or we abort
|
|
}
|
|
|
|
/*--------------------------------------------------------------------------------------------------------------------*/
|
|
|
|
// Read CAN message, record time
|
|
|
|
if (mcp2515.readMessage(&received_msg) == MCP2515::ERROR_OK) { // We have an incoming message
|
|
if (received_msg.can_id == 5 && received_msg.data[5] == 9) { // CB is sending ABORT command message to AB
|
|
last_cb_message_time = millis();
|
|
set_desired_relay_states();
|
|
manual_abort_loop();
|
|
} else if (received_msg.can_id == 50) { // CB is sending normal command message to AB
|
|
// Send CAN message to Control Box
|
|
last_cb_message_time = millis();
|
|
set_desired_relay_states();
|
|
control_relays_and_servos();
|
|
format_can_response();
|
|
mcp2515.sendMessage(&acknowledge_msg);
|
|
}
|
|
}
|
|
|
|
// Message is available over ethernet
|
|
#if (ENABLE_ETHERNET_COMMS == true)
|
|
if (UDPWrapper.is_message_available()) {
|
|
char* incoming_msg = UDPWrapper.get_incoming_packet();
|
|
|
|
if (strcmp("daq_request", incoming_msg) == 0) {
|
|
char* outgoing_msg = format_daq_response();
|
|
|
|
UDPWrapper.send_response(outgoing_msg, 9);
|
|
delete outgoing_msg;
|
|
}
|
|
}
|
|
#endif
|
|
control_relays_and_servos();
|
|
}
|