Hey everyone,
My team has been trying to get our robot to drive our robot straight but there seems to be an issue with talons. A few(random ones) lose communications while driving. As a result, the robot drives at an angle, and never straight.
I have updated the talons to the latest firmware(4.1) and configured them to the right ids.
The robot runs on 3 mini cims fyi.
Although the issue is there when driving forwards and backwards, we noticed that it’s more laggy when driving forwards.
Does anyone know what the problem is?
Heres my drivetrain and joystick command code:
public DriveTrain() {
leftTalon1 = new TalonSRX(RobotMap.leftTalon1);
leftTalon2 = new TalonSRX(RobotMap.leftTalon2);
leftTalon3 = new TalonSRX(RobotMap.leftTalon3);
rightTalon1 = new TalonSRX(RobotMap.rightTalon1);
rightTalon2 = new TalonSRX(RobotMap.rightTalon2);
rightTalon3 = new TalonSRX(RobotMap.rightTalon3);
leftTalon1.setInverted(true);
leftTalon2.setInverted(true);
leftTalon3.setInverted(true);
}
public void initDefaultCommand() {
// Set the default command for a subsystem here.
// setDefaultCommand(new MySpecialCommand());
setDefaultCommand(new JoystickCommand());
}
public void drive(ControlMode mode, double left, double right) {
leftTalon1.set(mode, left);
leftTalon2.set(mode, left);
leftTalon3.set(mode, left);
rightTalon1.set(mode, right);
rightTalon2.set(mode, right);
rightTalon3.set(mode, right);
}
//joystick code:
protected void execute() {
double turn = -1 * OI.joystick2.getRawAxis(0);
turn = Math.abs(turn) < .05 ? 0 : turn;
System.out.println("turn: " + turn);
double throttle = OI.joystick3.getRawAxis(2) - OI.joystick3.getRawAxis(3);
//throttle = -1 * OI.joystick.getRawAxis(1);
throttle = Math.abs(throttle) < .1 ? 0 : throttle;
drive.drive(ControlMode.PercentOutput, sig(throttle - cubeRoot(turn)), sig(throttle + cubeRoot(turn)));
SmartDashboard.putNumber("left throttle", sig(throttle - cubeRoot(turn)));
SmartDashboard.putNumber("right throttle", sig(throttle + cubeRoot(turn)));
}
public double cubeRoot(double val) {
if (val >= 0) {
return Math.pow(val, 3 / 2d);
} else {
return -Math.pow(-val, 3 / 2d);
}
}
public double sig(double val) {
return 2 / (1 + Math.pow(Math.E, -7 * Math.pow(val, 3))) - 1;
}