task displayMyMotorPositions(){
float clawEncoderValue, wristEncoderValue, armEncoderValue, calibratedGyroDegrees, calibratedGyroHeading;
float leftMotorValue, rightMotorValue;
while(true) {
calibratedGyroDegrees = getGyroDegreesFloat(gyro);
calibratedGyroHeading = getGyroHeadingFloat(gyro);
clawEncoderValue = getMotorEncoder(clawMotor);
wristEncoderValue = getMotorEncoder(wristMotor);
armEncoderValue = getMotorEncoder(armMotor);
leftMotorValue = getMotorEncoder(leftMotor);
rightMotorValue = getMotorEncoder(rightMotor);
eraseDisplay();
displayString(0, "claw: %f3", clawEncoderValue);
displayString(1, "wrist: %f3", wristEncoderValue);
displayString(2, "arm: %f3", armEncoderValue);
displayString(3, "L: %f3 R: %f3", leftMotorValue, rightMotorValue);
displayString(4, "d: %f3 h: %f3", calibratedGyroDegrees, calibratedGyroHeading);
wait1Msec(1000);
}
}
/// rotate with counterclockwise (left) as positive
void rotate(int degrees, bool reset){
if(reset)
resetGyro(gyro);
float kP = 1;
if(degrees > 0){
// counterclockwise turn
while(getGyroHeading(gyro) < degrees){
setMotorSpeed(leftMotor, -proportionalOutput(kP, degrees, getGyroDegreesFloat(gyro)));
setMotorSpeed(rightMotor, proportionalOutput(kP, degrees, getGyroDegreesFloat(gyro)));
}
} else {
while(getGyroHeading(gyro) > degrees){
setMotorSpeed(leftMotor, -proportionalOutput(kP, degrees, getGyroDegreesFloat(gyro)));
setMotorSpeed(rightMotor, proportionalOutput(kP, degrees, getGyroDegreesFloat(gyro)));
}
}
setMotorSpeed(leftMotor, 0);
setMotorSpeed(rightMotor, 0);
sleep(200);
}
/// drive straight using the gyro
void gyroDriveInches(float inches, int driveSpeed) {
inches = inches-2;
float kP = 2.5;
bool onTarget = false;
resetMotorEncoder(leftMotor);
resetMotorEncoder(rightMotor);
resetGyro(gyro);
while(!onTarget) {
if(getMotorEncoder(leftMotor) < inches/7.8) {
setMotorSpeed(leftMotor, driveSpeed + proportionalOutput(kP, 0, getGyroDegreesFloat(gyro)));
}else {
onTarget = true;
}
if(getMotorEncoder(rightMotor) < inches/7.8) {
setMotorSpeed(rightMotor, driveSpeed - proportionalOutput(kP, 0, getGyroDegreesFloat(gyro)));
}
}
setMotorSpeed(leftMotor, 0);
setMotorSpeed(rightMotor, 0);
sleep(200);
}
