Robot simulation only moves on the x access

Hi, we are currently trying to configure a simulation to our driveTrain using the phoenix vendor library.
we tried recoding a couple of part of the program and check for a keyboard error, but it still only works on the X access. if someone has already encountered this problem and solved it we would be glad to get a direction

// 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 com.ctre.phoenix.motorcontrol.TalonFXSimCollection;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;

import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants;
import frc.robot.RobotContainer;

public class DriveTrain extends SubsystemBase {
  /** Creates a new DriveTrain. */
  private WPI_TalonFX backLeftMotor; 
  private WPI_TalonFX backRightMotor;
  private WPI_TalonFX frontLeftMotor; 
  private WPI_TalonFX frontRightMotor;
  private DifferentialDrive differentialDrive;
  private edu.wpi.first.math.kinematics.DifferentialDriveOdometry DifferentialDriveOdometry;

  protected TalonFXSimCollection backLeftMotorSim;
  protected TalonFXSimCollection backRightMotorSim;
  protected TalonFXSimCollection frontLeftMotorSim;
  protected TalonFXSimCollection frontRightMotorSim;
  protected DifferentialDrivetrainSim differentialDriveSim;
  protected Field2d simField;

  
  public DriveTrain() {
    this.backLeftMotor = new WPI_TalonFX(Constants.kBackLeftMotor);
    this.backRightMotor = new WPI_TalonFX(Constants.kBackRightMotor);
    this.frontLeftMotor = new WPI_TalonFX(Constants.kFrontLeftMotor);
    this.frontRightMotor = new WPI_TalonFX(Constants.kFrontRightMotor);

    this.frontLeftMotorSim = frontLeftMotor.getSimCollection();
    this.frontRightMotorSim = frontRightMotor.getSimCollection();


    this.backLeftMotorSim = backLeftMotor.getSimCollection();
    this.backRightMotorSim = backRightMotor.getSimCollection();

    this.backLeftMotor.follow(this.frontLeftMotor);
    this.backRightMotor.follow(this.frontRightMotor);

    this.differentialDrive = new DifferentialDrive(this.frontLeftMotor, this.frontRightMotor);
    this.DifferentialDriveOdometry = new edu.wpi.first.math.kinematics.DifferentialDriveOdometry(new Rotation2d(), 0, 0);
    // I currently dont know to real parameters of our robot's driveTrain, these are temp variables 
    this.differentialDriveSim = new DifferentialDrivetrainSim(DCMotor.getCIM(2), Constants.kGearRatio, 2.1, 26.5, Units.inchesToMeters(Constants.kWheelRadiusInches), 0.546, null);
    
    this.simField = new Field2d();
    SmartDashboard.putData("Field", simField);
  }

  public WPI_TalonFX getBackLeftMotor(){return this.backLeftMotor;}
  
  public TalonFXSimCollection getbackLeftMotorSim(){return this.backLeftMotor.getSimCollection();}
  public TalonFXSimCollection getbackRightMotorSim(){return this.backRightMotor.getSimCollection();}
  public TalonFXSimCollection getfrontLeftMotorSim(){return this.frontLeftMotor.getSimCollection();}
  public TalonFXSimCollection getfrontRightMotorSim(){return this.frontRightMotor.getSimCollection();}
  public DifferentialDrivetrainSim getdifferentialDriveSim(){return this.differentialDriveSim;}
  
  public void robotArcadeDrive(double move, double rot){
    if (move < 0.05) {move = 0;}
    if (rot < 0.05) {rot = 0;}
    differentialDrive.arcadeDrive(move, rot * -1);
  }

  public void robotTankDrive(double leftMove, double rightMoveStick){
    differentialDrive.tankDrive(leftMove, rightMoveStick);
  }


public static int distanceToNativeUnits(double positionMeters){
  double wheelRotations = positionMeters/(2 * Math.PI * Units.inchesToMeters(Constants.kWheelRadiusInches));
  double motorRotations = wheelRotations * Constants.kSensorGearRatio;
  int sensorCounts = (int)(motorRotations * Constants.kCountsPerRev);
  return sensorCounts;
}

public static double nativeUnitsToDistanceMeters(double sensorCounts){
  double motorRotations = (double)sensorCounts / Constants.kCountsPerRev;
  double wheelRotations = motorRotations / Constants.kSensorGearRatio;
  double positionMeters = wheelRotations * (2 * Math.PI * Units.inchesToMeters(Constants.kWheelRadiusInches));
  return positionMeters;
}

public static int velocityToNativeUnits(double velocityMetersPerSecond){
  double wheelRotationsPerSecond = velocityMetersPerSecond/(2 * Math.PI * Units.inchesToMeters(Constants.kWheelRadiusInches));
  double motorRotationsPerSecond = wheelRotationsPerSecond * Constants.kSensorGearRatio;
  double motorRotationsPer100ms = motorRotationsPerSecond / Constants.k100msPerSecond;
  int sensorCountsPer100ms = (int)(motorRotationsPer100ms * Constants.kCountsPerRev);
  return sensorCountsPer100ms;
}



  @Override
  public void periodic() {
    // This method will be called once per scheduler run
    // Currently we don't have a configured gyro so the angle should always be 0, thats why we have a new object in the constructor
    DifferentialDriveOdometry.update(new Rotation2d(), nativeUnitsToDistanceMeters(this.frontLeftMotor.getSelectedSensorPosition()),
    nativeUnitsToDistanceMeters(this.frontRightMotor.getSelectedSensorPosition()));
    simField.setRobotPose(DifferentialDriveOdometry.getPoseMeters());
  }

  public void simulationInit(){}

  public void simulationPeriodic(){
    RobotContainer.driveArcade.getfrontLeftMotorSim().setBusVoltage(RobotController.getBatteryVoltage());
    RobotContainer.driveArcade.getfrontRightMotorSim().setBusVoltage(RobotController.getBatteryVoltage());
    
    RobotContainer.driveArcade.backLeftMotorSim.setBusVoltage(RobotController.getBatteryVoltage());
    RobotContainer.driveArcade.backRightMotorSim.setBusVoltage(RobotController.getBatteryVoltage());

    RobotContainer.driveArcade.getdifferentialDriveSim().setInputs(RobotContainer.driveArcade.getfrontLeftMotorSim().getMotorOutputLeadVoltage(), RobotContainer.driveArcade.getfrontRightMotorSim().getMotorOutputLeadVoltage() * -1);

    RobotContainer.driveArcade.getdifferentialDriveSim().update(Constants.kUpdateTime);

    differentialDriveSim.update(0.02);

    this.frontLeftMotorSim.setIntegratedSensorRawPosition(distanceToNativeUnits(this.differentialDriveSim.getRightPositionMeters() * -1));
    this.frontLeftMotorSim.setIntegratedSensorVelocity(velocityToNativeUnits(this.differentialDriveSim.getRightVelocityMetersPerSecond() * -1));

    this.frontRightMotorSim.setIntegratedSensorRawPosition(distanceToNativeUnits(this.differentialDriveSim.getRightPositionMeters() * -1));
    this.frontRightMotorSim.setIntegratedSensorVelocity(velocityToNativeUnits(this.differentialDriveSim.getRightVelocityMetersPerSecond() * -1));
    
    this.backLeftMotorSim.setIntegratedSensorRawPosition(distanceToNativeUnits(this.differentialDriveSim.getRightPositionMeters() * -1));
    this.backLeftMotorSim.setIntegratedSensorVelocity(velocityToNativeUnits(this.differentialDriveSim.getRightVelocityMetersPerSecond() * -1));

    this.backRightMotorSim.setIntegratedSensorRawPosition(distanceToNativeUnits(this.differentialDriveSim.getRightPositionMeters() * -1));
    this.backRightMotorSim.setIntegratedSensorVelocity(velocityToNativeUnits(this.differentialDriveSim.getRightVelocityMetersPerSecond() * -1));
  }
}
1 Like

Update:
we got half of the problem fixed!
we thought because we don’t have a gyro we wouldn’t need it so we init an instant of gyro and called it a day.
the gyro had a critical part in the rotation process. right now the x access error is fixed but currently we can only move using the ‘s’ and ‘d’ chars and not the whole “wasd” keyboard.
now we are trying to replace the .follow method with an actual motor contoller group method

code:

// 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 com.ctre.phoenix.motorcontrol.TalonFXSimCollection;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import com.ctre.phoenix.sensors.BasePigeonSimCollection;
import com.ctre.phoenix.sensors.WPI_PigeonIMU;

import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants;
import frc.robot.RobotContainer;

public class DriveTrain extends SubsystemBase {
  /** Creates a new DriveTrain. */
  private WPI_TalonFX backLeftMotor; 
  private WPI_TalonFX backRightMotor;
  private WPI_TalonFX frontLeftMotor; 
  private WPI_TalonFX frontRightMotor;
  private WPI_PigeonIMU pigeon;
  private DifferentialDrive differentialDrive;
  private edu.wpi.first.math.kinematics.DifferentialDriveOdometry DifferentialDriveOdometry;

  protected TalonFXSimCollection backLeftMotorSim;
  protected TalonFXSimCollection backRightMotorSim;
  protected TalonFXSimCollection frontLeftMotorSim;
  protected TalonFXSimCollection frontRightMotorSim;
  protected BasePigeonSimCollection pigeonSim;
  protected DifferentialDrivetrainSim differentialDriveSim;
  protected Field2d simField;

  
  public DriveTrain() {
    this.backLeftMotor = new WPI_TalonFX(Constants.kBackLeftMotor);
    this.backRightMotor = new WPI_TalonFX(Constants.kBackRightMotor);
    this.frontLeftMotor = new WPI_TalonFX(Constants.kFrontLeftMotor);
    this.frontRightMotor = new WPI_TalonFX(Constants.kFrontRightMotor);

    this.frontLeftMotorSim = frontLeftMotor.getSimCollection();
    this.frontRightMotorSim = frontRightMotor.getSimCollection();

    this.pigeon = new WPI_PigeonIMU(Constants.kPigeonPort);
    this.pigeonSim = this.pigeon.getSimCollection();

    this.backLeftMotorSim = backLeftMotor.getSimCollection();
    this.backRightMotorSim = backRightMotor.getSimCollection();

    this.backLeftMotor.follow(this.frontLeftMotor);
    this.backRightMotor.follow(this.frontRightMotor);

    this.differentialDrive = new DifferentialDrive(this.frontLeftMotor, this.frontRightMotor);
    this.DifferentialDriveOdometry = new edu.wpi.first.math.kinematics.DifferentialDriveOdometry(this.pigeon.getRotation2d(), 0, 0);
    // I currently dont know to real parameters of our robot's driveTrain, these are temp variables 
    this.differentialDriveSim = new DifferentialDrivetrainSim(DCMotor.getCIM(2), Constants.kGearRatio, 2.1, 26.5, Units.inchesToMeters(Constants.kWheelRadiusInches), 0.546, null);
    
    this.simField = new Field2d();
    SmartDashboard.putData("Field", simField);
  }

  public WPI_TalonFX getBackLeftMotor(){return this.backLeftMotor;}
  
  public TalonFXSimCollection getbackLeftMotorSim(){return this.backLeftMotor.getSimCollection();}
  public TalonFXSimCollection getbackRightMotorSim(){return this.backRightMotor.getSimCollection();}
  public TalonFXSimCollection getfrontLeftMotorSim(){return this.frontLeftMotor.getSimCollection();}
  public TalonFXSimCollection getfrontRightMotorSim(){return this.frontRightMotor.getSimCollection();}
  public DifferentialDrivetrainSim getdifferentialDriveSim(){return this.differentialDriveSim;}
  
  public void robotArcadeDrive(double move, double rot){
    if (move < 0.05) {move = 0;}
    if (rot < 0.05) {rot = 0;}
    differentialDrive.arcadeDrive(move, rot * -1);
  }

  public void robotTankDrive(double leftMove, double rightMoveStick){
    differentialDrive.tankDrive(leftMove, rightMoveStick);
  }


public static int distanceToNativeUnits(double positionMeters){
  double wheelRotations = positionMeters/(2 * Math.PI * Units.inchesToMeters(Constants.kWheelRadiusInches));
  double motorRotations = wheelRotations * Constants.kSensorGearRatio;
  int sensorCounts = (int)(motorRotations * Constants.kCountsPerRev);
  return sensorCounts;
}

public static double nativeUnitsToDistanceMeters(double sensorCounts){
  double motorRotations = (double)sensorCounts / Constants.kCountsPerRev;
  double wheelRotations = motorRotations / Constants.kSensorGearRatio;
  double positionMeters = wheelRotations * (2 * Math.PI * Units.inchesToMeters(Constants.kWheelRadiusInches));
  return positionMeters;
}

public static int velocityToNativeUnits(double velocityMetersPerSecond){
  double wheelRotationsPerSecond = velocityMetersPerSecond/(2 * Math.PI * Units.inchesToMeters(Constants.kWheelRadiusInches));
  double motorRotationsPerSecond = wheelRotationsPerSecond * Constants.kSensorGearRatio;
  double motorRotationsPer100ms = motorRotationsPerSecond / Constants.k100msPerSecond;
  int sensorCountsPer100ms = (int)(motorRotationsPer100ms * Constants.kCountsPerRev);
  return sensorCountsPer100ms;
}



  @Override
  public void periodic() {
    // This method will be called once per scheduler run
    // Currently we don't have a configured gyro so the angle should always be 0, thats why we have a new object in the constructor
    DifferentialDriveOdometry.update(this.pigeon.getRotation2d(), nativeUnitsToDistanceMeters(this.frontLeftMotor.getSelectedSensorPosition()),
    nativeUnitsToDistanceMeters(this.frontRightMotor.getSelectedSensorPosition()));
    simField.setRobotPose(DifferentialDriveOdometry.getPoseMeters());
  }

  public void simulationInit(){}

  public void simulationPeriodic(){
    RobotContainer.driveArcade.getfrontLeftMotorSim().setBusVoltage(RobotController.getBatteryVoltage());
    RobotContainer.driveArcade.getfrontRightMotorSim().setBusVoltage(RobotController.getBatteryVoltage());
    
    RobotContainer.driveArcade.backLeftMotorSim.setBusVoltage(RobotController.getBatteryVoltage());
    RobotContainer.driveArcade.backRightMotorSim.setBusVoltage(RobotController.getBatteryVoltage());

    RobotContainer.driveArcade.getdifferentialDriveSim().setInputs(RobotContainer.driveArcade.getfrontLeftMotorSim().getMotorOutputLeadVoltage(), RobotContainer.driveArcade.getfrontRightMotorSim().getMotorOutputLeadVoltage() * -1);

    RobotContainer.driveArcade.getdifferentialDriveSim().update(Constants.kUpdateTime);

    differentialDriveSim.update(0.02);

    this.frontLeftMotorSim.setIntegratedSensorRawPosition(distanceToNativeUnits(this.differentialDriveSim.getRightPositionMeters() * -1));
    this.frontLeftMotorSim.setIntegratedSensorVelocity(velocityToNativeUnits(this.differentialDriveSim.getRightVelocityMetersPerSecond() * -1));

    this.frontRightMotorSim.setIntegratedSensorRawPosition(distanceToNativeUnits(this.differentialDriveSim.getRightPositionMeters() * -1));
    this.frontRightMotorSim.setIntegratedSensorVelocity(velocityToNativeUnits(this.differentialDriveSim.getRightVelocityMetersPerSecond() * -1));
    
    this.pigeonSim.setRawHeading(this.differentialDriveSim.getHeading().getDegrees());
    // this.backLeftMotorSim.setIntegratedSensorRawPosition(distanceToNativeUnits(this.differentialDriveSim.getRightPositionMeters() * -1));
    // this.backLeftMotorSim.setIntegratedSensorVelocity(velocityToNativeUnits(this.differentialDriveSim.getRightVelocityMetersPerSecond() * -1));

    // this.backRightMotorSim.setIntegratedSensorRawPosition(distanceToNativeUnits(this.differentialDriveSim.getRightPositionMeters() * -1));
    // this.backRightMotorSim.setIntegratedSensorVelocity(velocityToNativeUnits(this.differentialDriveSim.getRightVelocityMetersPerSecond() * -1));

  }
}

As joystick values range from -1 to 1, these set anything from -1 to 0.05 to 0 getting rid of half the values.

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