LiftPreset2-18

LiftPreset2-18.c

#define LIFT_UP_POW -100
#define ENC_SIGN sgn(LIFT_UP_POW)

#define LIFT_MAX ENC_SIGN * 9800 //high goal
#define NINETY_GOAL ENC_SIGN * 7400
#define SIXTY_GOAL ENC_SIGN * 4600
#define THIRTY_GOAL ENC_SIGN * 2000
#define LIFT_MIN 0

void joyLift(DesiredMotorVals *desiredMotorVals, DesiredEncVals *desiredEncVals, TJoystick *joyState){
  long encTarget = ENC_OFF; //turn off targeting by default
  if (joyButtonPressed(joyState, JOY2, BUTTON_RB)) { //raise
    desiredMotorVals->power[Lift] = LIFT_UP_POW;
    encTarget = LIFT_MAX;
  } else if (joyButtonPressed(joyState, JOY2, BUTTON_LB)) { //lower
    desiredMotorVals->power[Lift] = -1 * LIFT_UP_POW;
    encTarget = LIFT_MIN;
  } else {
    if (joyGetTophat(joyState, JOY2) == TOPHAT_N) { //high goal
      if (abs(motorGetEncoder(Lift)) > abs(NINETY_GOAL)) {
        desiredMotorVals->power[Lift] = -1 * LIFT_UP_POW;
      } else {
        desiredMotorVals->power[Lift] = LIFT_UP_POW;
      }
      encTarget = NINETY_GOAL;
    } else if (joyGetTophat(joyState, JOY2) == TOPHAT_E) { //med goal
      ... // handle rest of the preset cases
    }
  }
  motorSetEncoder(desiredEncVals, Lift, encTarget);
}