// Truck Door Automation with linear actuators.
// By Travis Christensen - TravPlan on github.
// Motion control of 8x linear actuators to open and close the hood, and doors on a custom
// truck built by Harse Autocraft

// Motoron Motor Controllers using I2C - Motoron Library by Pololu Corporation.
// AceButton Library by Brian T. Park

// The motors will stop but automatically recover if:
// - Motor power (VIN) is interrupted, or
// - A temporary motor fault occurs, or
// - A command timeout occurs.
//
// The motors will stop until you power cycle or reset your
// Arduino if:
// - The Motoron experiences a reset.
//
// If a latched motor fault occurs, the motors
// experiencing the fault will stop until you power cycle motor
// power (VIN) or cause the motors to coast.

#include <Arduino.h>


#include <Motoron.h>
#include <AceButton.h>
#include <RunningAverage.h>

using namespace ace_button;

// ADC reference voltage: change to 3300 if using a 3.3 V Arduino.
  const uint16_t referenceMv = 5000;

// Minimum allowed VIN voltage.  You can raise this to be closer
// to your power supply's expected voltage.
  const uint16_t minVinVoltageMv = 4000;

// Define which status flags the Motoron should treat as errors.
  const uint16_t errorMask =
    (1 << MOTORON_STATUS_FLAG_PROTOCOL_ERROR) | (1 << MOTORON_STATUS_FLAG_CRC_ERROR) | (1 << MOTORON_STATUS_FLAG_COMMAND_TIMEOUT_LATCHED) | (1 << MOTORON_STATUS_FLAG_MOTOR_FAULT_LATCHED) | (1 << MOTORON_STATUS_FLAG_NO_POWER_LATCHED) | (1 << MOTORON_STATUS_FLAG_UART_ERROR) | (1 << MOTORON_STATUS_FLAG_RESET) | (1 << MOTORON_STATUS_FLAG_COMMAND_TIMEOUT);

//Timing and debugging variables for serialOutput
  unsigned long previousTime = 0;
  const int debugging = 1; // 1 = serial stream enabled, 0 = disabled
  const unsigned long serialInterval = 500; //update rate of serial stream in ms

// Specifiy the door actuators
// This code creates an object for each Motoron controller.
// The number passed as the first argument to each constructor
// below should be the 7-bit I2C address of the controller.
// Use the I2CSetAddresses sketch to
// assign a unique I2C address to each Motoron, and then
// modify the list below to match the setup.

//PRIOR TO 2023-08-28
  // MotoronI2C mc1(10);  // define address for HOOD controller
  // MotoronI2C mc2(11);  // define address for DRIVER'S DOOR SWING/LEFT REAR SWING controller
  // MotoronI2C mc3(12);  // define address for PASSENGER DOOR SWING/RIGHT REAR SWING controller
  // MotoronI2C mc4(13);  // define address for DRIVER DOOR FOLD/PASSENGER DOOR FOLD controller
  // MotoronI2C mc5(14);  // define address for LEFT REAR FOLD/RIGHT REAR FOLD controller
 
 // POST 2023-08-28
  MotoronI2C mc1(10);  // define address for HOOD ACTUATORS (2 IN PARRALLEL)/REAR FOLD ACTUATORS (2(4) IN PARELLEL) controller
  MotoronI2C mc2(11);  // define address for DRIVER'S DOOR SWING/LEFT REAR SWING controller
  MotoronI2C mc3(12);  // define address for PASSENGER DOOR SWING/RIGHT REAR SWING controller
  MotoronI2C mc4(13);  // define address for DRIVER DOOR FOLD/PASSENGER DOOR FOLD controller
  //MotoronI2C mc5(14);  // define address for LEFT REAR FOLD/RIGHT REAR FOLD controller
 

// // LED states. Used during testing.
//   const int LED_ON = HIGH;
//   const int LED_OFF = LOW;
// //these are just for defining some LED pins for testing/debugging
//   const int hoodMode = 13;
//   const int hoodOpen = 12;
//   const int hoodClose = 11;

// Physical pin numbers attached to the buttons
  const int hoodBut = 14;
  const int driveBut = 15;//15
  const int passBut = 16;
  const int rearBut = 28;
  const int allBut = 18;

//Digital Inputs
//**TO DO ** **Finish setting up pins in this section
  const int parkSig = 19;  // pin to enable doors *******THIS IS NOT IMPLEMENTED YET***********
  const int leftSig = 22;  //This was set to 20 which is the SDA pin as was messing up the I2C bus
  const int rightSig = 23; //This was set to 21 which is the SCL pin as was messing up the I2C bus
  const int runningSig = 2;
  const int brakeSig = 3;

//Analog Inputs
  int hoodPot = A0;
  //int hoodPotRight = A1;
  int drivePotSwing = A2;
  int drivePotFold = A3;
  int passPotSwing = A4;
  int passPotFold = A5;
  int rearPotFold = A6;
  // int rearPotLeftFold = A6;
  int rearPotLeftSwing = A7;
  // int rearPotRightFold = A8;
  int rearPotRightSwing = A9;
//**TO DO ** ** DONT NEED THESE ANYMORE. Could change these 2 inputs below for the rear door fold actuators,
//but using the poly fuses should work ok.
  int driveFoldCurrent = A10;
  int passFoldCurrent = A11;
//Analog input Averaging
//mc2.2 current sensor calibration DFC(drivers door fold):
//mc3.2 current sensor calibration PFC(passenger door fold):
// Current: -1000mA <->   0mA   <-> +1000mA
// Counts:     0    <-> 516-521 <->   1024
// Gain Calibrated using: 1 counts = 2mA
// Tested at 302mA which showed ~518 counts when light off, to ~670 when on on both A08 (drivers fold current sensor) and A09 (passenger fold current sensor)
// At an averaging rate of 30, there is about a +/-2.5 counts variation from the ~counts in the test on the above line
  RunningAverage DFC(30); //Drivers fold current. 30 samples seems like a good compromise between response time vs stability
  RunningAverage PFC(30); //Passenger fold current




//Outputs
//**TO DO ** **Finish setting up pins in this section *******THIS IS NOT IMPLEMENTED YET***********
  const int leftOut = 4;
  const int rightOut = 5;
  const int runningOut = 6;
  const int brakeOut = 7;
  //door magnet relay outputs - relay closed when pin LOW
  const int driveMag = 8;//8
  const int passMag = 9;
  const int rearMag = 10;


//Declare Variables for button action modes
  int hoodActiveMode = 0;
  int hoodFlip = 0;
  int hoodLong = 0;

  int driveActiveMode = 0;
  int driveFlip = 0;
  int driveLong = 0;

  int passActiveMode = 0;
  int passFlip = 0;
  int passLong  = 0;

  int rearActiveMode = 0;
  int rearFlip = 0;
  int rearLong = 0;

  int allActiveMode = 0;
  int allFlip = 0;
  int allLong = 0;

  int hoodTimer = 0;
  int driveTimer = 0;
  int passTimer = 0;
  int rearTimer = 0;

//Declare variables to hold current sensed values from Motorons
//"mc11" is motoron mc1 on motor output channel 1. The others follow this scheme.
  unsigned int mc11CurrentSense = 0;
  unsigned int mc12CurrentSense = 0;
  unsigned int mc21CurrentSense = 0;
  unsigned int mc22CurrentSense = 0;
  unsigned int mc31CurrentSense = 0;
  unsigned int mc32CurrentSense = 0;
  unsigned int mc41CurrentSense = 0;
  unsigned int mc42CurrentSense = 0;
  // unsigned int mc51CurrentSense = 0;
  // unsigned int mc52CurrentSense = 0;

//Set max currents units. This is not actual current but a variable used to trigger functions
//based on the number received from Motoron getCurrentSenseRaw as tested.
//These values have to be adjusted when speed and or current limits are changed
  const int hoodCurrentStopLeft = 12;  
  const int hoodCurrentStopRight = 3;

  const int driveCurrentStopSwing = 13;
  const int driveCurrentStopFold = 13;

  const int passCurrentStopSwing = 4;
  const int passCurrentStopFoldOpen = 761;
  const int passCurrentStopFoldClose = 261;

  const int rearCurrentStopLeftFold = 12;
  const int rearCurrentStopLeftSwing = 12;
  const int rearCurrentStopRightFold = 12;
  const int rearCurrentStopRightSwing = 12;

//variables to hold the state of actuator overcurrent events
// 0 = ok/no over current event, 1 = over current event active
  int hoodOverCurrent = 0;
  int driveSwingOverCurrent = 0;
  int driveFoldOverCurrent = 0;
  int passSwingOverCurrent = 0;
  int passFoldOverCurrent = 0;
  int rearLeftSwingOverCurrent = 0;
  int rearFoldOverCurrent = 0;
  int rearRightSwingOverCurrent = 0;
  // int rearRightFoldOverCurrent = 0;

//Define motion operation states for each mass. 0 = Stopped, 1 = Opening, 2 = Closing
  int hoodState = 0; 
  int driveState = 0; 
  int passState = 0; 
  int rearState = 0; 

// All buttons automatically use the default "AceButton" System ButtonConfig
  AceButton button1(hoodBut);
  AceButton button2(driveBut);
  AceButton button3(passBut);
  AceButton button4(rearBut);
  AceButton button5(allBut);

  //Button timeout variables

  unsigned long hoodPrevMillis = 0;
  unsigned long drivePrevMillis = 0;
  unsigned long passPrevMillis = 0;
  unsigned long rearPrevMillis = 0;


//Basic settings definition for Motorons
void setupMotoron(MotoronI2C& mc) {

  mc1.reinitialize();
  mc1.disableCrc();
  mc2.reinitialize();
  mc2.disableCrc();
  mc2.reinitialize();
  mc3.disableCrc();
  mc4.reinitialize();
  mc4.disableCrc();
  // mc5.reinitialize();
  // mc5.disableCrc();  

  // Clear the reset flag, which is set after the controller
  // reinitializes and counts as an error.
  mc1.clearResetFlag();
  mc2.clearResetFlag();
  mc3.clearResetFlag();
  mc4.clearResetFlag();
  // mc5.clearResetFlag();  

  // By default, the Motoron is configured to stop the motors if
  // it does not get a motor control command for 1500 ms. Uncomment
  // the lines below to adjust this time or disable the timeout feature.
  // mc.setCommandTimeoutMilliseconds(1000);
  mc1.disableCommandTimeout();
  mc1.setErrorMask(errorMask);
  mc2.disableCommandTimeout();
  mc2.setErrorMask(errorMask);
  mc3.disableCommandTimeout();
  //mc3.setErrorMask(errorMask); //*********mc3 has an error if this line is uncommented. I donno why.
  mc4.disableCommandTimeout();
  mc4.setErrorMask(errorMask);
  // mc5.disableCommandTimeout();
  //mc5.setErrorMask(errorMask);
}



void setup() {
  
  Serial.begin(115200);
  while (!Serial) delay(10);
  delay(800);

  //Start the I2C service
  Wire.begin();

  //Run setups on all Motorons
  setupMotoron(mc1);
  setupMotoron(mc2);
  setupMotoron(mc3);
  setupMotoron(mc4);
  // setupMotoron(mc5);


  // Define INPUT pinmodes
  pinMode(hoodBut, INPUT_PULLUP);
  pinMode(driveBut, INPUT_PULLUP);
  pinMode(passBut, INPUT_PULLUP);
  pinMode(rearBut, INPUT_PULLUP);
  pinMode(allBut, INPUT_PULLUP);

  pinMode(parkSig, INPUT_PULLUP);
  pinMode(leftSig, INPUT_PULLUP);
  pinMode(rightSig, INPUT_PULLUP);
  pinMode(runningSig, INPUT_PULLUP);
  pinMode(brakeSig, INPUT_PULLUP);

  // Define OUTPUT pinmodes
  // pinMode(hoodMode, OUTPUT); these three hood pins were used for leds during initial testing
  // pinMode(hoodOpen, OUTPUT);
  // pinMode(hoodClose, OUTPUT);
  pinMode(driveMag, OUTPUT);
  pinMode(passMag, OUTPUT);
  pinMode(rearMag, OUTPUT);


  //set magnet relay pins HIGH on system startup to activate relay, and energize magnets
  digitalWrite(driveMag, HIGH);
  digitalWrite(passMag, HIGH);
  digitalWrite(rearMag, HIGH);




  // Configure the ButtonConfig with the event handler, and enable all higher
  // level events.
  ButtonConfig* buttonConfig = ButtonConfig::getSystemButtonConfig();
  buttonConfig->setEventHandler(handleEvent);
  buttonConfig->setFeature(ButtonConfig::kFeatureClick);
  buttonConfig->setFeature(ButtonConfig::kFeatureDoubleClick);
  buttonConfig->setFeature(ButtonConfig::kFeatureLongPress);
  buttonConfig->setFeature(ButtonConfig::kFeatureSuppressClickBeforeDoubleClick);
  buttonConfig->setFeature(ButtonConfig::kFeatureSuppressAfterClick);
  buttonConfig->setFeature(ButtonConfig::kFeatureSuppressAfterDoubleClick);
  // buttonConfig->setFeature(ButtonConfig::kFeatureRepeatPress);

  //*******************************************
  //HOOD and REAR FOLD CONTROLLER SETUP
  //*******************************************
  //Hood Left Actuator
  mc1.setMaxAcceleration(1, 80);
  mc1.setMaxDeceleration(1, 300);
  mc1.setDirectionChangeDelay(1, 100);  //100ms lag between direction changes
  //mc2.setCurrentLimit(1, 10);           //Use "Current sense offset" sketch and use (offset value*125)/128.
  //Then, desired mA*20/5000. Add these two numbers together, and input value.

  //Rear doors left and right fold actuators
  mc1.setMaxAcceleration(2, 80);
  mc1.setMaxDeceleration(2, 300);
  mc1.setDirectionChangeDelay(2, 100);  //100ms lag between direction changes
  //mc2.setCurrentLimit(2, 50);           //- 1000mA max - Use "Current sense offset" sketch and use (offset value*125)/128.
  //Then, desired mA*20/5000. Add these two numbers together, and input value.

  //*******************************************
  //DRIVER'S DOOR CONTROLLER SETUP
  //*******************************************
  //Door Swing Actuator
  mc2.setMaxAcceleration(1, 80);
  mc2.setMaxDeceleration(1, 300);
  mc2.setDirectionChangeDelay(1, 100);  //100ms lag between direction changes
  //mc2.setCurrentLimit(1, 10);           //Use "Current sense offset" sketch and use (offset value*125)/128.

  //Then, desired mA*20/5000. Add these two numbers together, and input value.

  //Door Fold Actuator
  mc4.setMaxAcceleration(1, 300);
  mc4.setMaxDeceleration(1, 300);
  mc4.setDirectionChangeDelay(1, 100);  //100ms lag between direction changes
  //mc2.setCurrentLimit(2, 50);           //- 1000mA max - Use "Current sense offset" sketch and use (offset value*125)/128.
  //Then, desired mA*20/5000. Add these two numbers together, and input value.

  //*******************************************
  //PASSENGER DOOR CONTROLLER SETUP
  //*******************************************
  //Door Swing Actuator
  mc3.setMaxAcceleration(1, 80);
  mc3.setMaxDeceleration(1, 300);
  mc3.setDirectionChangeDelay(1, 100);  //100ms lag between direction changes
 // mc3.setCurrentLimit(1, 50);           //Use "Current sense offset" sketch and use (offset value*125)/128.
  //Then, desired mA*20/5000. Add these two numbers together, and input value.

  //Door Fold Actuator
  mc4.setMaxAcceleration(2, 300);
  mc4.setMaxDeceleration(2, 300);
  mc4.setDirectionChangeDelay(2, 100);  //100ms lag between direction changes
 // mc3.setCurrentLimit(2, 50);           //Use "Current sense offset" sketch and use (offset value*125)/128.
  //Then, desired mA*20/5000. Add these two numbers together, and input value.

  //*******************************************
  //REAR DOOR CONTROLLERS SETUP
  //*******************************************
  //Door Left Swing Actuator
  mc2.setMaxAcceleration(2, 80);
  mc2.setMaxDeceleration(2, 300);
  mc2.setDirectionChangeDelay(2, 100);  //100ms lag between direction changes
 // mc4.setCurrentLimit(1, 50);           //Use "Current sense offset" sketch and use (offset value*125)/128.
  //Then, desired mA*20/5000. Add these two numbers together, and input value.

  //Door Right Swing Actuator
  mc3.setMaxAcceleration(2, 80);
  mc3.setMaxDeceleration(2, 300);
  mc3.setDirectionChangeDelay(2, 100);  //100ms lag between direction changes
 // mc4.setCurrentLimit(2, 50);           //Use "Current sense offset" sketch and use (offset value*125)/128.
  //Then, desired mA*20/5000. Add these two numbers together, and input value.

//   //Door Left Fold Actuator
//   mc5.setMaxAcceleration(1, 80);
//   mc5.setMaxDeceleration(1, 300);
//   mc5.setDirectionChangeDelay(1, 100);  //100ms lag between direction changes
//  // mc4.setCurrentLimit(1, 50);           //Use "Current sense offset" sketch and use (offset value*125)/128.
//   //Then, desired mA*20/5000. Add these two numbers together, and input value.

//   //Door Right Fold Actuator
//   mc5.setMaxAcceleration(2, 80);
//   mc5.setMaxDeceleration(2, 300);
//   mc5.setDirectionChangeDelay(2, 100);  //100ms lag between direction changes
//  // mc4.setCurrentLimit(2, 50);           //Use "Current sense offset" sketch and use (offset value*125)/128.
//   //Then, desired mA*20/5000. Add these two numbers together, and input value.

}


// /*SETUP ERROR CHECKING FOR MOTOR DRIVERS*/ **CURRENTLY ONLY WORKING ON MC1(ADDR. 10)
// void checkCommunicationError(uint8_t errorCode) {
//   if (errorCode) {
//     while (1) {
//       mc1.reset();
//       Serial.print(F("Communication error: "));
//       Serial.println(errorCode);
//       delay(1000);
//     }
//   }
// }

// void checkControllerError(uint16_t status) {
//   if (status & errorMask) {
//     while (1) {
//       // One of the error flags is set.  The Motoron should
//       // already be stopping the motors.  We report the issue to
//       // the user and send reset commands to be extra careful.
//       mc1.reset();
//       Serial.print(F("Controller error: 0x"));
//       Serial.println(status, HEX);
//       delay(1000);
//     }
//   }
// }

// void checkVinVoltage(uint16_t voltageMv) {
//   if (voltageMv < minVinVoltageMv) {
//     while (1) {
//       mc1.reset();
//       Serial.print(F("VIN voltage too low: "));
//       Serial.println(voltageMv);
//       delay(1000);
//     }
//   }
// }

// void checkForProblems() {
//   uint16_t status = mc1.getStatusFlags();
//   checkCommunicationError(mc1.getLastError());
//   checkControllerError(status);

//   uint32_t voltageMv = mc1.getVinVoltageMv(referenceMv);
//   checkCommunicationError(mc1.getLastError());
//   checkVinVoltage(voltageMv);
// }

/*********************************************************************************
**********************************************************************************
BUTTONS MANAGEMENT
*********************************************************************************
*********************************************************************************/
// The event handler for all buttons.
void handleEvent(AceButton* button, uint8_t eventType, uint8_t buttonState) {

  // Print out a message for all events, for all buttons. This is mainly for testing/debugging.
  Serial.print(F("handleEvent(): pin: "));
  Serial.print(button->getPin());
  Serial.print(F("; eventType: "));
  Serial.print(eventType);
  Serial.print(F("; buttonState: "));
  Serial.print(buttonState);
  Serial.print(F("; Active Mode: "));
  Serial.print(driveActiveMode);
  Serial.print(F("; Flip Direction: "));
  Serial.println(driveFlip);

  switch (eventType) {

/*********************************************************************************
BUTTON FAST CLICK OPERATIONS
*********************************************************************************/
    case AceButton::kEventClicked:
//HOOD CLICKS
      if (button->getPin() == hoodBut && hoodFlip == 0) {
        hoodActiveMode = 1;
        hoodState = 0;
        doHoodOpen();
        Serial.print("Hood Open Call");
        hoodFlip = 1;
      } else if (button->getPin() == hoodBut && hoodFlip == 1) {
        hoodActiveMode = 1;
        hoodState = 0;
        doHoodClose();
        hoodFlip = 0;
      }
    
//DRIVER DOOR CLICKS
      if (button->getPin() == driveBut && driveFlip == 0) {
        driveActiveMode = 1;
        driveState = 0;
        doDriveOpen();
        driveFlip = 1;
        Serial.print("Driver Open Call");
      } else if (button->getPin() == driveBut && driveFlip == 1) {
        driveActiveMode = 1;
        driveState = 0;
        doDriveClose();
        driveFlip = 0;
      }
      
//PASSENGER DOOR CLICKS
      if (button->getPin() == passBut && passFlip == 0) {
        passActiveMode = 1;
        passState = 0;
        doPassOpen();
        passFlip = 1;
      } else if (button->getPin() == passBut && passFlip == 1) {
        passActiveMode = 1;
        passState = 0;
        doPassClose();
        passFlip = 0;
      }
      
//REAR DOOR CLICKS
      if (button->getPin() == rearBut && rearFlip == 0) {
        rearActiveMode = 1;
        rearState = 0;
        doRearOpen();
        rearFlip = 1;
      } else if (button->getPin() == rearBut && rearFlip == 1) {
        rearActiveMode = 1;
        rearState = 0;
        doRearClose();
        rearFlip = 0;
      }
           
//ALL CLICKS
      if (button->getPin() == allBut && allFlip == 0) {
        allActiveMode = 1;
        hoodState = 0;
        driveState = 0;
        passState = 0;
        rearState = 0;
        doAllOpen();
        allFlip = 1;
      } else if (button->getPin() == allBut && allFlip == 1) {
        allActiveMode = 1;
        hoodState = 0;
        driveState = 0;
        passState = 0;
        rearState = 0;        
        doAllClose();
        allFlip = 0;
      }
      break;      

/*********************************************************************************
BUTTON PRESSED OPERATIONS (LONGER CLICK)
*********************************************************************************/

//HOOD PRESSES
    case AceButton::kEventPressed:
      if (button->getPin() == hoodBut && hoodActiveMode == 1) {
        doHoodStop();
        hoodActiveMode = 0;
      }
      
//drive PRESSES
      if (button->getPin() == driveBut && driveActiveMode == 1) {
        doDriveStopBoth();
        driveActiveMode = 0;
      }
      
//pass PRESSES
      if (button->getPin() == passBut && passActiveMode == 1) {
        doPassStopBoth();
        passActiveMode = 0;
      }
      
//rear PRESSES
      if (button->getPin() == rearBut && rearActiveMode == 1) {
        doRearStopBoth();
        rearActiveMode = 0;
      }
      
//all PRESSES
      if (button->getPin() == allBut && allActiveMode == 1) {
        doAllStop();
        allActiveMode = 0;
      }
      break;                        


/*********************************************************************************
BUTTON RELEASED OPERATIONS
*********************************************************************************/

//Hood Release
    case AceButton::kEventReleased:
      if (button->getPin() == hoodBut && hoodActiveMode == 1 && hoodLong == 1) {
        doHoodStop();
        hoodActiveMode = 0;
        hoodLong = 0;
      }
      
//drive Release
      if (button->getPin() == driveBut && driveActiveMode == 1 && driveLong == 1) {
        doDriveStopBoth();
        driveActiveMode = 0;
        driveLong = 0;
      }
      
//pass Release
      if (button->getPin() == passBut && passActiveMode == 1 && passLong == 1) {
        doPassStopBoth();
        passActiveMode = 0;
        passLong = 0;
      }
      
//rear Release
      if (button->getPin() == rearBut && rearActiveMode == 1 && rearLong == 1) {
        doRearStopBoth();
        rearActiveMode = 0;
        rearLong = 0;
      }
      
//all Release
      if (button->getPin() == allBut && allActiveMode == 1 && allLong == 1) {
        doAllStop();
        allActiveMode = 0;
        allLong = 0;
      }
      break;      

/*********************************************************************************
BUTTON DOUBLE-CLICK OPERATIONS
*********************************************************************************/

//Currently no double-click operations

/*********************************************************************************
BUTTON LONG-PRESS OPERATIONS
*********************************************************************************/

//Hood Long Presses
    case AceButton::kEventLongPressed:
      if (button->getPin() == hoodBut && hoodFlip == 0) {
        hoodActiveMode = 1;
        hoodState = 0;
        doHoodOpen();
        hoodFlip = 1;
        hoodLong = 1;
      } else if (button->getPin() == hoodBut && hoodFlip == 1) {
        hoodActiveMode = 1;
        hoodState = 0;
        doHoodClose();
        hoodFlip = 0;
        hoodLong = 1;
      }
      
//drive Long Presses
      if (button->getPin() == driveBut && driveFlip == 0) {
        driveActiveMode = 1;
        driveState = 0;
        doDriveOpen();
        driveFlip = 1;
        driveLong = 1;
      } else if (button->getPin() == driveBut && driveFlip == 1) {
        driveActiveMode = 1;
        driveState = 0;
        doDriveClose();
        driveFlip = 0;
        driveLong = 1;
      }
      
//pass Long Presses
      if (button->getPin() == passBut && passFlip == 0) {
        passActiveMode = 1;
        passState = 0;
        doPassOpen();
        passFlip = 1;
        passLong = 1;
      } else if (button->getPin() == passBut && passFlip == 1) {
        passActiveMode = 1;
        passState = 0;
        doPassClose();
        passFlip = 0;
        passLong = 1;
      }
      
//rear Long Presses
      if (button->getPin() == rearBut && rearFlip == 0) {
        rearActiveMode = 1;
        rearState = 0;
        doRearOpen();
        rearFlip = 1;
        rearLong = 1;
      } else if (button->getPin() == rearBut && rearFlip == 1) {
        rearActiveMode = 1;
        rearState = 0;
        doRearClose();
        rearFlip = 0;
        rearLong = 1;
      }
      
//all Long Presses
      if (button->getPin() == allBut && allFlip == 0) {
        allActiveMode = 1;
        hoodState = 0;
        driveState = 0;
        passState = 0;
        rearState = 0;
        doAllOpen();
        allFlip = 1;
        allLong = 1;
      } else if (button->getPin() == allBut && allFlip == 1) {
        allActiveMode = 1;
        hoodState = 0;
        driveState = 0;
        passState = 0;
        rearState = 0;        
        doAllClose();
        allFlip = 0;
        allLong = 1;
      }
      break;


  }
}




void loop() {

doHoodTimer();
doDriveTimer();
doPassTimer();
doRearTimer();



  //Motor controller error checking call
  //checkForProblems();
  
  // Should be called every 4-5ms or faster, for the default debouncing time
  // of ~20ms.
  button1.check();
  button2.check();
  button3.check();
  button4.check();
  button5.check();

//update all actuator positions
  hoodPot = analogRead(A0);
  //hoodPotRight = analogRead(A0); //changed this to be the same as above because potentiometers are in parallel
  drivePotSwing = analogRead(A2);
  drivePotFold = analogRead(A3);
  passPotSwing = analogRead(A4);
  passPotFold = analogRead(A5);
  rearPotFold = analogRead(A6);
  // rearPotLeftFold = analogRead(A6);
  rearPotLeftSwing =analogRead(A7);
  // rearPotRightFold = analogRead(A8);
  rearPotRightSwing = analogRead(A9);
  driveFoldCurrent = analogRead(A10);
  passFoldCurrent = analogRead(A11);

  //average actuator position values
  DFC.addValue(driveFoldCurrent);
  PFC.addValue(passFoldCurrent);

//sense raw current value from motor controllers
  mc11CurrentSense = mc1.getCurrentSenseRaw(1);
  mc12CurrentSense = mc1.getCurrentSenseRaw(2);

  mc21CurrentSense = mc2.getCurrentSenseRaw(1);
  mc22CurrentSense = mc2.getCurrentSenseRaw(2);

  mc31CurrentSense = mc3.getCurrentSenseRaw(1);
  mc32CurrentSense = mc3.getCurrentSenseRaw(2);

  mc41CurrentSense = mc4.getCurrentSenseRaw(1);
  mc42CurrentSense = mc4.getCurrentSenseRaw(2);

  // mc51CurrentSense = mc5.getCurrentSenseRaw(1);
  // mc52CurrentSense = mc5.getCurrentSenseRaw(2);


/***CHECK ACTUATOR SYNC AND CURRENT LIMITS****
*********************************************/
  checkSync();

//disable all doors and energize magnets if vehicle is not in park
// if (parkSig == HIGH) {
//   doHoodLeftStop();
//   doHoodRightStop();
//   doDriveSwingStop();
//   doDriveFoldStop();
//   doPassSwingStop();
//   doPassFoldStop();
//   doRearLeftSwingStop();
//   doRearLeftFoldStop();
//   doRearRightSwingStop();
//   doRearRightFoldStop();
//   digitalWrite(driveMag, HIGH);
//   digitalWrite(passMag, HIGH);
//   digitalWrite(rearMag, HIGH);

// }
  //Trigger Hood fully open event based on hood opening current stop value (hoodOpenCurrentStop). **This value seems to be low resolution.
  // if (mc11CurrentSense > hoodCurrentStopLeft) {
  //   doHoodLeftStop();
  // }

  // if (mc12CurrentSense < hoodCurrentStopRight) {
  //   doHoodRightStop();
  // }
  // if (mc21CurrentSense > driveCurrentStopSwing) {
  //   doDriveSwingStop();
  //   driveSwingOverCurrent = 1;
  // }
  // if (mc22CurrentSense > driveCurrentStopFold) {
  //   doDriveFoldStop();
  //   driveFoldOverCurrent = 1;
  // }
//  if (passState == 1 && ((PFC.getAverage(), 2) >= passCurrentStopFoldOpen)){
//    passFoldOverCurrent = 1;
//  }else {
//     passFoldOverCurrent = 0;
     
//  }

//  if (passState == 2 && ((PFC.getAverage(), 2) >= passCurrentStopFoldClose)){
//    passFoldOverCurrent = 1;
//  }else {
//     passFoldOverCurrent = 0;
  
//  }

  // Serial.print(PFC.getAverage(), 2);
  // Serial.println();

 //Main Debugging/setup serial output
 if (debugging == 1){
  unsigned long currentTime = millis();
  //run the serial output once every "serialInterval", default value is 500ms.
  if (currentTime - previousTime >= serialInterval){
    //HOOD
    Serial.println("START");
    Serial.print("HOOD:           ");
    Serial.print("hoodPot  = ");
    Serial.print(hoodPot);
    Serial.print("  ");
    // Serial.print(" hoodPotRight= ");
    // Serial.print(hoodPotRight);
    // Serial.print("  ");
    Serial.print(" hoodCurrent  = ");
    Serial.print(mc11CurrentSense);
    Serial.print("  ");
    Serial.print("hoodOverCurrent=   ");
    Serial.print(hoodOverCurrent);
    Serial.print("  ");    
    Serial.print("hoodState = ");
    Serial.print("  ");
    Serial.print(hoodState);
    Serial.print("  ");    
    Serial.print("hoodSpeed = ");
    Serial.print("  ");
    Serial.print(mc1.getTargetSpeed(1));    
    Serial.println();
    
    //DRIVER DOOR
    Serial.print("DRIVER DOOR:    ");
    Serial.print("drivePotSwing= ");
    Serial.print(drivePotSwing);
    Serial.print("  ");
    Serial.print(" drivePotFold= ");
    Serial.print(drivePotFold);
    Serial.print("  ");
    Serial.print(" driveSwingCurrent= ");
    Serial.print(mc21CurrentSense);
    Serial.print("  ");
    Serial.print("driveSwingOverCurrent= ");
    Serial.print(driveSwingOverCurrent);
    Serial.print("  ");        
    Serial.print("driveFoldCurrent= ");
    Serial.print(DFC.getAverage(), 2);
    Serial.print("  ");
    Serial.print("driveFoldOverCurrent= ");
    Serial.print(driveFoldOverCurrent);
    Serial.print("  ");    
    Serial.print("driveState= ");
    Serial.print(driveState);
    Serial.print("  ");
    Serial.println();
    
    //PASSENGER DOOR
    Serial.print("PASSENGER DOOR: ");
    Serial.print("passPotSwing=  ");
    Serial.print(passPotSwing);
    Serial.print("  ");
    Serial.print(" passPotFold=  ");
    Serial.print(passPotFold);
    Serial.print("  ");
    Serial.print(" passSwingCurrent=  ");
    Serial.print(mc31CurrentSense);
    Serial.print("  ");
    Serial.print("passSwingOverCurrent=  ");
    Serial.print(passSwingOverCurrent);
    Serial.print("  ");        
    Serial.print("passFoldCurrent=  ");
    Serial.print(PFC.getAverage(), 2);
    Serial.print("  ");
    Serial.print("passFoldOverCurrent=  ");
    Serial.print(passFoldOverCurrent);
    Serial.print("  ");    
    Serial.print("passState=  ");
    Serial.print(passState);
    Serial.print("  ");    
    Serial.println();

    //REAR DOORS
    Serial.print("REAR DOORS:     ");
    Serial.print(" rearPotFold= ");
    Serial.print(rearPotFold);
    Serial.print("  ");
    Serial.print("rearPotLeftSwing=   ");
    Serial.print(rearPotLeftSwing);
    Serial.print("  ");
    Serial.print(" rearLeftSwingCurrent=   ");
    Serial.print(mc41CurrentSense);
    Serial.print("  ");
    Serial.print("rearLeftSwingOverCurrent=   ");
    Serial.print(rearLeftSwingOverCurrent);
    Serial.print("  ");        
    Serial.print("rearPotRightSwing=   ");
    Serial.print(rearPotRightSwing);
    Serial.print("  ");
    // Serial.print(" rearPotRightFold= ");
    // Serial.print(rearPotRightFold);
    // Serial.print("  ");
    Serial.print("rearRightSwingCurrent= ");
    Serial.print(mc42CurrentSense);
    Serial.print("  ");
    Serial.print("     rearRightSwingOverCurrent= ");
    Serial.print(rearRightSwingOverCurrent);
    Serial.print("  ");    
    // Serial.print(" rearLeftFoldCurrent=   ");
    // Serial.print(mc51CurrentSense);
    // Serial.print("  ");
    // Serial.print("rearLeftFoldOverCurrent=   ");
    // Serial.print(rearLeftFoldOverCurrent);
    // Serial.print("  ");        
    // Serial.print("rearRightFoldCurrent= ");
    // Serial.print(mc52CurrentSense);
    // Serial.print("  ");
    // Serial.print("     rearRightFoldOverCurrent= ");
    // Serial.print(rearRightFoldOverCurrent);
    // Serial.print("  ");        
    Serial.print("rearState=  ");
    Serial.print(rearState);
    Serial.print("  ");       

    Serial.println();
    Serial.println("END");
    Serial.println(); 
    previousTime = currentTime;
  }
 } 
}