Rogue Arm Movement with Talon SRX

Technical Programming
1

I am a new FRC programming mentor (I have experience with programming for FLL, FTC, and VEX EDR) with a programming team with no experience. We have implemented a command based robot using robot builder and Java.

On this year’s robot we have a rotating shoulder (a little over 180 degrees of motion). We use PID controls to move the shoulder to a set position. Sometimes, when we enable the robot (after a disable), the shoulder begins to move without a command being given – this has caused some catastrophic mechanical failures of our motor mount. We are currently working around this issue by powering off the robot every time we restart – which I wouldn’t think would be a good plan going into a tournament!

Our shoulder is powered by a bag motor attached to a Talon SRX. We have a command that we execute from shuffleboard (using a command widget) to move the shoulder to the proper height (hatch rocket low, hatch rocket mid, etc.). We also have a command that allows the operator to move the shoulder using a joystick – We rarely use this – the drive team quickly figured out that the program was far more efficient than they are. The shoulder sub-system does NOT have a default command. All of our encoder positions use values based on the ABSOLUTE position; however, our PID uses the relative value – we seed the relative value with the absolute value to take advantage of the much faster refresh rate of the relative position. Code for the shoulder sub-system, the command, and the encoder values we attempt to hit are shown below.

When this problem occurs on the actual robot, our shuffleboard shows that the relative and absolute encoders are not the same. My suspicion is that when we enable the robot, the Talon is always returning to the last “move to position” command that it had – we don’t ordinarily see it because the arm hasn’t moved – but when the absolute and relative encoders are no longer in sync the arm will move to an unachievable position.

Other Points

  • Currently our command never ends (because I don’t know under what condition to actually end it) – it is simply interrupted by the next command requested on the shoulder sub-system.
  • We have an identical setup on our wrist (this problem also occurs on the wrist but has yet to cause a catastrophic mechanical failure).
  • Because of our gear ratios we are prone to moving past an absolute position of 0.
  • I believe that this rogue movement is probably a newbie mistake – failing to initialize something in the right place, etc.

On our test system, we have a Talon SRX with no motor or encoder. This is what I see:

  1. Power On: Lights on Talon SRX are flashing orange
  2. Enable: Lights on Talon SRX are solid orange and then revert to flashing orange.
  3. Execute Command: Lights on Talon SRX are green (occasionally flash orange and then return to green.
  4. Disable: Lights on Talon SRX are flashing orange.
  5. Enable: Lights on Talon SRX are green (no command has been issued from shuffleboard)
  6. Power Off/On : Lights on Talon SRX are flashing orange
  7. Enable: Lights on Talon SRX are flashing orange.

THIS IS OUR SHOULDER SUBSYSTEM CODE:
// RobotBuilder Version: 2.0
//
// This file was generated by RobotBuilder. It contains sections of
// code that are automatically generated and assigned by robotbuilder.
// These sections will be updated in the future when you export to
// Java from RobotBuilder. Do not put any code or make any change in
// the blocks indicating autogenerated code or it will be lost on an
// update. Deleting the comments indicating the section will prevent
// it from being updated in the future.

package org.usfirst.frc5053.FRC2019DeepSpace.subsystems;

import org.usfirst.frc5053.FRC2019DeepSpace.Robot;
import org.usfirst.frc5053.FRC2019DeepSpace.commands.*;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.PIDOutput;
import edu.wpi.first.wpilibj.PIDSource;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

import com.ctre.phoenix.motorcontrol.ControlMode;
import com.ctre.phoenix.motorcontrol.FeedbackDevice;
import com.ctre.phoenix.motorcontrol.can.BaseMotorController;
// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=IMPORTS
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;

// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=IMPORTS

/**
*
*/
public class shoulderLift extends Subsystem {

// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTANTS

// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTANTS

final int kSlotIdx = 0;
final int kPIDLoopIdx = 0;
final int kTimeoutMs = 30;
boolean kSensorPhase = false;
boolean kMotorInvert = false;
final double kP = 8.0; // SNW 16.0 was good, except it started oscillating at the rocket high
final double kI = 0.0;
final double kD = 0.0;
final double kF = 0.0;
final int kIzone = 0;
final double kPeakOutput = 1.0;
final int encoderMariginOfError = 10;
final boolean kSoftLimitEnabled = true;
final int kSoftLimitForward = 3000;
final int kSoftLimitReverse = 100;
final boolean kEnableCurrentLimit = true;
final int kPeakCurrentLimit = 25; //amps`
final int kPeakCurrentDuration = 200; //milliseconds

// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS
private WPI_TalonSRX shoulderTalonSRX;

// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS

public shoulderLift() {
    System.out.println("shoulderLift START");
    // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS
    shoulderTalonSRX = new WPI_TalonSRX(2);
    
    
    

    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS  

    SmartDashboard.putNumber("armHeightEnum", Robot.oi.INITIAL_AND_ERROR_BUTTON);
    configureShoulder();
    syncEncoderValues();   
    System.out.println("shoulderLift END");
}

@Override
public void initDefaultCommand() {
    System.out.println("shoulderLift.initDefaultCommand START");
    // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DEFAULT_COMMAND


// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DEFAULT_COMMAND

    // Set the default command for a subsystem here.
    // setDefaultCommand(new MySpecialCommand());
    System.out.println("shoulderLift.initDefaultCommand END");
}

@Override
public void periodic() {
    System.out.println("shoulderLift.periodic START");
    // Put code here to be run every loop
    int relPosition = shoulderTalonSRX.getSelectedSensorPosition(0);
    int absPosition = shoulderTalonSRX.getSensorCollection().getPulseWidthPosition();
    double shoulderSpeed = shoulderTalonSRX.get();

    SmartDashboard.putNumber("ShoulderSensorPos: ", relPosition);
    SmartDashboard.putNumber("ShoulderSensorAbs: ", absPosition);
    SmartDashboard.putBoolean("shoulderSynced", isEncoderSynced());
    SmartDashboard.putNumber("shoulderCurrent", shoulderTalonSRX.getOutputCurrent());
    // SmartDashboard.putNumber("ShoulderSpeed: ", shoulderSpeed);
    // SmartDashboard.putNumber("ShoulderJoystickAxis(1): ", Robot.oi.logitech.getRawAxis(1));
    System.out.println("shoulderLift.periodic END");
}

// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CMDPIDGETTERS


// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CMDPIDGETTERS

// Put methods for controlling this subsystem
// here. Call these from Commands.

// 20190208 SNW begin added code
public void moveShoulder(double inputSpeed) {
    System.out.println("shoulderLift.moveShoulder START");
    shoulderTalonSRX.set(ControlMode.PercentOutput, inputSpeed);
    System.out.println("shoulderLift.moveShoulder END");
}

public void stopShoulder(){
    System.out.println("shoulderLift.stopShoulder START");
    shoulderTalonSRX.set(ControlMode.PercentOutput, 0);
    System.out.println("shoulderLift.stopShoulder END");
}
// 20190208 SNW end added code

public void startMoveToPosition(double targetPosition){
    System.out.println("shoulderLift.startMoveToPosition START");

    if (isEncoderSynced())
    {
        System.out.println("shoulderLift.startMoveToPosition ISSUE TALON COMMAND");
        shoulderTalonSRX.set(ControlMode.Position, targetPosition);
        SmartDashboard.putNumber("shoulderTargetPosition", shoulderTalonSRX.getClosedLoopTarget(0));
    }
    else 
    {
        System.out.println("shoulderLift.startMoveToPosition OUT OF SYNC DO NOTHING");
        SmartDashboard.putBoolean("shoulderSynced", false);
    }

    System.out.println("shoulderLift.startMoveToPosition END");
}

public void configureShoulder(){
    System.out.println("shoulderLift.configureShoulder START");
    // configure the shoulder encoder to have the soft limits of 100-3000
            
    /* Config the sensor used for Primary PID and sensor direction */ 
    shoulderTalonSRX.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 
                                                  kPIDLoopIdx,
                                                  kTimeoutMs);

    shoulderTalonSRX.setInverted(kMotorInvert);

    /* Config the peak and nominal outputs, 12V means full */
    shoulderTalonSRX.configNominalOutputForward(0, kTimeoutMs);
    shoulderTalonSRX.configNominalOutputReverse(0, kTimeoutMs);
    shoulderTalonSRX.configPeakOutputForward(1, kTimeoutMs);
    shoulderTalonSRX.configPeakOutputReverse(-1, kTimeoutMs);

    /**
    * Config the allowable closed-loop error, Closed-Loop output will be
    * neutral within this range. See Table in Section 17.2.1 for native
    * units per rotation.
    */
    shoulderTalonSRX.configAllowableClosedloopError(0, kPIDLoopIdx, kTimeoutMs);

    /* Config Position Closed Loop gains in slot0, tsypically kF stays zero. */
   shoulderTalonSRX.config_kF(kPIDLoopIdx, kF, kTimeoutMs);
   shoulderTalonSRX.config_kP(kPIDLoopIdx, kP, kTimeoutMs);
   shoulderTalonSRX.config_kI(kPIDLoopIdx, kI, kTimeoutMs);
   shoulderTalonSRX.config_kD(kPIDLoopIdx, kD, kTimeoutMs);

   // configures TalonSRX to prevent overrotation of the motor
   shoulderTalonSRX.configForwardSoftLimitEnable(kSoftLimitEnabled);
   shoulderTalonSRX.configForwardSoftLimitThreshold(kSoftLimitForward);
   shoulderTalonSRX.configReverseSoftLimitEnable(kSoftLimitEnabled);
   shoulderTalonSRX.configReverseSoftLimitThreshold(kSoftLimitReverse);

   // enables current limits on the motor
   shoulderTalonSRX.enableCurrentLimit(true);
   shoulderTalonSRX.configPeakCurrentLimit(kPeakCurrentLimit);
   shoulderTalonSRX.configPeakCurrentDuration(kPeakCurrentDuration);
   System.out.println("shoulderLift.configureShoulder END");
}

public void syncEncoderValues(){
    System.out.println("shoulderLift.syncEncoderValues START");

/**
* Grab the 360 degree position of the MagEncoder's absolute
* position, and intitally set the relative sensor to match.
*/
int absolutePosition = shoulderTalonSRX.getSensorCollection().getPulseWidthPosition();

/* Mask out overflows, keep bottom 12 bits */
absolutePosition &= 0xFFF;
if (kSensorPhase) { absolutePosition *= -1; }
if (kMotorInvert) { absolutePosition *= -1; }

/* Set the quadrature (relative) sensor to match absolute */
shoulderTalonSRX.setSelectedSensorPosition(absolutePosition, kPIDLoopIdx, kTimeoutMs);
System.out.println("shoulderLift.syncEncoderValues END");
}

public boolean isEncoderSynced()
{
    System.out.println("shoulderLift.isEncoderSynced START");
    /**
     * Grab the 360 degree position of the MagEncoder's absolute
     * position, and intitally set the relative sensor to match.
     */
    int absolutePosition = shoulderTalonSRX.getSensorCollection().getPulseWidthPosition();

    /* Mask out overflows, keep bottom 12 bits */
    absolutePosition &= 0xFFF;
    if (kSensorPhase) { absolutePosition *= -1; }
    if (kMotorInvert) { absolutePosition *= -1; }

    /* Set the quadrature (relative) sensor to match absolute */
    int relativePosition= shoulderTalonSRX.getSelectedSensorPosition(kPIDLoopIdx);

    if (relativePosition >= (absolutePosition-encoderMariginOfError) &&
        relativePosition <= (absolutePosition+encoderMariginOfError))
        {
            System.out.println("shoulderLift.isEncoderSynced END: true");
            return true;
        }
    else 
        {
            System.out.println("shoulderLift.isEncoderSynced END: true");
            return false;
        }
}

}

THIS IS OUR COMMAND CODE
NOTE THIS COMMAND IS PART OF A GROUP COMMAND (BELOW)
// RobotBuilder Version: 2.0
//
// This file was generated by RobotBuilder. It contains sections of
// code that are automatically generated and assigned by robotbuilder.
// These sections will be updated in the future when you export to
// Java from RobotBuilder. Do not put any code or make any change in
// the blocks indicating autogenerated code or it will be lost on an
// update. Deleting the comments indicating the section will prevent
// it from being updated in the future.

package org.usfirst.frc5053.FRC2019DeepSpace.commands;
import edu.wpi.first.wpilibj.command.Command;
import org.usfirst.frc5053.FRC2019DeepSpace.Robot;

/**
*
*/
public class shoulderAutoPosition extends Command {

// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=VARIABLE_DECLARATIONS
private double m_shoulderPosition;

// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=VARIABLE_DECLARATIONS

// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTOR
public shoulderAutoPosition(double shoulderPosition) {

// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTOR
    // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=VARIABLE_SETTING
    m_shoulderPosition = shoulderPosition;

    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=VARIABLE_SETTING
    // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=REQUIRES
    requires(Robot.shoulderLift);

// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=REQUIRES
}

// Called just before this Command runs the first time
@Override
protected void initialize() {
}

// Called repeatedly when this Command is scheduled to run
@Override
protected void execute() {
    Robot.shoulderLift.startMoveToPosition(m_shoulderPosition);
}

// Make this return true when this Command no longer needs to run execute()
@Override
protected boolean isFinished() {
    return false;
    // return true;
}

// Called once after isFinished returns true
@Override
protected void end() {
}

// Called when another command which requires one or more of the same
// subsystems is scheduled to run
@Override
protected void interrupted() {
}

}

THIS IS OUR GROUP COMMAND THAT GETS EXECUTED FROM SHUFFLEBOARD

// RobotBuilder Version: 2.0
//
// This file was generated by RobotBuilder. It contains sections of
// code that are automatically generated and assigned by robotbuilder.
// These sections will be updated in the future when you export to
// Java from RobotBuilder. Do not put any code or make any change in
// the blocks indicating autogenerated code or it will be lost on an
// update. Deleting the comments indicating the section will prevent
// it from being updated in the future.

package org.usfirst.frc5053.FRC2019DeepSpace.commands;

import edu.wpi.first.wpilibj.command.CommandGroup;
import org.usfirst.frc5053.FRC2019DeepSpace.subsystems.*;

/**
*
*/
public class operateArm extends CommandGroup {

// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=PARAMETERS
public operateArm(double shoulderPosition, double wristPosition, double rightDistance, double leftDistance) {

// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=PARAMETERS
    // Add Commands here:
    // e.g. addSequential(new Command1());
    //      addSequential(new Command2());
    // these will run in order.

    // To run multiple commands at the same time,
    // use addParallel()
    // e.g. addParallel(new Command1());
    //      addSequential(new Command2());
    // Command1 and Command2 will run in parallel.

    // A command group will require all of the subsystems that each member
    // would require.
    // e.g. if Command1 requires chassis, and Command2 requires arm,
    // a CommandGroup containing them would require both the chassis and the
    // arm.
    // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=COMMAND_DECLARATIONS
    //addParallel(new distanceAutoPosition(0, 0));
    //addParallel(new shoulderAutoPosition(0));
    //addParallel(new wristAutoPosition(0));

// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=COMMAND_DECLARATIONS

/**********************
 * Each time the JAVA is generated from robot builder, the automatically generated
 * code does not include the parameters.
 * 
 * After JAVA is generated from robot builder, comment out the addParallel 
 * commands in the AUTOGENERATED CODE ABOVE.
 ***********************/
    addParallel(new distanceAutoPosition(rightDistance, leftDistance));
    addParallel(new shoulderAutoPosition(shoulderPosition));
    addParallel(new wristAutoPosition(wristPosition));
}

}

THESE ARE THE ABSOLUTE ENCODER POSITIONS (shoulder, wrist, right distance, left distance)

public double[][] operateArmParameters =
{
{0, 0, 0, 0}, // 0: Not Used
{671, 2421, 1, 2}, // 1: Cargo ==> Rocket High
{1321, 2239, 3, 4}, // 2: Cargo ==> Rocket Middle
{2096, 1749, 5, 6}, // 3: Cargo ==> Rocket Low
{537, 2864, 7, 8}, // 4: Hatch ==> Rocket High
{1607, 2166, 9, 10}, // 5: Hatch ==> Rocket Middle
{2605, 1341, 11, 12}, // 6: Hatch ==> Rocket Low
{2605, 1341, 13, 14}, // 7: Hatch ==> Cargo Ship
{1632, 2321, 15, 16}, // 8: Cargo ==> Cargo Ship
{2605, 1341, 17, 18}, // 9: Hatch ==> Feeder (Human Player)
{1738, 2048, 19, 20}, // 10: Cargo ==> Feeder (Human Player)
{2931, 580, 21, 22}, // 11: Hatch ==> Transport Position
{2837, 1447, 23, 24}, // 12: Cargo ==> Ground Pickup
{106, 1710, 25, 26} // 13: Cargo ==> Rocket High (from back of Robot)
};

2

We have experienced an issue where after disabling and re-enabling the robot, the elevator and arm would quickly jump to the position they were in before the robot was disabled and then subsequently jump back. We fixed this issue by setting a neutral output using elevatorMaster.set(ControlMode.PercentOutput, 0.0) when the robot enters the disabled state.

4 Likes
4

Couple of items -

During Disable:

During disable, you should explicitly leave motion magic mode. This keeps your mechanism from immediately servoing back to the last set position when you re-enable.
Calling set(PercentOutput, 0) should be sufficient.

Comparing Absolute/Relative:

Also, when comparing the absolute and relative encoder values, you should poll both of the values from SensorCollection(). This prevents the values from being mismatched if sensorPhase or invert are set to true - the SensorCollection values are the raw sensor data and don’t reflect the sensorPhase/invert (unlike the SelectedSensorPosition, which does reflect sensorPhase/invert).

Encoder Values Not being synced:

If your values are synced on boot but later don’t match, it’s possible you have an intermittent connection to your sensor - when the wiring connection is good again, the absolute position will still be correct, but the relative position won’t have changed at all while the connection is bad.

To test this, you can do the following:

  1. Sync the absolute/relative values
  2. Move the arm up and down 10 times (either by hand or using PercentOutput), ending at the same position you started.
  3. Check if the values are still synced.

If the values are no longer synced after doing this, you likely have an intermittent connection between the Talon and the encoder. You can also use Phoenix Tuner to plot the position while moving the arm to see if the position stops increasing/decreasing at any point during the motion.

1 Like
5

To add more FRC, WPILIb command-based advice:

You can reset all the commands in the scheduler when you enter the Teleop and Autonomous init methods.

Scheduler.getInstance().removeAll();

Then in the interrupted() method, you can set your Arm motors to PercentOutput 0.0.

Well, what we do is in interrupted() we call end() , which sets the motors to 0.0. in percent output mode.

By and large though, teaching your students the command-based framework they should always be answering the three questions when designing a command:

  1. What subsystems will I need/interrupt when I call this command?
  2. What sensors and information need to be setup beforehand to ensure the command will run safely, and as expected?
  3. When is the command finished (when is isFinished() going to return true) and what should I do when I end (either voluntarily or not)?

Our Github is public if you want to use it as a reference point, or if you have any further questions about how things we do work (or don’t work :slight_smile: ) I’m happy to help.

1 Like
6

We never had rogue movements but out of the gate we added code to get the current position and set the target position to that so in the auto/teleop-init() this sequence was called. This was a fall back to my experience on how industrial servos often work.

1 Like
7

I want to clarify my previous statement:

If you disable and then re-enable while still in motion magic mode and still using the same target position, your mechanism will attempt to hold position using standard PID control (the same way it holds position at the end of a motion magic movement).
If your mechanism moves while disabled, this means that on re-enabling your mechanism will immediately attempt to position closed-loop back to the previous position.

If you explicitly leave motion magic while disabled, then on re-enabling (and re-entering motion magic mode and your previous target position) your mechanism will motion magic back to the target instead of using standard position PID.

8

Thanks, Jacob, for this additional information.

I admit, however, that I’m a little surprised by that behavior, as I don’t recall seeing it in the documentation on MotionMagic mode.

Are there other times that the TalonSRX will use standard position PID when the most recent “set()” command to the TalonSRX was a MotionMagic command? Is it the case that MotionMagic is only used for initial “long-range” movement to the set point, and that once the set point has been reached, only standard PID control is used?

Thanks!

9

Current

If the Talon/Victor is not in Motion Magic mode, it will sync up as expected. When it enters MM, the profiling starts. If the Talon/Victor is in MM and motor output is disrupted (due to robot-disable, limits, or psychical disconnect from motor) then the profile’s target will still approach and reach the final target. This has always been the case.

Future

I think detecting the robot-disable and resyncing so that it profiles from where its at is reasonable. I’ll write it up in our tracker.

10

Thanks, Omar, for the quick, thorough response.

Something that’s still not clear to me and that I haven’t been able to determine empirically (although I haven’t set up an experiment to find out specificially) is if the trapezoidal profile gets re-generated each and every time the .set() method is called, or if the earlier profile is “modified” in progress, or ?

I think a couple of practical examples (both from our robot arm implementation this year) might make my question easier to understand:

1 - We are using the 4-parameter .set() method, in order to use the ArbitraryFeedForward feature to provide “gravity compensation”). The call looks something like the below:

controller.set(ControlMode.MotionMagic, desiredPositionInCounts, DemandType.ArbitraryFeedForward, feedForwardInPercentVbus);

For feedForward, we are passing in k * cos(theta), where theta is the angle away from horizontal for the arm at the arm’s current position (basically, the position of the arm from the shoulder joint, relative to the ground.)

We re-compute the feedForward on every timed iteration through the robot code (i.e at 50hZ, every 20ms), and thus call

We are wondering if calling controller.set(ControlMode.MotionMagic, desiredPositionInCounts, DemandType.ArbitraryFeedForward, feedForwardInPercentVbus) again and again every 20ms with the same desiredPositionInCounts, but a different feedForwardInPercentVbus is continually causing Motion Magic (MM) to start at the beginning of the profile over and over again? If this is the case, do we need to avoid calling MM repeatedly with slightly different parameters than the previous .set() call, because every “set” call has to start the profile again from scratch?

2 - Same scenario as above, but consider a case where we are controlling the position of a 2nd arm segment. While the above control loop is controlling the “arm” position via a shoulder joint, there is a 2nd arm segment beyond a “wrist” joint. We are using motion magic for this segment with essentially the same call:

controller.set(ControlMode.MotionMagic, desiredPositionInCounts, DemandType.ArbitraryFeedForward, feedForwardInPercentVbus);

For feedForward, we are passing in k * cos(theta), where theta is the angle away from horizontal for the “wrist segment” at the “wrist’s current position.” This angle is computed as a function of both the shoulder angle and the wrist angle.
We also update this value and re-call .set() on every timed iteration (a rate of 50hz).

However, in some cases, our “desired set point” for the wrist segment isn’t an absolute position of the joint, relative to the arm, but is a position relative to the ground. (For example, say we want to move the wrist to a position horizontal to the ground.) Accordingly, the “desired wrist set point” that we send to the TalonSRX for the wrist varies while the shoulder joint is moving. So, in this case, the repeated calls we are making to .set() have not only a varying feed-forward position, but also a (slightly) varying “desiredPositionInCounts”.

We are wondering if this situation of calling controller.set(ControlMode.MotionMagic, desiredPositionInCounts, DemandType.ArbitraryFeedForward, feedForwardInPercentVbus) again and again every 20ms with a similar (but different) desiredPositionInCounts to the prior call, and a similar (but different) feedForwardInPercentVbus is continually causing Motion Magic (MM) to start at the beginning of the profile over and over again?

Thanks!

PS: If we’re going about this all wrong, and there’s a better way to do multi-joint arm control with the TalonSRX features, we’re all ears!

Best regards,
–ken

11

Your approach is correct.

Calling set(4 params) does not reset anything in the profile - it simply updates the “final” target position that the Talon/Victor should shoot for.

You can prove this by plotting the Active Trajectory Position and Velocity while changing the (final) target you pass into set(). Note that the target velocity still ramps/curves regardless of how often you call set().

That’s why the GitHub motion magic example wires the set() call to the joystick. So you can change the target on-the-fly with a periodic call, and observe the response (thereby gain intuition on how it works).

Calling set() periodically may be important if you are using MotorSafety (which defaults off in our MC classes - and defaults on in WPILIBs drivetrains).

The reason why the disable-circumstance is unique because the Talon/Victor continues to produce the ramp profile, and calculating the motor output to get there. MM controller doesn’t know that the motor output is disabled and to not expect the sensor to move for other reasons. When you change control mode, Talon/Victor re-synchronizes in preparation for the next MM event.

In hindsight, we can additionally sync up when the motor controller is disabled. That’s trivial. I don’t think that will be done mid-comp season though, unless enough people ask for it.

Makes sense?

1 Like
12

Thanks so much for confirming this for us!

Sounds like we should trying doing that – we haven’t done that yet!

13

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