// 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());
}
}
// When the robot is on autonomous period, it will drive forwards at half speed for about two
// seconds, then stops
void Robot::AutonomousPeriodic() {
/*
if(autoLoopCounter < 100) { //Check if we've completed 100 loops (approximately 2 seconds)
myRobot.Drive(-0.5, 0.0); // drive forwards half speed
autoLoopCounter++;
} else {
myRobot.Drive(0.0, 0.0); // stop robot
}
*/
// Correct course (adjust for physical motor imperfections) based on encoders
// Print the raw encoder data
SmartDashboard::PutNumber("Encoder L get raw", encoder1.GetRaw());
SmartDashboard::PutNumber("Encoder R get raw", encoder2.GetRaw());
}
// When the robot is on autonomous period, it will drive forwards at half speed for about two
// seconds, then stops
void Robot::AutonomousPeriodic() {
/*
if(autoLoopCounter < 100) { //Check if we've completed 100 loops (approximately 2 seconds)
myRobot.Drive(-0.5, 0.0); // drive forwards half speed
autoLoopCounter++;
} else {
myRobot.Drive(0.0, 0.0); // stop robot
}
*/
// Edits the gyro angle to account for drift
if (fabs(gyro.GetRate()) > GYRO_DRIFT_VALUE)
editedGyroAngle = gyro.GetAngle();
else {
gyro.Reset();
editedGyroAngle = 0;
}
// Adjust course based on gyro data
if (editedGyroAngle > 0)
autoTurn = 0.3;
else if (editedGyroAngle < 0)
autoTurn = -0.3;
else
autoTurn = 0;
// Go foward 3 feet at 1/3 speed, stop (adjusting for drift)
if (encoder1.GetRaw() < ONE_FOOT_LEFT_ENCODER*3 || encoder2.GetRaw() < ONE_FOOT_RIGHT_ENCODER*3)
myRobot.Drive(0.3, autoTurn);
else
myRobot.Drive(0.0, 0.0);
// Print the raw encoder data
SmartDashboard::PutNumber("Encoder L get raw", encoder1.GetRaw());
SmartDashboard::PutNumber("Encoder R get raw", encoder2.GetRaw());
}
void OperatorControl(void) {
char count=0;
double target = 0, speed = 0;
while(!IsDisabled()) {
double tmpStick = -1*stick.GetRawAxis(2);
if(tmpStick < .2 && tmpStick > -.2) tmpStick=0;
target += tmpStick*1.5;
int location = enc.GetRaw();
if(stick.GetRawButton(5)) {
up.Set(true);
down.Set(false);
}else if(stick.GetRawButton(7) && location > 0) {
down.Set(true);
up.Set(false);
}else if(stick.GetRawButton(8)) {
down.Set(true);
up.Set(false);
}else if(stick.GetRawButton(9)){
speed = pid(target, location);
if(speed > 1) {
up.Set(true);
down.Set(false);
}else if(speed < -1) {
up.Set(false);
down.Set(true);
}else{
up.Set(false);
down.Set(false);
}
}else if(stick.GetRawButton(10)) {
enc.Reset();
}else{
up.Set(false);
down.Set(false);
}
if(stick.GetRawButton(1))
target = 2;
if(stick.GetRawButton(4))
target = 400;
if(stick.GetRawButton(3))
target = 200;
if(stick.GetRawButton(2))
target = 70;
Wait(.02);
while(count++%30==0) cerr << location << '\t' << target << '\t' << speed << endl;
}
}
/**
* Periodic code for teleop mode should go here.
*
* Use this method for code which will be called periodically at a regular
* rate while the robot is in teleop mode.
*/
void RobotDemo::TeleopPeriodic()
{
m_robotDrive.MecanumDrive_Cartesian(m_driveStick.GetX(), m_driveStick.GetY(), m_driveStick2.GetX(), m_gyro.GetAngle());
printf("rate: %d\n", (int) m_encoder.GetRaw());
}
// 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());
}
}
/**
* Runs the motor from the output of a Joystick.
*/
void OperatorControl() {
LifterEncoder.StartLiveWindowMode();
LifterEncoder.Reset();
while (IsOperatorControl() && IsEnabled()) {
// Set the motor controller's output.
// This takes a number from -1 (100% speed in reverse) to +1 (100% speed forwards).
// lifterA_motor.Set(joy.GetY());
// lifterB_motor.Set(joy.GetY());
if ( stick.GetRawButton(1))
{
}
else if (stick.GetRawButton(2))
{
}
else if (stick.GetRawButton(3))
{
// Elevator down
if (BottomLimitSwitch.Get() == 0) {
lifterA_motor.Set(0.5);
lifterB_motor.Set(0.5);
} else {
lifterA_motor.Set(0);
lifterB_motor.Set(0);
}
}
else if (stick.GetRawButton(4))
{
}
else if (stick.GetRawButton(5))
{
// Elevator up
if (TopLimitSwitch.Get() == 0) {
lifterA_motor.Set(-0.5);
lifterB_motor.Set(-0.5);
} else {
lifterA_motor.Set(0);
lifterB_motor.Set(0);
}
}
else if (stick.GetRawButton(6))
{
}
else if (stick.GetRawButton(7))
{
}
else if (stick.GetRawButton(8))
{
}
else if (stick.GetRawButton(9))
{
}
else if (stick.GetRawButton(10))
{
}
else if (stick.GetRawButton(11))
{
}
else if (stick.GetRawButton(12))
{
}
else
{
lifterA_motor.Set(0.0);
lifterB_motor.Set(0.0);
}
if(BottomLimitSwitch.Get()==true)
{
LifterEncoder.Reset();
}
// Send some stuff to the dashboard
SmartDashboard::PutBoolean("Top Limit Switch", TopLimitSwitch.Get());
SmartDashboard::PutBoolean("Bottom Limit Switch", BottomLimitSwitch.Get());
SmartDashboard::PutNumber("Encoder Position",LifterEncoder.GetRaw());
Wait(kUpdatePeriod); // Wait 5ms for the next update.
}
}
