main.cpp

/**
 * Paradigm Hyperloop Prototype Node Control Firmware
 *
 * ABOUT:
 * This file defines the controls logic for a single Brake Node.  A Brake Node
 * is special from a generic node in that it is directly connected to all
 * the required sensors and actuators to adequately control the brake Module
 * in the event of an emergency, a crucial element to building a fail-safe
 * vehicle.
 *
 * HARDWARE:
 * This module is designed to control Pod 2 Brakes, with a few changes. The
 * following hardware is expected to be connected to this node:
 *   Actuators
 *    - 1 5/3 Solenoid (2 coils, 3 states - Engage, Closed, Release)
 *   Sensors
 *    - 2 Analog 5V press transducers (Honeywell 150PSI) 1 Tank, 1 Piston
 *    - 2 Analog 5V Sharp Distance Sensors (2-10cm)
 *    - 3 0-5V Cold Comp Thermocouple Amplifiers
 *
 * COMMUNICATIONS:
 * This module assumes an Arduino Compatible Ethernet IC is connected.
 * This module acquires its MAC and IP at compile time.
 *   Defaults:
 *    - MAC_ADDRESS: 0xDE, 0xAD, 0xBE, 0xEF, 0xFE, 0xED
 *    - IP_ADDRESS: 192, 168, 1, 177
 *   Note:
 *    Static Assignment of MAC and IP address should be investigated, there are
 *    Possibly better alternatives.
 *    - Determine if a MAC can be burned into the NIC once during assembly
 *        -> Then we could avoid compile time and use DHCP.
 *    - Determine if a default MAC is already burned into the NIC
 *    - Determine if we can generate a unique MAC based on the uC serial #
 *
 * DESIGN:
 * See the infamous KeynoteCAD for design notes on the design of this Prototype
 * controller.
 */

/////////////////////// HEADERS /////////////////////
#include <Arduino.h>
#include <Ethernet.h>
#include <EthernetUdp.h> // UDP library from: bjoern@cs.stanford.edu 12/30/2008
#include <SPI.h>         // needed for Arduino versions later than 0018

#define __ASSERT_USE_STDERR
#include <assert.h>

#include <Adafruit_ASFcore.h>
#include <Adafruit_SleepyDog.h>

/////////////////////// CONFIG /////////////////////

#ifndef MAC_ADDRESS
#define MAC_ADDRESS 0xDE, 0xAD, 0xBE, 0xEF, 0xFE, 0xED
#endif

#ifndef IP_ADDRESS
#define IP_ADDRESS 192, 168, 1, 177
#endif

#ifndef LISTEN_PORT
#define LISTEN_PORT 8888 // UDP
#endif

#define MESSAGE_TIMEOUT 1000      // milliseconds
#define PISTON_EMPTY_THRESH 3     // PSI
#define PISTON_AREA 40            // Square Inches
#define BRAKE_FLUTTER_TIME 100    // milliseconds
#define FORCE_SETPOINT_THRESH 100 // Newtons

/////////////////////// CONVERTIONS /////////////////////

#define POUND_FORCE_TO_NEWTONS(x) (445 * x) / 100
#define BIAS(old_val, new_val, bias, total)                                    \
  (((((old_val) * (bias) / (total)) +                                          \
     ((new_val) * ((total) - (bias)) / (total)))) /                            \
   (total))

/////////////////////// NETWORK SETUP /////////////////////

// Enter a MAC address and IP address for your controller below.
// The IP address will be dependent on your local network:
byte mac[6] = {MAC_ADDRESS};
IPAddress ip(IP_ADDRESS);

unsigned int localPort = LISTEN_PORT; // local port to listen on

// buffers for receiving and sending data
char packetBuffer[UDP_TX_PACKET_MAX_SIZE]; // buffer to hold incoming packet,
char ReplyBuffer[UDP_TX_PACKET_MAX_SIZE];  // buffer to build outgoing packet,

// An EthernetUDP instance to let us send and receive packets over UDP
EthernetUDP Udp;

/////////////////////// VALVE ABSTRACTIONS /////////////////////

// Generic 2/2 valve state enumeration
typedef enum valve_state {
  kValveFault = 0,   // Valve has faulted, unknown state
  kValveOpen = 1,    // Valve is open
  kValveClosed = 2,  // Valve is closed
  kValveOpening = 3, // Valve is opening, not confirmed opened
  kValveClosing = 4  // Valve is closing, not confirmed closed
} valve_state_t;

typedef struct valve {
  valve_state_t state;
  int pin;
  uint32_t last_update;
} valve_t;

// A Brake valve is a standard 5/3 solenoid
typedef enum brake_valve_state {
  kBrakeValveFault = 0,     // Valve has faulted, unknown state
  kBrakeValveEngaged = 1,   // Valve is engaging air to brake
  kBrakeValveClosed = 2,    // Valve is closed
  kBrakeValveReleased = 3,  // Valve is releasing air from brake
  kBrakeValveEngaging = 4,  // Valve is traveling to engaged state
  kBrakeValveClosing = 5,   // Valve is traveling to closed state
  kBrakeValveReleasing = 6, // Valve is traveling to released state
} brake_valve_state_t;

typedef struct brake_valve {
  brake_valve_state_t state;
  int engage_pin;
  int release_pin;
  uint32_t last_update;
} brake_valve_t;

/////////////////////// SENSOR ABSTRACTIONS /////////////////////

struct sensor;
typedef int (*processor)(struct sensor *sensor);

typedef struct sensor {
  char name[16];
  uint8_t channel;
  int32_t raw;
  int32_t val;
  processor process;
} sensor_t;

/////////////////////// NODE STATE /////////////////////
typedef enum node_mode {
  kModeInit = 0,
  kModeStandard = 1,
  kModeReset = 2,
  kModeProgram = 3,
  kModeManual = 4,
  kModeInhibit = 5,
  kModeEmergency = 6
} node_mode_t;

typedef struct node {
  uint8_t mode;
  sensor_t dist_rear;
  sensor_t dist_front;
  sensor_t temp_rear;
  sensor_t temp_front;
  sensor_t tank_press;
  sensor_t piston_press;
  sensor_t *sensors[16]; // Array of pointers to sensors in channel order
  int32_t last_message;
  int32_t force_setpoint;
  int32_t ref_press;
  brake_valve_t valve;
} node_t;

/////////////////////// NETWORK PACKETS /////////////////////

typedef struct req_packet {
  uint8_t mode;
  int32_t arg0;
  int32_t arg1;
} req_packet_t;

typedef struct resp_packet {
  uint8_t mode;
  int32_t tank_press;
  int32_t piston_press;
  int32_t dist_front;
  int32_t dist_rear;
  int32_t valve_state;
  int32_t temp_front;
  int32_t temp_rear;
  int32_t force_setpoint;
} resp_packet_t;

/////////////////////// GLOBAL STATE STORAGE /////////////////////

node_t BRAKE = {0};

/////////////////////// SENSOR HELPERS /////////////////////

void sensor_init(sensor_t *s, char *name, uint8_t channel, processor p) {
  int i = 0;
  while (name[i] != '\0' && i < 15) {
    s->name[i] = name[i];
    i++;
  }
  s->name[i] = '\0';
  s->channel = channel;
  s->raw = 0;
  s->process = p;
  s->process(s);

  assert(BRAKE.sensors[channel] == NULL);
  BRAKE.sensors[channel] = s;
}

void sensor_destroy(sensor_t *s) {
  assert(BRAKE.sensors[s->channel] == s);
  BRAKE.sensors[s->channel] = NULL;
}

int _dist_sensor(sensor_t *s) {
  uint32_t new_val = map(s->raw, 0, 4095, 220, 10); // Bullshit
  s->val = BIAS(s->val, new_val, 0, 255);
  return 0;
}

int _temp_sensor(sensor_t *s) {
  uint32_t new_val = map(s->raw, 0, 4095, 220, 10); // Bullshit
  s->val = BIAS(s->val, new_val, 0, 255);
  return 0;
}

int _press_sensor(sensor_t *s) {
  uint32_t new_val = map(s->raw, 0, 4095, 220, 10); // Bullshit
  s->val = BIAS(s->val, new_val, 0, 255);
  return 0;
}

/////////////////////// GENERIC UTILITIES /////////////////////

void print_ip(IPAddress ip) {
  for (int i = 0; i < 4; i++) {
    Serial.print(ip[i], DEC);
    if (i < 3) {
      Serial.print(".");
    }
  }
}

/////////////////////// CONTROLLER UTILITIES /////////////////////

int32_t calculate_current_force() {
  int32_t relative_100s_psi = BRAKE.piston_press.val - BRAKE.ref_press;

  int32_t force_pounds = (relative_100s_psi * PISTON_AREA) / 100;

  return POUND_FORCE_TO_NEWTONS(force_pounds);
}

void read_sensor(sensor_t *s) {
  s->raw = analogRead(s->channel);
  s->process(s);
}

void set_brake_valve(brake_valve_t *b, brake_valve_state new_state) {
  int engage_val = (new_state == kBrakeValveEngaged ? HIGH : LOW);
  digitalWrite(b->engage_pin, engage_val);

  int release_val = (new_state == kBrakeValveReleased ? HIGH : LOW);
  digitalWrite(b->release_pin, release_val);

  b->state = new_state;
}

/////////////////////// CORE CONTROLLER STEPS /////////////////////
/* Do: Network, Sensors, State, then Outputs */

void handle_network() {
  int packetSize = Udp.parsePacket();
  if (packetSize) {
    Serial.print("Received packet of size ");
    Serial.println(packetSize);
    Serial.print("From ");
    IPAddress remote = Udp.remoteIP();
    print_ip(remote);
    Serial.print(", port ");
    Serial.println(Udp.remotePort());

    // read the packet into packetBufffer
    req_packet_t pkt;

    Udp.read((uint8_t *)&pkt, sizeof(req_packet_t));
    Serial.println("Contents:");
    Serial.println((char *)&pkt);

    if (packetSize == sizeof(req_packet_t)) {
      BRAKE.mode = pkt.mode;
      if (BRAKE.mode == kModeStandard) {
        BRAKE.force_setpoint = pkt.arg0;
        BRAKE.ref_press = pkt.arg1;
      } else if (BRAKE.mode == kModeManual) {
        BRAKE.valve.state = (brake_valve_state_t)pkt.arg0;
      }
    }

    resp_packet_t response;
    response.mode = BRAKE.mode;
    response.tank_press = BRAKE.tank_press.val;
    response.piston_press = BRAKE.piston_press.val;
    response.dist_front = BRAKE.dist_front.val;
    response.dist_rear = BRAKE.dist_rear.val;
    response.valve_state = BRAKE.valve.state;
    response.temp_front = BRAKE.temp_front.val;
    response.temp_rear = BRAKE.temp_rear.val;
    response.force_setpoint = BRAKE.force_setpoint;

    // send a reply to the IP address and port that sent us the packet we
    // received
    Udp.beginPacket(Udp.remoteIP(), Udp.remotePort());
    Udp.write((uint8_t *)&response, sizeof(resp_packet_t));
    Udp.endPacket();
  }
}

void read_sensors() {
  int i = 0;
  while (BRAKE.sensors[i]) {
    read_sensor(BRAKE.sensors[i]);
    i++;
  }
}

void update_states() {
  if (BRAKE.mode == kModeStandard) {
    if (BRAKE.last_message - millis() > MESSAGE_TIMEOUT) {
      BRAKE.mode = kModeEmergency;
    }

  } else if (BRAKE.mode == kModeReset) {
    if (BRAKE.piston_press.val < PISTON_EMPTY_THRESH) {
      delay(101); // hit watchdog timer
    }
  }
}

void output_actuators() {
  if (BRAKE.mode == kModeStandard) {
    if (millis() - BRAKE.valve.last_update < BRAKE_FLUTTER_TIME) {
      // Skip, valve was recently changed, do not change yet
      return;
    }

    int32_t current_force = calculate_current_force();
    int32_t desired_force = BRAKE.force_setpoint;
    int32_t err = desired_force - current_force;

    // TODO: Deal with situations when tank pressure is getting low
    // (disallow release until controller sets force_setpoint <= 0)

    // TODO: Deal with overtemp situations?
    // (actually we probably don't want to actively control based on temp)

    // TODO: Deal with position of pad.
    // (Ensure that position front and back indicated pad is extended)
    brake_valve_state_t new_state = BRAKE.valve.state;
    if (abs(err) <= FORCE_SETPOINT_THRESH) {
      // Within Acceptable Range
      new_state = kBrakeValveClosed;
    } else if (err > 0) {
      // Underpressed
      new_state = kBrakeValveReleased;
    } else if (err < 0) {
      // Overpressed
      new_state = kBrakeValveEngaged;
    }

    if (BRAKE.valve.state != new_state) {
      set_brake_valve(&BRAKE.valve, new_state);
    }
  } else if (BRAKE.mode == kModeManual) {
    set_brake_valve(&BRAKE.valve, BRAKE.valve.state);
  } else if (BRAKE.mode == kModeEmergency) {
    set_brake_valve(&BRAKE.valve, kBrakeValveEngaged);
    // NOTE: if packet arrives from controller, the emergency is lifted.
  }
}

/////////////////////// SETUP & LOOP /////////////////////

void setup() {
  // 1 second startup watchdog
  Watchdog.enable(1000);

  // Initial Mode
  BRAKE.mode = kModeInit;

  // Setup Sensors
  sensor_init(&BRAKE.dist_front, (char *)"dist_front", 0, _dist_sensor);
  sensor_init(&BRAKE.dist_rear, (char *)"dist_rear", 0, _dist_sensor);

  sensor_init(&BRAKE.temp_front, (char *)"temp_front", 0, _temp_sensor);
  sensor_init(&BRAKE.temp_rear, (char *)"temp_rear", 0, _temp_sensor);

  sensor_init(&BRAKE.tank_press, (char *)"tank_press", 0, _press_sensor);
  sensor_init(&BRAKE.piston_press, (char *)"piston_press", 0, _press_sensor);

  // Set the UDP message timeout
  BRAKE.last_message = millis();

  // Configure the brake valve
  BRAKE.valve.engage_pin = 1;
  BRAKE.valve.release_pin = 2;
  BRAKE.valve.state = kBrakeValveClosed;
  BRAKE.valve.last_update = millis();

  // start the Ethernet and UDP:
  Ethernet.begin(mac, ip);
  Udp.begin(localPort);

  Serial.begin(9600);

  // 100ms watchdog.  Auto Reset on hang
  Watchdog.disable();
  Watchdog.enable(100);
}

void loop() {
  Watchdog.reset();

  // if there's data available, read a packet
  handle_network();

  // Read in and update sensors
  read_sensors();

  // Change controller mode if needed (network timeout, controller reset)
  update_states();

  // Outputs
  output_actuators();
}

/////////////////////// DEBUG /////////////////////

// handle diagnostic informations given by assertion and abort program
// execution:
void __assert(const char *__func, const char *__file, int __lineno,
              const char *__sexp) {
  // transmit diagnostic informations through serial link.
  Serial.println(__func);
  Serial.println(__file);
  Serial.println(__lineno, DEC);
  Serial.println(__sexp);
  Serial.flush();
  // abort program execution.
  abort();
}