void DisabledInit(){
printf("finished %s at %f",mode.c_str(),GetTime());
leftMotor->SetControlMode(defaultMode);
rightMotor->SetControlMode(defaultMode);
leftMotor->Set(0);
rightMotor->Set(0);
}
void TeleopInit()
{
//Set slaves
l_slave->SetControlMode(CANTalon::ControlMode::kFollower);
r_slave->SetControlMode(CANTalon::ControlMode::kFollower);
l_slave->Set(1);
r_slave->Set(2);
//l_slave->SetClosedLoopOutputDirection(true);
//r_slave->SetClosedLoopOutputDirection(true);
zeroAll();
}
Robot() :
robotDrive(Motor1, Motor2), // these must be initialized in the same order
stick(5), // as they are declared above.
lw(LiveWindow::GetInstance()),
autoLoopCounter(0),
Motor1(21),
Motor2(12),
Slave1(20),
Slave2(14),
t_motor(13),
arm_Motor(23),
finger_Motor(22),
intake_Spin_Motor(11),
intake_Winch_Motor(13),
stick2(4),
autoLoopCounter2(0)
{
robotDrive.SetExpiration(0.1);
robotDrive.SetSafetyEnabled(false);
Slave1.SetControlMode(CANSpeedController::kFollower);
Slave1.Set(21);
Slave2.SetControlMode(CANSpeedController::kFollower);
Slave2.Set(12);
Motor2.SetInverted(true); //12
Slave2.SetInverted(true);//14
arm_Motor.SetInverted(false);//23
t_motor.SetInverted(true);//23
// t_motor.SetControlMode(CANSpeedController::kVoltage);
// t_motor.Set(0);
// CameraServer::GetInstance()->SetQuality(50);
// CameraServer::GetInstance()->SetSize(2);
// //the camera name (ex "cam0") can be found through the roborio web interface
// CameraServer::GetInstance()->StartAutomaticCapture("cam0");
t_motor.SetControlMode(CANSpeedController::kPercentVbus);
// t_motor.SetVoltageCompensationRampRate(24.0);
t_motor.SetFeedbackDevice(CANTalon::QuadEncoder);
t_motor.SetPosition(0);
// t_motor.SetPID(1, 0, 0);
arm_Motor.SetControlMode(CANSpeedController::kPercentVbus);
finger_Motor.SetControlMode(CANSpeedController::kPercentVbus);
// ourRangefinder = new AnalogInput(0);
}
DriveTrain(Vision* visionTracking) :
IComponent(new string("DriveTrain")),
leftDriveMaster(new CANTalon(1)),
leftDriveSlave1(new CANTalon(3)),
leftDriveSlave2(new CANTalon(5)),
rightDriveMaster(new CANTalon(2)),
rightDriveSlave1(new CANTalon(4)),
rightDriveSlave2(new CANTalon(6)),
shift(new Solenoid(4)),
vision(visionTracking),
visionPIDSource(new DrivePIDSource()),
visionPIDOutput(new DrivePIDOutput()),
visionPID(new PIDController(0.70f, 0, 0, visionPIDSource, visionPIDOutput)),
angleETC(new ErrorTimeCubed(DRIVE_ANGLE_TOLERANCE, 45.0f, -180.0f, 180.0f)),
crossTime(new Timer()),
hasCrossed(false),
crossState(0),
isClimbing(true),
driveTime(new Timer()),
timedDriveState(0),
shiftHigh(new RobotButton(RobotButton::JoystickType::PRIMARY, RobotButton::ControlTypes::KEY, JOYSTICK_BUMPER_RIGHT)),
shiftLow(new RobotButton(RobotButton::JoystickType::PRIMARY, RobotButton::ControlTypes::KEY, JOYSTICK_BUMPER_LEFT)),
stateUntoggle(new RobotButton(RobotButton::JoystickType::PRIMARY, RobotButton::ControlTypes::KEY, JOYSTICK_BACK)),
autoCrossToggle(new RobotButton(RobotButton::JoystickType::PRIMARY, NEW_JOYSTICK ? RobotButton::ControlTypes::AXIS : RobotButton::ControlTypes::KEY, JOYSTICK_TRIGGER_RIGHT)),
reverseToggle(new RobotButton(RobotButton::JoystickType::PRIMARY, RobotButton::ControlTypes::KEY, JOYSTICK_X)),
crossSpeedMultiplier(1.0f),
crossingForward(true),
leftSpeedCurrent(0),
rightSpeedCurrent(0),
targetDistance(0),
crossReverse(false),
reverse(true),
primaryDriving(false),
state(DriveState::NONE)
{
leftDriveMaster->SetControlMode(CANTalon::ControlMode::kPosition);
leftDriveMaster->SetFeedbackDevice(CANTalon::FeedbackDevice::QuadEncoder);
leftDriveMaster->ConfigEncoderCodesPerRev(1024);
leftDriveMaster->Enable();
leftDriveSlave1->SetControlMode(CANTalon::ControlMode::kFollower);
leftDriveSlave1->Enable();
leftDriveSlave1->Set(1);
leftDriveSlave2->SetControlMode(CANTalon::ControlMode::kFollower);
leftDriveSlave2->Enable();
leftDriveSlave2->Set(1);
rightDriveMaster->SetControlMode(CANTalon::ControlMode::kPosition);
rightDriveMaster->SetFeedbackDevice(CANTalon::FeedbackDevice::QuadEncoder);
leftDriveMaster->ConfigEncoderCodesPerRev(1024);
rightDriveMaster->Enable();
rightDriveSlave1->SetControlMode(CANTalon::ControlMode::kFollower);
rightDriveSlave1->Enable();
rightDriveSlave1->Set(2);
rightDriveSlave2->SetControlMode(CANTalon::ControlMode::kFollower);
rightDriveSlave2->Enable();
rightDriveSlave2->Set(2);
visionPID->SetInputRange(-1, 1);
visionPID->SetOutputRange(-1, 1);
visionPID->SetContinuous(true);
visionPID->SetAbsoluteTolerance(0.05);
visionPID->Disable();
}
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();
}