// Start auto mode
void AutonomousInit() override {
autoSelected = *((int*) chooser.GetSelected()); // autonomous mode chosen in dashboard
currentState = 1;
ahrs->ZeroYaw();
ahrs->GetFusedHeading();
autoLength = SmartDashboard::GetNumber(AUTO_LENGTH, AUTO_LENGTH_DEFAULT);
autoSpeed = SmartDashboard::GetNumber(AUTO_SPEED, AUTO_SPEED_DEFAULT);
autoIntakeSpeed = SmartDashboard::GetNumber(AUTO_INTAKE_SPEED, AUTO_INTAKE_SPEED_DEFAULT);
liftdown->Set(false);
}
void AutonomousGyroTurn() {
switch (currentState) {
case 1:
timer->Reset();
timer->Start();
//State: Stopped
//Transition: Driving State
currentState = 2;
break;
case 2:
//State: Driving
//Stay in State until 2 seconds have passed--`
//Transition: Gyroturn State
drive->TankDrive(0.5, 0.5);
if (timer->Get() >= 1) {
drive->TankDrive(0.0, 0.0);
ahrs->ZeroYaw();
currentState = 3;
turnController->SetSetpoint(90);
turnController->Enable();
}
break;
case 3:
//State: Gyroturn
//Stay in state until navx yaw has reached 90 degrees
//Transition: Driving State
drive->TankDrive(0.5 * rotateRate, -0.5 * rotateRate);
// if (ahrs->GetYaw() >= 90) {
if (turnController->OnTarget()) {
drive->TankDrive(0.0, 0.0);
currentState = 4;
timer->Reset();
timer->Start();
}
break;
case 4:
//State:Driving
//Stay in state until 2 seconds have passed
//Transition: State Stopped
drive->TankDrive(0.5, 0.5);
if (timer->Get() >= 1) {
currentState = 5;
timer->Stop();
}
break;
case 5:
//State: Stopped
drive->TankDrive(0.0, 0.0);
break;
}
}
/**
* Runs the motors with Mecanum drive.
*/
void OperatorControl()
{
robotDrive.SetSafetyEnabled(false);
while (IsOperatorControl() && IsEnabled())
{
bool reset_yaw_button_pressed = stick.GetRawButton(1);
if ( reset_yaw_button_pressed ) {
ahrs->ZeroYaw();
}
bool rotateToAngle = false;
if ( stick.GetRawButton(2)) {
turnController->SetSetpoint(0.0f);
rotateToAngle = true;
} else if ( stick.GetRawButton(3)) {
turnController->SetSetpoint(90.0f);
rotateToAngle = true;
} else if ( stick.GetRawButton(4)) {
turnController->SetSetpoint(179.9f);
rotateToAngle = true;
} else if ( stick.GetRawButton(5)) {
turnController->SetSetpoint(-90.0f);
rotateToAngle = true;
}
double currentRotationRate;
if ( rotateToAngle ) {
turnController->Enable();
currentRotationRate = rotateToAngleRate;
} else {
turnController->Disable();
currentRotationRate = stick.GetTwist();
}
try {
/* Use the joystick X axis for lateral movement, */
/* Y axis for forward movement, and the current */
/* calculated rotation rate (or joystick Z axis), */
/* depending upon whether "rotate to angle" is active. */
robotDrive.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(),
currentRotationRate ,ahrs->GetAngle());
} catch (std::exception ex ) {
std::string err_string = "Error communicating with Drive System: ";
err_string += ex.what();
DriverStation::ReportError(err_string.c_str());
}
Wait(0.005); // wait 5ms to avoid hogging CPU cycles
}
}
void RobotInit() override {
lw = LiveWindow::GetInstance();
drive->SetExpiration(20000);
drive->SetSafetyEnabled(false);
//Gyroscope stuff
try {
/* Communicate w/navX-MXP via the MXP SPI Bus. */
/* Alternatively: I2C::Port::kMXP, SerialPort::Port::kMXP or SerialPort::Port::kUSB */
/* See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface/ for details. */
ahrs = new AHRS(SPI::Port::kMXP);
} catch (std::exception ex) {
std::string err_string = "Error instantiating navX-MXP: ";
err_string += ex.what();
//DriverStation::ReportError(err_string.c_str());
}
if (ahrs) {
LiveWindow::GetInstance()->AddSensor("IMU", "Gyro", ahrs);
ahrs->ZeroYaw();
// Kp Ki Kd Kf PIDSource PIDoutput
turnController = new PIDController(0.015f, 0.003f, 0.100f, 0.00f,
ahrs, this);
turnController->SetInputRange(-180.0f, 180.0f);
turnController->SetOutputRange(-1.0, 1.0);
turnController->SetAbsoluteTolerance(2); //tolerance in degrees
turnController->SetContinuous(true);
}
chooser.AddDefault("No Auto", new int(0));
chooser.AddObject("GyroTest Auto", new int(1));
chooser.AddObject("Spy Auto", new int(2));
chooser.AddObject("Low Bar Auto", new int(3));
chooser.AddObject("Straight Spy Auto", new int(4));
chooser.AddObject("Adjustable Straight Auto", new int(5));
SmartDashboard::PutNumber(AUTO_LENGTH, AUTO_LENGTH_DEFAULT);
SmartDashboard::PutNumber(AUTO_SPEED, AUTO_SPEED_DEFAULT);
SmartDashboard::PutNumber(AUTO_INTAKE_SPEED, AUTO_INTAKE_SPEED_DEFAULT);
SmartDashboard::PutData("Auto Modes", &chooser);
liftdown->Set(false);
comp->Start();
}
void TeleopPeriodic() override {
float leftPower, rightPower; // Get the values for the main drive train joystick controllers
leftPower = -leftjoystick->GetY();
rightPower = -rightjoystick->GetY();
float multiplier; // TURBO mode
if (rightjoystick->GetRawButton(1))
{
multiplier = 1;
} else {
multiplier = 0.5;
}
// wtf is a setpoint - it's an angle to turn to
if (leftjoystick->GetRawButton(6)) {
turnController->Reset();
turnController->SetSetpoint(0);
turnController->Enable();
ahrs->ZeroYaw();
//ahrs->Reset();
}
// Press button to auto calculate angle to rotate bot to nearest ball
// if(leftjoystick->GetRawButton(99))
// {
// ahrs->ZeroYaw();
// turnController->Reset();
// turnController->SetSetpoint(mqServer.GetDouble("angle"));
// turnController->Enable();
// aimState = 1;
// }
switch(aimState)
{
default:
case 0: // No camera assisted turning
//Drive straight with one controller, else: drive with two controllers
if (leftjoystick->GetRawButton(1)) {
drive->TankDrive(leftPower * multiplier, leftPower * multiplier,
false);
} else if (leftjoystick->GetRawButton(2)) {
drive->TankDrive(leftPower * multiplier + rotateRate,
leftPower * multiplier + -rotateRate, false);
} else {
drive->TankDrive(leftPower * multiplier, rightPower * multiplier,
false);
}
break;
case 1: // Camera assisted turning, deny input from controllers
drive->TankDrive(rotateRate, -rotateRate, false);
if(turnController->OnTarget() || leftjoystick->GetRawButton(97)) {
aimState = 0; // Finished turning, auto assist off
turnController->Disable();
turnController->Reset();
}
break;
}
// That little flap at the bottom of the joystick
float scaleIntake = (1 - (controlstick->GetThrottle() + 1) / 2);
// Depending on the button, our intake will eat or shoot the ball
if (controlstick->GetRawButton(2)) {
intake->Set(-scaleIntake);
shooter->Set(scaleIntake);
} else if (controlstick->GetRawButton(1)) {
intake->Set(scaleIntake);
shooter->Set(-scaleIntake);
} else {
intake->Set(0);
shooter->Set(0);
}
// Control the motor that lifts and descends the intake bar
float intake_lever_power = 0;
if (controlstick->GetRawButton(6)) {
manual = true;
intake_lever_power = .3;
// intakeLever->Set(.30); // close
} else if (controlstick->GetRawButton(4)) {
manual = true;
intake_lever_power = -.4;
// intakeLever->Set(-.40); // open
} else if (controlstick->GetRawButton(3)){
manual = true;
intake_lever_power = -scaleIntake;
// intakeLever->Set(-scaleIntake);
} else if (controlstick->GetRawButton(5)) {
manual = true;
intake_lever_power = scaleIntake;
// intakeLever->Set(scaleIntake);
} else {
if (manual) {
manual = false;
lastLiftPos = intakeLever->GetEncPosition();
intakeLever->SetControlMode(CANTalon::ControlMode::kPosition);
intakeLever->SetFeedbackDevice(CANTalon::FeedbackDevice::QuadEncoder);
intakeLever->SetPID(1, 0.001, 0.0);
intakeLever->EnableControl();
}
intake_hold = true;
intakeLever->Set(lastLiftPos);
}
if (manual) {
intake_hold = false;
intakeLever->SetControlMode(CANTalon::ControlMode::kPercentVbus);
intakeLever->Set(intake_lever_power);
}
if (controlstick->GetRawButton(11)) {
lift->Set(true);
liftdown->Set(false);
} else if (controlstick->GetRawButton(12)){
lift->Set(false);
liftdown->Set(true);
} else if (controlstick->GetRawButton(7)) {
liftdown->Set(false);
}
if (controlstick->GetRawButton(9)) {
winch->Set(scaleIntake);
} else if (controlstick->GetRawButton(10)) {
winch->Set(-scaleIntake);
} else {
winch->Set(0);
}
if (controlstick->GetPOV() == 0 && !bounce ) {
constantLift -= 0.05;
bounce = true;
} else if (controlstick->GetPOV() == 180 && !bounce) {
constantLift += 0.05;
bounce = true;
} else if (controlstick->GetPOV() == 270 && !bounce) {
constantLift = 0;
bounce = true;
} else {
bounce = false;
}
UpdateDashboard();
}
void AutonomousSpy() {
// Strategy 1 - start as spy with a boulder, score in lower goal. Starts with intake facing low goal
// -------------------------------------------------------------------------------------------------------------------
switch (currentState) {
case 1:
// -State: stopped
timer->Reset();
timer->Start();
ahrs->ZeroYaw();
currentState = 2;
break;
// --transition: state Driving Forward
case 2:
// -State: Driving Forward
// --wait until lined up with low goal
// --transition: State stopped
drive->TankDrive(0.5, 0.5);
if (timer->Get() >= 1) { // NEEDS TO BE SET
// -State: stopped
// --wait until stopped
drive->TankDrive(0.0, 0.0);
currentState = 3;
timer->Reset();
timer->Start();
}
break;
// --transition: State Shooting
case 3:
// -State: Shooting
// --wait until shooting complete
intake->Set(-.5);
if (timer->Get() >= .7) { //Find Out Actual Time
intake->Set(0);
timer->Reset();
timer->Start();
currentState = 4;
}
break;
// --transition: State Backing Up
case 4:
// -State: Backing Up
// --wait until off tower ramp
drive->TankDrive(-0.5, -0.5);
if (timer->Get() > 1) {
drive->TankDrive(0.0, 0.0);
ahrs->ZeroYaw();
ahrs->Reset();
currentState = 5;
turnController->SetSetpoint(-65.5);
turnController->Enable();
}
break;
// --transition: Turning
case 5:
// -State: Turning Left
// --wait until 65 degrees has been reached to line up with low bar
drive->TankDrive(-0.5, 0.5);
if (turnController->OnTarget()) {
drive->TankDrive(0.0, 0.0);
timer->Reset();
timer->Start();
currentState = 6;
}
break;
// --transition: Backing Up
case 6:
// -State backing Up
// --wait until near guard wall
drive->TankDrive(-0.5, -0.5);
if (timer->Get() >= 1) {
drive->TankDrive(0.0, 0.0);
ahrs->ZeroYaw();
ahrs->Reset();
currentState = 7;
turnController->SetSetpoint(-24.5);
turnController->Enable();
}
break;
// --transition: Turn Left
case 7:
// -State: Turn Right
// --wait until 25 degree turn has been made to line with low bar
drive->TankDrive(-0.5, 0.5);
if (turnController->OnTarget()) {
drive->TankDrive(0.0, 0.0);
timer->Reset();
timer->Start();
currentState = 8;
}
break;
// --transition: Back Up
case 8:
// -State: Backing Up
// --wait until backed through low bar
drive->TankDrive(-0.5, -0.5);
if (timer->Get() >= 1) { // NeedTo Update Value
timer->Stop();
currentState = 9;
}
break;
// --transition: Stopped
case 9:
// -State: Stopped
drive->TankDrive(0.0, 0.0);
break;
}
}