TalonSRX Motion Magic Problems

Technical Programming
#1

Were using a talon SRX with motion magic and when we drive it won’t stop when because the encoder is running backward. If we change the sensor phase it seems to have no effect and If we set our target to a negative the motor runs the opposite way. But if the change the .setinverted to true it still blows past its target. Does anyone know what I am doing wrong? Here is our code.

/----------------------------------------------------------------------------/
/* Copyright © 2018 FIRST. All Rights Reserved. /
/ Open Source Software - may be modified and shared by FRC teams. The code /
/ must be accompanied by the FIRST BSD license file in the root directory of /
/ the project. /
/----------------------------------------------------------------------------*/

package frc.robot.subsystems;

import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

import com.ctre.phoenix.motorcontrol.;
import com.ctre.phoenix.motorcontrol.can.;
import frc.robot.OI;

/**

  • Add your docs here.
    */
    public class WristSubsystem extends Subsystem {
    int kTimeoutMs = 10;
    int kPIDLoopIdx = 0;
    WPI_TalonSRX SRXwrist = new WPI_TalonSRX(OI.SRXwrist);
    public WristSubsystem(){
    //setting pid values

     int absolutePosition = SRXwrist.getSelectedSensorPosition(kTimeoutMs) & 0xFFF;

 SRXwrist.setSelectedSensorPosition(absolutePosition, kPIDLoopIdx, kTimeoutMs);
 SRXwrist.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, kPIDLoopIdx, kTimeoutMs);

    SRXwrist.setSensorPhase(true); //rm invert
    SRXwrist.setInverted(false);
    SRXwrist.selectProfileSlot(0, kPIDLoopIdx);
    SRXwrist.configNominalOutputForward(0, kTimeoutMs);
    SRXwrist.configNominalOutputReverse(0, kTimeoutMs);
    SRXwrist.configPeakOutputForward(1, kTimeoutMs);
    SRXwrist.configPeakOutputReverse(-1, kTimeoutMs);
    SRXwrist.configAllowableClosedloopError(0, kPIDLoopIdx, kTimeoutMs);
    SRXwrist.configMotionCruiseVelocity(15000, kTimeoutMs);
    SRXwrist.configMotionAcceleration(200, kTimeoutMs);
    SRXwrist.config_kF(kPIDLoopIdx, 0.0, kTimeoutMs);
    SRXwrist.config_kP(kPIDLoopIdx, 17, kTimeoutMs);
    SRXwrist.config_kI(kPIDLoopIdx, 0, kTimeoutMs);
    SRXwrist.config_kD(kPIDLoopIdx, 0, kTimeoutMs);
    SRXwrist.setSelectedSensorPosition(0, 0, 0);

     //setting pid values
 SmartDashboard.putNumber("wristpos", SRXwrist.getSelectedSensorPosition(0));

}
public void reset() {
SRXwrist.setSelectedSensorPosition(0, kPIDLoopIdx, kTimeoutMs);
}
public void initDefaultCommand() {
}
public void movetoHatchPosition() { // move wrist to hatch position
SRXwrist.set(ControlMode.MotionMagic,480);
}
public void movetoBallPosition() { // move wrist to ball position
SRXwrist.set(ControlMode.MotionMagic,650); //rm invert
}
public void movetoHomePosition() { // move wrist to home position
SRXwrist.set(ControlMode.MotionMagic,0);
}

public void in() { // moves the wrist at set speed
SRXwrist.set(ControlMode.PercentOutput,.35);
SmartDashboard.putNumber(“www”, 13290593);
}
public void out() { // moves the wrist at set speed
SRXwrist.set(ControlMode.PercentOutput,-.35);
}
public void stop() {
SRXwrist.set(0);
}
}

#2

Are you outputting your encoder counts so you see what its position is when you’re commanding the wrist? Your encoder might not be working if the encoder is damaged, wire is bad, etc.

Secondly, are you using the correct positions for your setpoints? Normally, 0-4096 encoder counts is 0-360 degrees of rotation, but if you have a gear reduction in between the encoder and wrist, you will need to scale your encoder counts accordingly (e.g. a 2.33 gear reduction would make your encoder counts 0-9544 for 0-360 degrees)

#3

Yes we are putting it to the smart dashboard

SmartDashboard.putNumber(“wristpos”, SRXwrist.getSelectedSensorPosition(0));

#4

Have you tried setting both setInverted and SetSensorPhase to true? This was the solution for us. It doesn’t make much sense to me since 2 negatives should make a positive, but hey it works for us.

#5

When the wrist is moved manually, the encoder moves negative (from zero at start), when looking at the value on smartdashboard. No combination of setSensorPhase and isinverted seems to make any difference to the values shown on SmartDashboard when the wrist is moved in the “positive” direction.

#6

Are you setting the encoder position anywhere else in the code? I see you set it to 0 when you initialize the subsystem, so if your wrist is somewhere in between its upper and lower limit, its range of motion will be different (e.g. When you initialize it while its at the bottom, its range may be 0-2000, but if its physically somewhere in the middle when you initialize it, its range will be -1000 to +1000)

Also, try lowering your config_kP value to a much smaller value? Like 0.2.

#7

setInverted sets the motor output to the opposite (green vs red on the motor controller and forwards and backwards on the motor). setSensorPhase changes the direction of the sensor increment.

#8

We are not setting the encoder anywhere else. And we tried turning the P down to no avail.

#9

Put this line before you set any of the SRXwrist settings:

SRXwrist.configFactoryDefault()

This will clear all lingering setttings that may be inadvertedly set on the Talon.

Next, try getting rid of the .configPeakOutputForward/Reverse and see if that helps.

Also, have you already tried swapping encoder cables?

1 Like
#10

Did you follow the CTRE bring up steps to get your motor and sensor in phase beforehand?

#11

We have tried switching encoder wires and setting factory default and getting rid of peak output did not seem to change the results

#12

After looking at them the motor and sensor do seem to be in phase.

#13

How are you calling your position functions in code? Are you just calling them once after a button press with nothing else after that?

Secondly, are you sure those positions are correct? If you always start at the same physical position when you redeploy code and then physically move the wrist to where it should be?

Are you actually using a CTRE Mag encoder? Is the LED on it staying green/yellow as your wrist moves? Have you tried swapping it out?

#15

it is called by simply holding down a button, and if we manually move the wrist to 650 it is in the right position. I just replaced the CTRE mag encoder and we still have the same issue.

#16

Do you have your code uploaded to Github so we can see the entire repository?

#17

Sounds like this was mentioned, but here is the relevant section in documentation.
https://phoenix-documentation.readthedocs.io/en/latest/ch14_MCSensor.html?highlight=setsensorphase#adjust-sensor-phase-using-api

SetInverted cannot be used to correct sensor phase. See link above.

Also don’t just flip the value until the closed-loop behaves. Correct the sensor phase first using open-loop control ( as documented).

Next Steps

After you have open-looped driven your motor and confirmed sensor velocity is positive when Talon is blinking green:

1 Like
#18

Is this button called in teleOpPeriodic() with an if() statement or called using the command-based structure. If its the latter, are you doing anything else in your execute()/end() functions?

Have you tried plugging in the encoder into a different Talon and checking if the behavior is the same?

#19

Here is a self test of the talon SRX sorry for the small text

image
image.png3000×2000 272 KB

#20
  • Talon 13 was not enabled (top of self test). Check the Driver Station - was the robot enabled?
  • If the DS was enabled, then check the CAN IDs you specified in source in your Talon constructors.
  • Make sure Talon 13 is the correct one - i.e. the one you are troubleshooting
  • Be sure to capture a self-test of the Talon (via Tuner) while closed-loop is misbehaving .
#21

I have narrowed the issue down to the fact that the sensor and the motor are not in phase. However changing the setsensorphase to true dosent do anything.