2-19 timepos

timePos.c

#define MAX_HARVESTMOVE_TIME_POS 800

typedef struct MotorState {
  ... //encoder data
  long timePosMs; //stores position in "time" (50 means net movement forward for 50 ms)
  long lastUpdateTimeMs; //last time this motorstate as updated
} MotorState;

void motorUpdateState() {
  for (int i=0; i<NUM_MOTORS; i++) {
    tMotor curMotor = motorList[i];
    if (motor[curMotor] > 0) {
      long deltaTime = (nPgmTime - motorStates[curMotor].lastUpdateTimeMs);
      motorStates[curMotor].timePosMs = motorStates[curMotor].timePosMs + deltaTime;
    } else if (motor[curMotor] < 0) {
      long deltaTime = (nPgmTime - motorStates[curMotor].lastUpdateTimeMs);
      motorStates[curMotor].timePosMs = motorStates[curMotor].timePosMs - deltaTime;
    }
    ... //handle encoders
  }
}

long motorGetTimePosMs(tMotor curMotor) {
  return motorStates[curMotor].timePosMs;
}

void joyHarvester(DesiredMotorVals *desiredMotorVals, TJoystick *joyState) {
  if (joyButtonPressed(joyState, JOY2, BUTTON_X)) { //down
    //check if exceeded time limit or override is enabled
    if ((motorGetTimePosMs(HarvesterMove) < MAX_HARVESTMOVE_TIME_POS) || 
          joyButtonPressed(joyState, JOY2, BUTTON_START)) {
      desiredMotorVals->power[HarvesterMove] = 50;
    } else {
      desiredMotorVals->power[HarvesterMove] = 0;
    }
  } else if (joyButtonPressed(joyState, JOY2, BUTTON_Y)) { //up
    ... //handle rest of button inputs
  }
}