// During every loop intervel of the teleop period
void Robot::TeleopPeriodic() {
// Tank drive, both left and right joystick control their respective motor along the
// joystick's 'y' axis
//myRobot.TankDrive(-rStick.GetRawAxis(RIGHT_STICK_Y), -lStick.GetRawAxis(LEFT_STICK_Y));
//myRobot.TankDrive(-gamePad.GetRawAxis(GAMEPAD_LEFT_STICK_Y), -gamePad.GetRawAxis(GAMEPAD_RIGHT_STICK_Y));
// Choose the teleop drive option
DriverControl(driveOption);
// Edits the gyro data to account for drift
/*
editedGyroRate = gyro.GetRate();
if (gyro.GetRate() > GYRO_DRIFT_VALUE_MIN && gyro.GetRate() < GYRO_DRIFT_VALUE_MAX) {
editedGyroRate = 0;
} else {
editedGyroRate += GYRO_DRIFT_VALUE_AVERAGE;
}
*/
editedGyroAngle = gyro.GetAngle();
SmartDashboard::PutString("Operation Type:", "TeleOp");
// Print gyro data
if (showGyro == true) {
SmartDashboard::PutNumber("Gyro Angle (Raw)", gyro.GetAngle()*GYRO_SCALE_FACTOR);
SmartDashboard::PutNumber("Gyro Angle (Edited)", editedGyroAngle*GYRO_SCALE_FACTOR);
SmartDashboard::PutNumber("Gyro Rate (Raw)", gyro.GetRate()*GYRO_SCALE_FACTOR);
SmartDashboard::PutNumber("Gyro Rate (Edited)", editedGyroRate*GYRO_SCALE_FACTOR);
}
// Print out the encoder data
// Raw encoder data
if (showEncoderRaw == true) {
SmartDashboard::PutNumber("Encoder L get raw", encoder1.GetRaw());
SmartDashboard::PutNumber("Encoder R get raw", encoder2.GetRaw());
}
// The delta encoder change
if (showEncoderRate == true) {
SmartDashboard::PutNumber("Encoder L get rate", encoder1.GetRate());
SmartDashboard::PutNumber("Encoder R (reversed) get rate", encoder2.GetRate());
}
// Print the encoder index
if (showEncoderIndex == true) {
SmartDashboard::PutNumber("Encoder L index", encoder1.GetFPGAIndex());
SmartDashboard::PutNumber("Encoder R index", encoder2.GetFPGAIndex());
}
}
// During every loop intervel of the teleop period
void Robot::TeleopPeriodic() {
// Tank drive, both left and right joystick control their respective motor along the
// joystick's 'y' axis
//myRobot.TankDrive(-rStick.GetRawAxis(RIGHT_STICK_Y), -lStick.GetRawAxis(LEFT_STICK_Y));
// Choose the teleop drive option
for(int c=0;c<7;c++) {
buttonDone[c] = false;
}
DriverControl(driveOption);
if(fabs(gyro.GetRate()) > GYRO_DRIFT_VALUE) {
editedGyroRate = gyro.GetAngle();
}
else {
editedGyroRate = 0;
gyro.Reset();
}
SmartDashboard::PutString("Operation Type:", "TeleOp");
// Print gyro data
if (showGyro == true) {
SmartDashboard::PutNumber("Gyro Angle", gyro.GetAngle()*GYRO_SCALE_FACTOR);
SmartDashboard::PutNumber("Gyro Rate (Raw)", gyro.GetRate()*GYRO_SCALE_FACTOR);
SmartDashboard::PutNumber("Gyro Rate (Edited)", editedGyroRate*GYRO_SCALE_FACTOR);
}
// Print out the encoder data
// Raw encoder data
if (showEncoderRaw == true) {
SmartDashboard::PutNumber("Encoder L get raw", encoder1.GetRaw());
SmartDashboard::PutNumber("Encoder R get raw", encoder2.GetRaw());
}
// The delta encoder change
if (showEncoderRate == true) {
SmartDashboard::PutNumber("Encoder L get rate", encoder1.GetRate());
SmartDashboard::PutNumber("Encoder R (reversed) get rate", encoder2.GetRate());
}
// Print the encoder index
if (showEncoderIndex == true) {
SmartDashboard::PutNumber("Encoder L index", encoder1.GetFPGAIndex());
SmartDashboard::PutNumber("Encoder R index", encoder2.GetFPGAIndex());
}
}
// During every loop intervel of the teleop period
void Robot::TeleopPeriodic() {
// Choose the teleop drive option
DriverControl(driveOption);
// A simple thing to test a motor
if (gamePad.GetRawButton(GAMEPAD_BUTTON_A) == true)
ballManipulator.GoUp();
else
ballManipulator.StopAll();
// Edits the gyro rate to account for drift
/*
editedGyroRate = gyro.GetRate();
if (gyro.GetRate() > GYRO_DRIFT_VALUE_MIN && gyro.GetRate() < GYRO_DRIFT_VALUE_MAX) {
editedGyroRate = 0;
} else {
editedGyroRate += GYRO_DRIFT_VALUE_AVERAGE;
}
*/
// Edits the gyro angle to account for drift
if (fabs(gyro.GetRate()) > GYRO_DRIFT_VALUE)
editedGyroAngle = gyro.GetAngle();
else {
gyro.Reset();
editedGyroAngle = 0;
}
// Reset the buttons
for(int c=0; c<7; c++)
buttonDone[c] = false;
// To reset encoder data for the wheels
if (gamePad.GetRawButton(GAMEPAD_BUTTON_Y) == true) {
encoder1.Reset();
encoder2.Reset();
}
// Print gyro data
if (showGyro == true) {
SmartDashboard::PutNumber("Gyro Angle (Raw)", gyro.GetAngle()*GYRO_SCALE_FACTOR);
SmartDashboard::PutNumber("Gyro Angle (Edited)", editedGyroAngle*GYRO_SCALE_FACTOR);
SmartDashboard::PutNumber("Gyro Rate (Raw)", gyro.GetRate()*GYRO_SCALE_FACTOR);
SmartDashboard::PutNumber("Gyro Rate (Edited)", editedGyroRate*GYRO_SCALE_FACTOR);
}
// Print out the encoder data
// Raw encoder data
// if (showEncoderRaw == true) {
SmartDashboard::PutNumber("Encoder L get raw", encoder1.GetRaw());
SmartDashboard::PutNumber("Encoder R get raw", encoder2.GetRaw());
// }
// The delta encoder change
if (showEncoderRate == true) {
SmartDashboard::PutNumber("Encoder L get rate", encoder1.GetRate());
SmartDashboard::PutNumber("Encoder R (reversed) get rate", encoder2.GetRate());
}
// Print the encoder index
if (showEncoderIndex == true) {
SmartDashboard::PutNumber("Encoder L index", encoder1.GetFPGAIndex());
SmartDashboard::PutNumber("Encoder R index", encoder2.GetFPGAIndex());
}
}