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); } }