/**
* Get the throttle value of the current joystick.
* This depends on the mapping of the joystick connected to the current port.
*
* @param port The USB port for this joystick.
*/
float GetThrottle(UINT32 port)
{
Joystick *stick = getJoystick(port);
if (stick == NULL)
return 0;
return stick->GetThrottle();
}
/**
* @brief Sets a cached joystick value.
* @param joy_id Which joystick to set the cached value for.
* @param stick A Joystick object with the X, Y, and Z axes set, as well as each of the buttons.
*/
void Proxy::SetJoystick(int joy_id, Joystick & stick)
{
wpi_assert(joy_id < NUMBER_OF_JOYSTICKS+1 && joy_id >= 0);
char tmp[32];
sprintf(tmp, "Joy%d", joy_id);
string name = tmp;
if(!disableAxes[joy_id-1]) {
set(name + 'X', stick.GetX());
set(name + 'Y', stick.GetY());
set(name + 'Z', stick.GetZ());
set(name + 'R', stick.GetTwist());
set(name + 'T', stick.GetThrottle());
for(int AxisId=1; AxisId<=6; AxisId++) {
sprintf(tmp, "%sA%d", name.c_str(), AxisId);
set(tmp, stick.GetRawAxis(AxisId));
}
} else {
if(!stick.GetRawButton(disableAxes[joy_id-1])) {
set(name + 'X', stick.GetX());
set(name + 'Y', stick.GetY());
set(name + 'Z', stick.GetZ());
set(name + 'R', stick.GetTwist());
set(name + 'T', stick.GetThrottle());
for(int AxisId=1; AxisId<=6; AxisId++) {
sprintf(tmp, "%sA%d", name.c_str(), AxisId);
set(tmp, stick.GetRawAxis(AxisId));
}
}
}
if(!disableButtons[joy_id-1]) {
for(unsigned i=1;i<=NUMBER_OF_JOY_BUTTONS;i++) {
sprintf(tmp, "%sB%d", name.c_str(), i);
set(tmp,stick.GetRawButton(i));
}
set(name + "BT", stick.GetTrigger());
} else {
if(!stick.GetRawButton(disableButtons[joy_id-1])) {
for(unsigned i=1;i<=NUMBER_OF_JOY_BUTTONS;i++) {
sprintf(tmp, "%sB%d", name.c_str(), i);
set(tmp,stick.GetRawButton(i));
}
set(name + "BT", stick.GetTrigger());
}
}
}
// Called repeatedly when this Command is scheduled to run
void DriveBot::Execute() {
Joystick* stick = Robot::oi->getGamePad();
if(Robot::mover->speedSwitch->Get()){
Robot::mover->rightMotor0->Set(deadband(stick->GetThrottle()*-0.75,.01));
Robot::mover->rightMotor1->Set(deadband(stick->GetThrottle()*-0.75,.01));
Robot::mover->leftMotor2->Set(deadband(stick->GetY()*0.75,.01));
Robot::mover->leftMotor3->Set(deadband(stick->GetY()*0.75,.01));
}else{
Robot::mover->rightMotor0->Set(stick->GetThrottle()*-0.25);
Robot::mover->rightMotor1->Set(stick->GetThrottle()*-0.25);
Robot::mover->leftMotor2->Set(stick->GetY()*0.25);
Robot::mover->leftMotor3->Set(stick->GetY()*0.25);
}
}
/**
* @brief Sets a cached joystick value.
* @param joy_id Which joystick to set the cached value for.
* @param stick A Joystick object with the X, Y, and Z axes set, as well as each of the buttons.
*/
void Proxy166::SetJoystick(int joy_id, Joystick & stick)
{
wpi_assert(joy_id < NUMBER_OF_JOYSTICKS && joy_id >= 0);
//semTake(JoystickLocks[joy_id], WAIT_FOREVER);
Joysticks[joy_id].X = stick.GetX();
Joysticks[joy_id].Y = stick.GetY();
Joysticks[joy_id].Z = stick.GetZ();
Joysticks[joy_id].throttle = stick.GetThrottle();
for(unsigned i=0;i<NUMBER_OF_JOY_BUTTONS;i++) {
Joysticks[joy_id].button[i] = stick.GetRawButton(i);
}
//semGive(JoystickLocks[joy_id]);
}
void DriveControl() { //Drives la wheels of the robot
flightX = flightStick->GetRawAxis(0); //Pull joystick side motion for later use
flightY = flightStick->GetRawAxis(1); //Pull joystick forward motion for later use (forward is -, backwards is +)
flightZ = flightStick->GetRawAxis(4); //Pull joystick twist motion for later use
flightThrottle = ((((shootStick->GetThrottle() - 1)*-1)/2) * .8 + .2); //Pull throttle to modify drive variables
//Throttle value is between .2 and 1.0
if (fabs(flightX) < deadZone) { //Deaden x
flightX = 0;
}
if (fabs(flightY) < deadZone) { //Deaden y
flightY = 0;
}
if (fabs(flightZ) < deadZone) { //Deaden z
flightZ = 0;
}
if (flightStick->GetRawButton(strafeButtonChannel)){ //Set drive to strafe mode
driveMode = 0;
}
if (flightStick->GetRawButton(arcadeButtonChannel)){ //Set drive to arcade mode
driveMode = 1;
}
if (flightStick->GetRawButton(fieldButtonChannel)){ //Set drive to field-centric mode
driveMode = 2;
}
if (shootStick->GetRawButton(gyroResetChannel)){ //Reset gyro with the trigger
yawGyro->Reset();
}
flightX = flightX * flightThrottle;
flightY = flightY * flightThrottle;
flightZ = flightZ * flightThrottle;
if(driveMode == 1){
robotDrive->MecanumDrive_Cartesian(flightZ, flightY, flightX, 0);
SmartDashboard::PutString("DriveMode", "Arcade");
}
else if(driveMode == 2){
robotDrive->MecanumDrive_Cartesian(flightX, flightY, flightZ, yawGyro->GetAngle());
SmartDashboard::PutString("DriveMode", "Field");
}
else{
robotDrive->MecanumDrive_Cartesian(flightX, flightY, flightZ, 0);
SmartDashboard::PutString("DriveMode", "Strafe");
}
SmartDashboard::PutNumber("GyroAngle", yawGyro->GetAngle());
}
void OperatorControl (void) {
GetWatchdog().SetEnabled(true); // We do want Watchdog in Teleop, though.
DriverStationLCD *dsLCD = DriverStationLCD::GetInstance();
dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, " ");
dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Joystick Mode");
SmartDashboard::GetInstance()->PutData("kinectMode?", kinectModeSelector);
SmartDashboard::GetInstance()->PutData("demoMode?", demoModeSelector);
SmartDashboard::GetInstance()->PutData("speedMode?", speedModeSelector);
while (IsOperatorControl() && IsEnabled()) {
GetWatchdog().Feed(); // Feed the Watchdog.
kinectMode = (bool) kinectModeSelector->GetSelected();
demoMode = (bool) demoModeSelector->GetSelected();
speedModeMult = static_cast<double*>(speedModeSelector->GetSelected());
if (kinectMode) {
dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, " ");
dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Kinect Mode");
if (!demoMode) {
if (kinectDrive.GetRawButton(KINECT_FORWARD_BUTTON)) {
motorDriveLeft.Set(LEFT_DRIVE_POWER * *speedModeMult);
motorDriveRight.Set(RIGHT_DRIVE_POWER * -1 * *speedModeMult);
} else if (kinectDrive.GetRawButton(KINECT_REVERSE_BUTTON)) {
motorDriveLeft.Set(LEFT_DRIVE_POWER * -1 * *speedModeMult);
motorDriveRight.Set(RIGHT_DRIVE_POWER * *speedModeMult);
} else if (kinectDrive.GetRawButton(KINECT_LEFT_BUTTON)) {
motorDriveLeft.Set(LEFT_DRIVE_POWER * -1 * *speedModeMult);
motorDriveRight.Set(RIGHT_DRIVE_POWER * -1 * *speedModeMult);
} else if (kinectDrive.GetRawButton(KINECT_RIGHT_BUTTON)) {
motorDriveLeft.Set(LEFT_DRIVE_POWER * *speedModeMult);
motorDriveRight.Set(RIGHT_DRIVE_POWER * *speedModeMult);
} else {
motorDriveLeft.Set(0);
motorDriveRight.Set(0);
}
}
if (kinectManipulator.GetRawButton(KINECT_SHOOT_BUTTON)) {
motorShooter.Set(SHOOTER_MOTOR_POWER);
motorFeed.Set(FEED_MOTOR_POWER);
motorPickup.Set(PICKUP_MOTOR_POWER);
} else if (kinectManipulator.GetRawButton(KINECT_SUCK_BUTTON)) {
motorShooter.Set(SHOOTER_MOTOR_POWER * -1);
motorFeed.Set(FEED_MOTOR_POWER * -1);
motorPickup.Set(PICKUP_MOTOR_POWER * -1);
} else {
motorShooter.Set(0);
motorFeed.Set(0);
motorPickup.Set(0);
}
if (kinectManipulator.GetRawButton(KINECT_TURRET_LEFT_BUTTON)) {
motorTurret.Set(TURRET_POWER);
} else if(kinectManipulator.GetRawButton(KINECT_TURRET_RIGHT_BUTTON)) {
motorTurret.Set(TURRET_POWER * -1);
} else {
motorTurret.Set(0);
}
} else {
if (joystickDriveLeft.GetThrottle() == 0) {
dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, " ");
dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Xbox Mode");
motorDriveLeft.Set(Deadband(joystickManipulator.GetRawAxis(2) * -1 * *speedModeMult));
motorDriveRight.Set(Deadband(joystickManipulator.GetRawAxis(5) * *speedModeMult));
if (joystickManipulator.GetRawButton(XBOX_SHOOT_BUTTON) || (demoMode && joystickDriveRight.GetRawButton(XBOX_SHOOT_BUTTON))) {
motorShooter.Set(SHOOTER_MOTOR_POWER);
motorFeed.Set(FEED_MOTOR_POWER);
motorPickup.Set(PICKUP_MOTOR_POWER);
} else if (joystickManipulator.GetRawButton(XBOX_SUCK_BUTTON) || (demoMode && joystickDriveRight.GetRawButton(XBOX_SUCK_BUTTON))) {
motorShooter.Set(SHOOTER_MOTOR_POWER * -1);
motorFeed.Set(FEED_MOTOR_POWER * -1);
motorPickup.Set(PICKUP_MOTOR_POWER * -1);
} else {
motorShooter.Set(0);
motorFeed.Set(0);
motorPickup.Set(0);
}
if (joystickManipulator.GetRawAxis(3) > 0.2 || (demoMode && joystickDriveRight.GetRawAxis(3) > 0.2)) {
motorTurret.Set(TURRET_POWER);
} else if(joystickManipulator.GetRawAxis(3) < -0.2 || (demoMode && joystickDriveRight.GetRawAxis(3) < -0.2)) {
motorTurret.Set(TURRET_POWER * -1);
} else {
motorTurret.Set(0);
}
} else {
dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, " ");
dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Joystick Mode");
motorDriveLeft.Set(Deadband(joystickDriveLeft.GetY() * -1 * *speedModeMult));
motorDriveRight.Set(Deadband(joystickDriveRight.GetY() * *speedModeMult));
if (joystickManipulator.GetRawButton(MANIPULATOR_SHOOT_BUTTON)) {
motorShooter.Set(SHOOTER_MOTOR_POWER);
motorFeed.Set(FEED_MOTOR_POWER);
motorPickup.Set(PICKUP_MOTOR_POWER);
} else if (joystickManipulator.GetRawButton(MANIPULATOR_SUCK_BUTTON) || (demoMode && joystickDriveRight.GetRawButton(XBOX_SUCK_BUTTON))) {
motorShooter.Set(SHOOTER_MOTOR_POWER * -1);
motorFeed.Set(FEED_MOTOR_POWER * -1);
motorPickup.Set(PICKUP_MOTOR_POWER * -1);
} else {
motorShooter.Set(0);
motorFeed.Set(0);
motorPickup.Set(0);
}
motorTurret.Set(joystickManipulator.GetX() * -1 * TURRET_POWER);
}
}
dsLCD->UpdateLCD();
}
GetWatchdog().SetEnabled(false); // Teleop is done, so let's turn off Watchdog.
}
void TeleopPeriodic()
{
//camera->GetImage(frame);
//imaqDrawShapeOnImage(frame, frame, { 10, 10, 100, 100 }, DrawMode::IMAQ_DRAW_VALUE, ShapeMode::IMAQ_SHAPE_OVAL, 0.0f);
//CameraServer::GetInstance()->SetImage(frame);
printf("Left Encoder: %i, Right Encoder: %i, Gyro: %f\n", leftEnc->Get(), rightEnc->Get(), gyro->GetAngle());
drive->ArcadeDrive(driveStick);
drive->SetMaxOutput((1-driveStick->GetThrottle())/2);
//printf("%f\n", (1-stick->GetThrottle())/2);
//leftMotor->Set(0.1);
//rightMotor->Set(0.1);
if (shootStick->GetRawAxis(3) > 0.5) {
launch1->Set(1.0);
launch2->Set(1.0);
} else if (shootStick->GetRawAxis(2) > 0.5) {
printf("Power Counter: %i\n", powerCounter);
if (powerCounter < POWER_MAX) {
powerCounter++;
launch1->Set(-0.8);
launch2->Set(-0.8);
} else {
launch1->Set(-0.6);
launch2->Set(-0.6);
}
} else {
launch1->Set(0.0);
launch2->Set(0.0);
powerCounter = 0.0;
}
//use this button to spin only one winch, to lift up.
if (shootStick->GetRawButton(7)) {
otherWinch->Set(0.5);
} else if (shootStick->GetRawButton(8)) {
otherWinch->Set(-0.5);
} else {
otherWinch->Set(0.0);
}
if (shootStick->GetRawButton(5)) {
winch->Set(-0.7);
if (!shootStick->GetRawButton(7) && !shootStick->GetRawButton(8)) {
// otherWinch->Set(-0.5);
}
} else if (shootStick->GetRawButton(6)) {
winch->Set(0.7);
if (!shootStick->GetRawButton(7) && !shootStick->GetRawButton(8)) {
// otherWinch->Set(0.5);
}
} else {
winch->Set(0.0);
if (!shootStick->GetRawButton(7) && !shootStick->GetRawButton(8)) {
//, otherWinch->Set(0.0);
}
}
if (shootStick->GetRawButton(1)) {
launchPiston->Set(1);
} else {
launchPiston->Set(0);
}
if (shootStick->GetRawButton(3)) {
Autonomous::alignWithGoal(drive, launch1, launch2, winch, otherWinch, table, timer);
}
if (shootStick->GetRawButton(3) && debounce == false) {
debounce = true;
if (defenseUp) {
defensePiston->Set(DoubleSolenoid::Value::kReverse);
defenseUp = false;
} else {
defenseUp =true;
defensePiston->Set(DoubleSolenoid::Value::kForward);
}
} else if (!shootStick->GetRawButton(3)){
debounce = false;
}
}
void OperatorControl(void)
{
bool button1Bool;
bool button2Bool;
bool button3Bool;
bool button4Bool;
bool button5Bool;
bool button6Bool;
bool button7Bool;
bool button8Bool;
bool button9Bool;
bool button10Bool;
bool button11Bool;
bool button12Bool;
float LeftaxisYValue;
float LeftaxisXValue;
float RightaxisXValue;
float RightaxisYValue;
float TriggerValue;
myRobot.SetSafetyEnabled(true);
while (IsOperatorControl())
{
button1Bool = button1.Get();
SmartDashboard::PutNumber("button1",button1Bool);
button2Bool = button2.Get();
SmartDashboard::PutNumber("button2",button2Bool);
button3Bool = button3.Get();
SmartDashboard::PutNumber("button3",button3Bool);
button4Bool = button4.Get();
SmartDashboard::PutNumber("button4",button4Bool);
button5Bool = button5.Get();
SmartDashboard::PutNumber("button5",button5Bool);
button6Bool = button6.Get();
SmartDashboard::PutNumber("button6",button6Bool);
button7Bool = button7.Get();
SmartDashboard::PutNumber("button7",button7Bool);
button8Bool = button8.Get();
SmartDashboard::PutNumber("button8",button8Bool);
button9Bool = button9.Get();
SmartDashboard::PutNumber("button9",button9Bool);
button10Bool = button10.Get();
SmartDashboard::PutNumber("button10",button10Bool);
button11Bool = button11.Get();
SmartDashboard::PutNumber("button11",button11Bool);
button12Bool = button12.Get();
SmartDashboard::PutNumber("button12",button12Bool);
LeftaxisXValue = stick.GetX();
SmartDashboard::PutNumber("Joystick1 X Axis",LeftaxisXValue);
LeftaxisYValue = 0 - stick.GetY();
SmartDashboard::PutNumber("Joystick1 Y Axis",LeftaxisYValue);
RightaxisXValue = stick.GetTwist();
SmartDashboard::PutNumber("Joystick2 X Axis", RightaxisXValue);
RightaxisYValue = 0 - stick.GetRawAxis(5);
SmartDashboard::PutNumber("Joystick2 Y Axis", RightaxisYValue);
TriggerValue = stick.GetThrottle();
SmartDashboard::PutNumber("Trigger value", TriggerValue);
Wait(0.005); // wait for a motor update time
}
}
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();
}