We are trying to get our team’s swerve drive working. We are using the FalconSwerveCode found here —> GitHub - dirtbikerxz/BaseTalonFXSwerve modiffed for navx, and SRX Mag Encoders.
Everything works for speed motors but not our position motors.
We have gone backward to just try and get a wheel to spin where we tell it. Our code is below. What we have found is that an unloaded motor with the same code does exactly what we want. Velocity starts large and decreases until it gets so low that the motor can’t move anymore which is within 0.005 rotation of our position. When we use the same code with a loaded motor the voltage starts small which isn’t enough to move the motor and never changes.
We have no idea why or what to do to fix this. Any ideas?
// 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;
import com.ctre.phoenix6.configs.Slot0Configs;
import com.ctre.phoenix6.controls.DutyCycleOut;
import com.ctre.phoenix6.controls.PositionVoltage;
import com.ctre.phoenix6.hardware.TalonFX;
import edu.wpi.first.wpilibj.DutyCycle;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
public class Robot extends TimedRobot {
private TalonFX wheel_motor, hand_motor ;
DutyCycleOut m_wheel_motor;
private PositionVoltage anglePosition;
@Override
public void robotInit() {
var slot0Configs = new Slot0Configs();
slot0Configs.kP = 24; // An error of 0.5 rotations results in 12 V output
slot0Configs.kI = 0; // no output for integrated error
slot0Configs.kD = 0.1; // A velocity of 1 rps results in 0.1 V output
wheel_motor = new TalonFX(6);
hand_motor = new TalonFX(9);
wheel_motor.getConfigurator().apply(slot0Configs);
hand_motor.getConfigurator().apply(slot0Configs);
wheel_motor.setPosition(0);
anglePosition = new PositionVoltage(0).withSlot(0);
}
@Override
public void robotPeriodic() {
System.out.println("\nWheel Motor Position: " + wheel_motor.getPosition());
System.out.println("Wheel Motor Voltage: " + wheel_motor.getMotorVoltage());
System.out.println(anglePosition.withPosition(100).getName());
//System.out.println("Hand Motor Position: " + hand_motor.getPosition());
//System.out.println("Hand Motor Voltage: " + hand_motor.getMotorVoltage());
}
@Override
public void teleopInit() {}
/** This function is called periodically during operator control. */
@Override
public void teleopPeriodic() {
wheel_motor.setControl(anglePosition.withPosition(100));
//hand_motor.setControl(anglePosition.withPosition(100));
//wheel_motor.setVoltage(0.1);
}
/** This function is called once when the robot is disabled. */
@Override
public void disabledInit() {}
/** This function is called periodically when disabled. */
@Override
public void disabledPeriodic() {}
/** This function is called once when test mode is enabled. */
@Override
public void testInit() {}
/** This function is called periodically during test mode. */
@Override
public void testPeriodic() {}
/** This function is called once when the robot is first started up. */
@Override
public void simulationInit() {}
/** This function is called periodically whilst in simulation. */
@Override
public void simulationPeriodic() {}
@Override
public void autonomousInit() {
}
@Override
public void autonomousPeriodic() {
}
}