When i enable the control mode in the turner for testing it works the light truns green and red if going forward or backward, but when i test it with the driver station it doesn’t enable properly.
Just in case here is my code:package frc.robot;
import com.ctre.phoenix.motorcontrol.ControlMode;
import com.ctre.phoenix.motorcontrol.NeutralMode;
import com.ctre.phoenix.motorcontrol.can.TalonSRX;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.Servo;
import edu.wpi.first.wpilibj.TimedRobot;
/**
- The VM is configured to automatically run this class, and to call the functions corresponding to
- each mode, as described in the TimedRobot documentation. If you change the name of this class or
- the package after creating this project, you must also update the build.gradle file in the
- project.
/
public class Robot extends TimedRobot {
/*- This function is run when the robot is first started up and should be used for any
- initialization code.
*/
WPI_TalonFX motorRight1 = new WPI_TalonFX(0);
WPI_TalonFX motorRight2 = new WPI_TalonFX(1);
WPI_TalonFX motorRight3 = new WPI_TalonFX(2);
MotorControllerGroup groupRight = new MotorControllerGroup(motorRight1, motorRight2, motorRight3);
WPI_TalonFX motorLeft1 = new WPI_TalonFX(4);
WPI_TalonFX motorLeft2 = new WPI_TalonFX(3);
WPI_TalonFX motorLeft3 = new WPI_TalonFX(5);
MotorControllerGroup groupLeft = new MotorControllerGroup(motorLeft1, motorLeft2, motorLeft3);
// Store the original servo position in a variable
double originalPosition;
TalonSRX window;
Servo Servo;
//Controller
Joystick gamePad;
@Override
public void robotInit()
{
window = new TalonSRX(10);
Servo = new Servo(7);
gamePad = new Joystick(0);
originalPosition = Servo.getAngle();
//Inverted setting
groupRight.setInverted(true);
groupLeft.setInverted(false);
motorLeft1.setNeutralMode(NeutralMode.Brake);
motorLeft2.setNeutralMode(NeutralMode.Brake);
motorLeft3.setNeutralMode(NeutralMode.Brake);
motorRight1.setNeutralMode(NeutralMode.Brake);
motorRight2.setNeutralMode(NeutralMode.Brake);
motorRight3.setNeutralMode(NeutralMode.Brake);
window.setNeutralMode(NeutralMode.Brake);
}
@Override
public void robotPeriodic() {}
@Override
public void autonomousInit() {}
@Override
public void autonomousPeriodic() {}
@Override
public void teleopInit() {}
@Override
public void teleopPeriodic() {
if(gamePad.getRawButton(5)){
window.set(ControlMode.PercentOutput, 1.0);
}
else if (gamePad.getRawButton(6)){
window.set(ControlMode.PercentOutput, -1.0);
}
else {
window.set(ControlMode.PercentOutput, 0);
}
if (gamePad.getRawButton(1)) {
// Rotate servo to angle
Servo.setAngle(90.0);
} else {
// Set servo angle to the original position
Servo.setAngle(originalPosition);
}
// Spin control - using two joysticks
double leftStickY = -gamePad.getRawAxis(4);
double rightStickY = -gamePad.getRawAxis(4);
// check if left joystick is held down and right joystick is up
if (leftStickY < -0.5 && rightStickY > 0.5) {
groupLeft.set(0.5);
groupRight.set(-0.5);
}
// check if left joystick is up and right joystick is down
else if (leftStickY > 0.5 && rightStickY < -0.5) {
groupLeft.set(-0.5);
groupRight.set(0.5);
} else {
// Drive control - Arcade Drive
double moveSpeed = -leftStickY; // inverted for proper control direction
double turnSpeed = gamePad.getRawAxis(4);
// adjust move speed for better control at lower speeds
moveSpeed = Math.copySign(Math.pow(moveSpeed, 2.0), moveSpeed);
// combine move and turn speeds
double leftSpeed = moveSpeed + turnSpeed;
double rightSpeed = moveSpeed - turnSpeed;
// limit motor speed to maximum allowed value
double maxSpeed = 0.8;
leftSpeed = Math.copySign(Math.min(Math.abs(leftSpeed), maxSpeed), leftSpeed);
rightSpeed = Math.copySign(Math.min(Math.abs(rightSpeed), maxSpeed), rightSpeed);
// set motor speeds
groupLeft.set(leftSpeed);
groupRight.set(rightSpeed);
}
}
@Override
public void disabledInit()
{
groupLeft.stopMotor();
groupRight.stopMotor();
// Set servo angle to the original position
Servo.setAngle(originalPosition);
}
@Override
public void disabledPeriodic() {}
@Override
public void simulationInit() {}
@Override
public void simulationPeriodic() {}
}