Hi My driving system is stuck in brake mode and won't come out?

Hi My driving system is stuck in brake mode and won’t come out? And " ERROR  1  DifferentialDrive… Output not updated often enough. See Using the WPILib Classes to Drive your Robot — FIRST Robotics Competition documentation for more information.  edu.wpi.first.wpilibj.MotorSafety.check(MotorSafety. java:139) 
" I get an error. What should I do?

2 Likes
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.

package frc.robot.subsystems;

import java.util.List;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.kinematics.DifferentialDriveOdometry;
import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
import edu.wpi.first.math.trajectory.Trajectory;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.networktables.GenericEntry;
import edu.wpi.first.wpilibj.ADXRS450_Gyro;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.interfaces.Gyro;
import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants;
import frc.robot.Robot;
import frc.robot.RobotContainer;

import com.ctre.phoenix.motorcontrol.ControlMode;
import com.ctre.phoenix.motorcontrol.NeutralMode;
import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import com.kauailabs.navx.frc.AHRS;
import com.pathplanner.lib.PathPlanner;
import com.pathplanner.lib.PathPlannerTrajectory;



public class DriveTrain extends SubsystemBase {


  //True = Arcade
  //False = Tank

  // CANSparkMax leftMotor = new CANSparkMax(Constants.leftMotor, MotorType.kBrushless);
  // CANSparkMax rightMotor = new CANSparkMax(Constants.rightMotor, MotorType.kBrushless);

  // Create our motor instances
  static WPI_TalonFX leftM = new WPI_TalonFX(Constants.DriveConstants.LeftMaster);
  static WPI_TalonFX leftS = new WPI_TalonFX(Constants.DriveConstants.LeftSlave);
  static WPI_TalonFX rightM = new WPI_TalonFX(Constants.DriveConstants.RightMaster);
  static WPI_TalonFX rightS = new WPI_TalonFX(Constants.DriveConstants.RightSlave);

  // Create motor control groups so it's easier to manage
  MotorControllerGroup leftSideDrive = new MotorControllerGroup(leftM, leftS);
  MotorControllerGroup rightSideDrive = new MotorControllerGroup(rightM, rightS);

DifferentialDrive differentialDrive = new DifferentialDrive(leftSideDrive, rightSideDrive);
  static DifferentialDrive diffDrive;
  public static AHRS NAVX = new AHRS(edu.wpi.first.wpilibj.SerialPort.Port.kUSB1);
  public static ADXRS450_Gyro g_gyro = new ADXRS450_Gyro();
  private final DifferentialDriveOdometry m_odometry;
  public Field2d m_field = new Field2d();

  
  
  public static double percentOutput = 0.6; // This variable controls the percent output
  public static boolean isReversed = false;
  

  private GenericEntry isReversedValSB = Robot.mainTab.add("REVERSED", isReversed).withPosition(1, 0).getEntry();
  private GenericEntry percentSpeedValSB = Robot.mainTab.add("SPEED%", (percentOutput * 100) + "%").withPosition(0, 0).getEntry();

  /** Creates a new DriveTrain. */
  public DriveTrain() {
    g_gyro.reset();
    NAVX.reset();
    rightM.configFactoryDefault();
    leftM.configFactoryDefault();
    rightS.configFactoryDefault();
    leftS.configFactoryDefault();

    rightM.configSupplyCurrentLimit(new SupplyCurrentLimitConfiguration(true, 35, 80, 0.75));
    leftM.configSupplyCurrentLimit(new SupplyCurrentLimitConfiguration(true, 35, 80, 0.75));
    rightS.configSupplyCurrentLimit(new SupplyCurrentLimitConfiguration(true, 35, 80, 0.75));
    leftS.configSupplyCurrentLimit(new SupplyCurrentLimitConfiguration(true, 35, 80, 0.75));

    // Set accleration and decleration to 1 second, it will take 1 second to go full throttle
    // rightM.configOpenloopRamp(0.4);
    // leftM.configOpenloopRamp(0.4);
    // rightS.configOpenloopRamp(0.4);
    // leftS.configOpenloopRamp(0.4);


    leftS.follow(leftM);
    rightS.follow(rightM);

    rightM.setInverted(true);
    rightS.setInverted(true);
    leftM.setInverted(false);
    leftS.setInverted(false);

    percentOutput = 0.6;
    isReversed = false;

    // leftM.configSelectedFeedbackSensor(FeedbackDevice.IntegratedSensor);
    // rightM.configSelectedFeedbackSensor(FeedbackDevice.IntegratedSensor);
    // leftS.configSelectedFeedbackSensor(FeedbackDevice.IntegratedSensor);
    // rightS.configSelectedFeedbackSensor(FeedbackDevice.IntegratedSensor);
    
    // fxConfig.primaryPID.selectedFeedbackSensor = FeedbackDevice.IntegratedSensor;

    resetEncoders();
    // setBreakMode(); 
    zeroHeading();

    diffDrive = new DifferentialDrive(leftSideDrive, rightSideDrive);

    m_odometry = new DifferentialDriveOdometry(
      g_gyro.getRotation2d(),
      encoderTicksToMeters(leftM.getSelectedSensorPosition()),
      encoderTicksToMeters(rightM.getSelectedSensorPosition())
    );

    Shuffleboard.getTab("Auto").add(m_field).withSize(7, 4).withPosition(3, 0);
  }

  @Override
  public void periodic() {
    // This method will be called once per scheduler run
    m_odometry.update(
      g_gyro.getRotation2d(),
      encoderTicksToMeters(leftM.getSelectedSensorPosition()),
      encoderTicksToMeters(rightM.getSelectedSensorPosition())
    );

    m_field.setRobotPose(getPose());
    isReversedValSB.setBoolean(isReversed);
    percentSpeedValSB.setString((percentOutput * 100) + "%");
  }



  public void showTraj(String pathName) {
    List<PathPlannerTrajectory> path = PathPlanner.loadPathGroup(pathName,
    PathPlanner.getConstraintsFromPath(pathName));
    m_field.getObject("Field").setTrajectory(new Trajectory());
    m_field.getObject("Field").setTrajectory(path.get(0));
  }

  public void showTraj() {
    m_field.getObject("Field").setTrajectory(new Trajectory());
  }

  
  
  public void runBalance(double sol, double sag){
    
    diffDrive.tankDrive(-sol,  sag);
    
    
    }
  
  public void setRight(ControlMode controlmode, double value) {
    rightM.set(ControlMode.PercentOutput, value);
  }

  public void setLeft(ControlMode controlmode, double value) {
    leftM.set(ControlMode.PercentOutput, value);
  }

  // This method can be used to convert encoder ticks to meters 
  public double encoderTicksToMeters(double currentEncoderValue) {
    double motorRotations = (double) currentEncoderValue / Constants.AutoConstants.kEncoderFullRev;
    double wheelRotations = motorRotations / Constants.DriveConstants.kGearRatio;
    double positionMeters = wheelRotations * Units.inchesToMeters(Constants.DriveConstants.kWheelCircumferenceInches);
    return positionMeters;
  }


  public void tankDrive(double leftSpeed, double rightSpeed) {
    
    diffDrive.tankDrive(leftSpeed, rightSpeed);
  }

  public static void setBreakMode() {
    leftM.setNeutralMode(NeutralMode.Brake);
    rightM.setNeutralMode(NeutralMode.Brake);
    leftS.setNeutralMode(NeutralMode.Brake);
    rightS.setNeutralMode(NeutralMode.Brake);
  }

  public static void setCoastMode() {
    leftM.setNeutralMode(NeutralMode.Coast);
    rightM.setNeutralMode(NeutralMode.Coast);
    leftS.setNeutralMode(NeutralMode.Coast);
    rightS.setNeutralMode(NeutralMode.Coast);
  }
  public void runBreak(){
    leftSideDrive.set(0.05);;
    rightSideDrive.set(-0.05);
    rightM.setNeutralMode(NeutralMode.Brake);
    rightS.setNeutralMode(NeutralMode.Brake);
    leftM.setNeutralMode(NeutralMode.Brake);
    leftS.setNeutralMode(NeutralMode.Brake);
    
  }
  
  public void resetEncoders() {
    leftM.setSelectedSensorPosition(0);
    leftS.setSelectedSensorPosition(0);
    rightM.setSelectedSensorPosition(0);
    rightS.setSelectedSensorPosition(0);
  }

  public double getRightEncoderPosition() {
    return encoderTicksToMeters(rightM.getSelectedSensorPosition());  
  }

  public double getLeftEncoderPosition() {
    return encoderTicksToMeters(leftM.getSelectedSensorPosition());
  }

  public double getRightEncoderVelocity() {
    // Multiply the raw velocity by 10 since it reports per 100 ms, we want the velocity in m/s
    return encoderTicksToMeters(rightM.getSelectedSensorVelocity()) * 10;
  }

  public double getLeftEncoderVelocity() {
    return encoderTicksToMeters(leftM.getSelectedSensorVelocity()) * 10;
  }

  public double getPitch() {
    // We are getting the roll because the actual "pitch" is the roll becuase of the way navX is mounted
    return NAVX.getRoll();
  }

  public double getTurnRate() {
    return -g_gyro.getRate();
  }

  public static double getHeading() {
    return g_gyro.getRotation2d().getDegrees();
  }

  public Pose2d getPose() {
    return m_odometry.getPoseMeters();
  }

  public void resetOdometery(Pose2d pose) {
    resetEncoders();
    m_odometry.resetPosition(
      g_gyro.getRotation2d(),
      encoderTicksToMeters(leftM.getSelectedSensorPosition()),
      encoderTicksToMeters(rightM.getSelectedSensorPosition()),
      pose
    );
  }


  public static Command getMap(){
    if(RobotContainer.m_chooser.getSelected() == 0){
      return  RobotContainer.ramAutoBuilder("nothing", Constants.AutoConstants.nothing);
    }
    else if(RobotContainer.m_chooser.getSelected() == 1){
      return RobotContainer.ramAutoBuilder("left1", Constants.AutoConstants.left1);
    }
    else if(RobotContainer.m_chooser.getSelected() == 2){
      return RobotContainer.ramAutoBuilder("left2", Constants.AutoConstants.left2);
    }
    else if(RobotContainer.m_chooser.getSelected() == 3){
      return RobotContainer.ramAutoBuilder("left3", Constants.AutoConstants.left3);
    }
    else if(RobotContainer.m_chooser.getSelected() == 4){
      return RobotContainer.ramAutoBuilder("left4", Constants.AutoConstants.left4);
    }
    else if(RobotContainer.m_chooser.getSelected() == 5){
      return RobotContainer.ramAutoBuilder("left5", Constants.AutoConstants.left5);
    }
    else if(RobotContainer.m_chooser.getSelected() == 6){
      return RobotContainer.ramAutoBuilder("together1", Constants.AutoConstants.together1);
    }
    else if(RobotContainer.m_chooser.getSelected() == 7){
      return RobotContainer.ramAutoBuilder("together2", Constants.AutoConstants.together2);
    }
    else if(RobotContainer.m_chooser.getSelected() == 8){
      return RobotContainer.ramAutoBuilder("right1", Constants.AutoConstants.right1);
    }
    else if(RobotContainer.m_chooser.getSelected() == 9){
      return RobotContainer.ramAutoBuilder("right2", Constants.AutoConstants.right2);
    }
    else{
      return null;
    }
        }


    
  public DifferentialDriveWheelSpeeds getWheelSpeeds() {
    return new DifferentialDriveWheelSpeeds(getLeftEncoderVelocity(), getRightEncoderVelocity());
  }

  public void tankDriveVolts(double leftVolts, double rightVolts) {
    leftSideDrive.setVoltage(leftVolts);
    rightSideDrive.setVoltage(rightVolts);
    diffDrive.feed();
  }

  public double getAverageEncoderDis() {
    return (getLeftEncoderPosition() + getRightEncoderPosition()) / 2.0;
  }


  public void setMaxOutputOfDrive(double maxOut) {
    diffDrive.setMaxOutput(maxOut);
  }

  public static void arcadeDrive(double fwd, double rot) {
    diffDrive.arcadeDrive(fwd, rot);
  }
  public void zeroHeading() {
    g_gyro.reset();
    NAVX.reset();
  }

  public Gyro getGyro() {
    return g_gyro;
  }
  public static void turnDrive(double turnSpeed){
        
    diffDrive.tankDrive(turnSpeed, -turnSpeed);
}
type or paste code here
`
1 Like

This might already be done by wpilib but I think it is that you need to feed the motor watchdogs. They make sure the motors are working. Do this in your periodic function and see if it works.

differentialDrive.feed();
1 Like

Have you tried pressing the break mode buttons on the back of the falcons? If it is glowing red passively then it’s set to break mode.

1 Like

Most likely there is an error in the codes of the robot. Because when I run the robot with a normal arcade drive code, the problem disappears. but this code seems to be causing trouble

Whilst I am NOT a coder…is the code jiving with the Phoenix? As in, everything you want the motors to do is also in the Phoenix app? Also, are your controllers up to date on the software?

I also have never put Falcons on a tank setup…mostly because we have been running KoP.

IF you are running a standard controller…I didn’t see the code for the Joystick in there…it may not be an issue…just an observation.

Again…I am not a coder. And I am just going off the code for our drivetrain…

Here are some codes you can look at to see what is going on…as far as the lights on the motor.

This topic was automatically closed 365 days after the last reply. New replies are no longer allowed.