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