void liftHeight(int height)
{
int goingUp = height > currentHeight() ? 1 : -1;
setLift(liftAutoSpeed * goingUp);
// The goingUp variable makes it so that if it's 60->10, it waits until -10 < -60
while (currentHeight() * goingUp < height * goingUp)
{
// Wait
}
setLift(0);
}
task usercontrol() {
int stick[4],
drivePwrs;
signed char liftTargCt = 0,
liftTargCtLast;
const signed char stickThresh = 7;
while (true) {
for(int i = 0; i < 4; i++)
stick[i] =
(fabs(vexRT[i]) >= stickThresh)
? vexRT[i]
: 0;
upToggle(&liftUpToggle, LIFT_UP_BTN);
upToggle(&liftDwnToggle, LIFT_DWN_BTN);
liftTargCtLast = liftTargCt;
liftTargCt += (pressToggle(&liftUpToggle, LIFT_UP_BTN) ^ pressToggle(&liftDwnToggle, LIFT_DWN_BTN))
? (pressToggle(&liftUpToggle, LIFT_UP_BTN))
? 1
: -1
: 0;
if(liftTargCt > 2)
liftTargCt = 0;
else if(liftTargCt < 0)
liftTargCt = 2;
setDrive(arcade(&drivePwrs, stick[LY], stick[RX]));
if(liftTargCt != liftTargCtLast)
setPid(&liftPid, liftSetPts[liftTargCt]);
setLift(upPid(&liftPid));
setClaw((CLAW_OPEN_BTN ^ CLAW_CLOSE_BTN)
? (CLAW_OPEN_BTN)
? 127
: -127
: 0);
wait1Msec(20);
}
}
