Tractor unit CAN code

#include "AA_MCP2515.h"
#include "PGN.h"
#include <LCD-I2C.h>

#define UP_BUTTON_PIN 5
#define DOWN_BUTTON_PIN 4
#define MASTER_SWITCH_PIN A0

// TODO: modify CAN_BITRATE, CAN_PIN_CS(Chip Select) pin, and CAN_PIN_INT(Interrupt) pin as required.
const CANBitrate::Config CAN_BITRATE = CANBitrate::Config_8MHz_500kbps;
const uint8_t CAN_PIN_CS = 10;
const int8_t CAN_PIN_INT = 2;

uint8_t valvePins[4] = {9, 8, 7, 6};
uint8_t pinMask = B11111110;

volatile uint32_t previousMillis = 0;
const uint16_t interval = 150;

uint8_t bitShiftMask[] = {56, 48, 40, 32, 24, 16, 8, 0};

CANConfig config(CAN_BITRATE, CAN_PIN_CS, CAN_PIN_INT);
CANController CAN1(config);

LCD_I2C lcd(0x27, 20, 4);

void setup() {
  lcd.begin();
  lcd.display();
  lcd.backlight();

  Serial.begin(115200);

  while(CAN1.begin(CANController::Mode::Normal) != CANController::OK) {
    lcd.setCursor(0, 0);
    lcd.print("CAN Controller fault");
    lcd.setCursor(0, 1);
    lcd.print("Retrying in 1 second");
    delay(1000);
    lcd.clear();
  }
  lcd.setCursor(0,0);
  lcd.print("CAN Controller OKAY");
  delay(500);
  lcd.clear();

  Serial.println(sizeof(valvePins));

  for (int i = 0; i<sizeof(valvePins); i++){
    pinMode(valvePins[i], INPUT_PULLUP);
  }
  pinMode(UP_BUTTON_PIN, INPUT_PULLUP);
  pinMode(DOWN_BUTTON_PIN, INPUT_PULLUP);
  pinMode(MASTER_SWITCH_PIN, INPUT_PULLUP);


}

void loop() {

  CANFrame frame;
  if (CAN1.read(frame) == CANController::IOResult::OK) {
    frame.print("RX: ");
    uint32_t id = frame.getId();
    uint8_t dlc = frame.getDlc();
    uint8_t *packet = frame.getData();
    uint8_t dataArray[dlc];
    for (uint8_t i = 0; i<dlc; i++){
      dataArray[i] = *(packet++);
    }

    switch(id) {
      case PRESSURE_PGN:
        uint16_t pressure =
        (dataArray[4] << 24) |
        (dataArray[5] << 16) |
        (dataArray[6] << 8) |
        (dataArray[7]);
        pressureRead(pressure);
        break;

      case FLOW_PGN:

        break;

      case SPEED_PGN:

        break;

    }
  }

  uint32_t currentMillis = millis();

  if (currentMillis - previousMillis >= interval) {
    previousMillis = currentMillis;
    uint8_t valveMessage = 0;
    for (int i=0; i<sizeof(valvePins); i++){
      uint8_t var = digitalRead(valvePins[i]);
      uint8_t var2 = var^pinMask; //XOR with B11111110 to produce the inverse, then
      uint8_t var3 = ~var2; // perform a NOT to flip the bits back. This is so we can invert a HIGH input to a 0 in our PGN Byte
      valveMessage |= var3 << i;
    }

    uint8_t data2[] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, valveMessage};

    CANFrame valveFrame(VALVE_PGN, data2, sizeof(data2));
    CAN1.write(valveFrame);
    valveFrame.print("TX: ");

   bool upbtn = digitalRead(UP_BUTTON_PIN);
   bool downbtn = digitalRead(DOWN_BUTTON_PIN);


   if (!upbtn && downbtn) {
    uint8_t UDPacket[] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xAA};
    CANFrame UDFrame(UP_DOWN_PGN, UDPacket, sizeof(UDPacket));
    CAN1.write(UDFrame);
    UDFrame.print("TX: ");
   }
   else if (upbtn && !downbtn) {
    uint8_t UDPacket[] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x55};
    CANFrame UDFrame(UP_DOWN_PGN, UDPacket, sizeof(UDPacket));
    CAN1.write(UDFrame);
    UDFrame.print("TX: ");
   }
   else {
    uint8_t UDPacket[] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
    CANFrame UDFrame(UP_DOWN_PGN, UDPacket, sizeof(UDPacket));
    CAN1.write(UDFrame);
    UDFrame.print("TX: ");
   }


  bool masterSwitch = digitalRead(MASTER_SWITCH_PIN);
    /*The switch is connected to ground and pulls the pin low when set to on.
     Because of this, we want to invert the bool to make it make more sense in our heads here*/
    if (!masterSwitch == true){
      uint8_t MPacket[] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xAA};
      CANFrame MFrame(MASTER_SWITCH_PGN, MPacket, sizeof(MPacket));
      CAN1.write(MFrame);
      MFrame.print("TX: ");
    }
    else if (!masterSwitch == false){
      uint8_t MPacket[] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x55};
      CANFrame MFrame(MASTER_SWITCH_PGN, MPacket, sizeof(MPacket));
      CAN1.write(MFrame);
      MFrame.print("TX: ");
    }

  }
}


void pressureRead(uint16_t pressure) {
  lcd.clear();

  double bar = pressure/1000.0;

  lcd.print(bar);
}