void driveRobot(int direction, int speed, int amount) { int initialEncoder = nMotorEncoder[rDriveFront]; int encoderGoal; if (direction == 1) { //forward encoderGoal = initialEncoder + amount; //forwards means we're counting up while (nMotorEncoder[rDriveFront] <= encoderGoal) { setDriveTrainMotors(direction*speed); } } else if (direction == -1) { //backwards encoderGoal = initialEncoder - amount; //backwards means we're counting down while (nMotorEncoder[rDriveFront] >= encoderGoal) { setDriveTrainMotors(direction*speed); } } setDriveTrainMotors(0); }
void startIOTask(void *ignore) { while(1) { setDriveTrainMotors(driveTrainStyle); motorSet(ARMLeft.port, ARMLeft.out * ARMLeft.reflected); motorSet(ARMRight.port, ARMRight.out * ARMRight.reflected); motorSet(ARMTopLeft.port, ARMTopLeft.out * ARMTopLeft.reflected); motorSet(ARMTopRight.port, ARMTopRight.out * ARMTopRight.reflected); motorSet(ARMBottomLeft.port, ARMBottomLeft.out * ARMBottomLeft.reflected); motorSet(ARMBottomRight.port, ARMBottomRight.out * ARMBottomRight.reflected); if(useIMEs) { imeGet(IMELEFT, &encVals[0]); imeGet(IMERIGHT, &encVals[1]); } else { encVals[0] = encoderGet(encLeft); encVals[1] = encoderGet(encRight); } gyroVal = gyroGet(gyro); delay(10); } }