Chief Delphi - Talon SRX spamming forward and backward constantly

Chief Delphi (http://www.chiefdelphi.com/forums/index.php)

- Programming (http://www.chiefdelphi.com/forums/forumdisplay.php?f=51)

- - Talon SRX spamming forward and backward constantly (http://www.chiefdelphi.com/forums/showthread.php?t=133884)

Talon SRX spamming forward and backward constantly

Hello everybody,

So our robot's Talon's are reversing forward and backward during teleOP and Auton. During the Auton program it's set to move forward a single motor at half speed for half a second then stop. What actually happens is 2 motors continually go forward and backward at full speed(I think). We have the Talons in a CAN daisy chain sequence and are using c++ to program it. Any help would be much appreciated.

Re: Talon SRX spamming forward and backward constantly

Can you post the code you're using to control said motors? (Just to eliminate the possibility of the code being the issue).

Re: Talon SRX spamming forward and backward constantly

Try calibrating it. That might solve the problem if it is not codes problem.

Re: Talon SRX spamming forward and backward constantly

My spitball guess is that both Talons have the same CAN address, and one is on the left side of the robot, and the other is on the right side of the robot. When you send the command, both motors respond by rotating in the same direction. However, as the motors are oriented in opposite directions, one of them appears to be going backward.

Re: Talon SRX spamming forward and backward constantly

Do you ever explicitly tell the motors to stop moving?

Re: Talon SRX spamming forward and backward constantly

I think Wenry may be in class, so his responses will be a bit delayed. I was at the build last night and helped him with his troubleshooting, so I will take a stab at answering your questions.

@Asymons - I don't have direct access to the code that was being used (it hasn't been pushed to our repo yet), but the final test was really simple. I'll try to replicate it here.

We had a command with the following methods:

Code:

void TestTalonCommand::Initialize()

{

        drivetrain->setMotorSpeed(1, 0.5);

}



void TestTalonCommand::End()

{

        drivetrain->setMotorSpeed(1, 0);

}

The command requires the drivebase subsystem, and it uses the built in timing from Command to run for 0.5 seconds.

The setMotorSpeed method is in our subclass of RobotDrive, and implemented like this:

Code:

void Drive::setMotorSpeed(unsigned int motorID, float speed)

{

        motors[motorID]->Set(speed);

}

motors is a vector created in the constructor, and each object is instantiated with "new CANTalon(canID);", where there are different CAN IDs for each of our Talon SRXes.

The Talons are in kThrottle or kPercentVbus mode (Both are names for ControlMode 0).

@Munchskull - I was wondering about that, but since the robot uses CAN instead of PWM I didn't think normal calibration applied. At least, I couldn't find anything in the Talon SRX User Guide for calibrating the Talons for CAN. Do you have documentation on how we could do this?

@GeeTwo - All of the Talons have unique IDs in the web interface. They are also all using the latest (1.4) version of the firmware.

@Christopher149 - Yes. This is the part that is exceptionally puzzling. When the move command is sent (either by moving the joystick or by calling Set()) the motors begin moving. However, when the joystick is returned to center or the command ends, the motors do not stop until the robot is disabled.

Thanks to all of you for responding.

Re: Talon SRX spamming forward and backward constantly

PWM Calibration has no effect when controlling Talon SRXs over CAN bus.
It sounds like a programming issue, can you just post it in it's entirety? Or PM me a link and I can run it on my robot.

Re: Talon SRX spamming forward and backward constantly

Sorry if this doesn't look pretty i'm new to forms. Also if i should add anything else just let me know please. Thank you for helping

TestTalonCommand.cpp file

Code:

#include "TestTalonCommand.h"



TestTalonCommand::TestTalonCommand()

        : TimedCommand("TestTalonCommand", 0.5)

{

        Requires(drivetrain);

}



void TestTalonCommand::Initialize()

{

        drivetrain->setMotorSpeed(1,0.5);

}



void TestTalonCommand::End()

{

        drivetrain->setMotorSpeed(1,0);

}

TestTalonCommand.h file

Code:

#ifndef TEST_TALON_COMMAND_H

#define TEST_TALON_COMMAND_H



#include "../Types/TimedCommand.h"



class TestTalonCommand : public TimedCommand {



public:

        TestTalonCommand();

        void Initialize();

        void End();

};



#endif

Mecanum.cpp

Code:

#include "MecanumDriveTrain.h"



#include "../RobotMap.h"

#if defined(MECANUM_DRIVE)



#include "../Custom/DriveTrain/EncoderDrive.h"

#include <CANTalon.h>

#include <Gyro.h>



#include "../Custom/Netconsole.h"



MecanumDriveTrain::MecanumDriveTrain() : DriveTrain("MecanumDriveTrain")

{

        frontRightMotor = new CANTalon(RobotMap::DriveBase::frontRightMotorID);

        frontLeftMotor = new CANTalon(RobotMap::DriveBase::frontLeftMotorID);

        backRightMotor = new CANTalon(RobotMap::DriveBase::backRightMotorID);

        backLeftMotor = new CANTalon(RobotMap::DriveBase::backLeftMotorID);





        

        gyro = new Gyro(RobotMap::DriveBase::gyroPort);

        gyro->SetSensitivity(RobotMap::DriveBase::gyroSensitivity);



        drive = new EncoderDrive(frontLeftMotor, backLeftMotor, frontRightMotor, backRightMotor);

        drive->SetSafetyEnabled(false);

        drive->ignoreEncoders();

        

        DEBUG_MOTOR(frontLeftMotor);

        DEBUG_MOTOR(frontRightMotor);

        DEBUG_MOTOR(backLeftMotor);

        DEBUG_MOTOR(backRightMotor);

}



MecanumDriveTrain::~MecanumDriveTrain()

{

        delete frontLeftMotor;

        delete frontRightMotor;

        delete backLeftMotor;

        delete backRightMotor;



        delete gyro;

}



void MecanumDriveTrain::move(float xValue, float yValue, float rotate)

{

        drive->MecanumDrive(xValue, yValue, rotate, gyro->GetAngle());

}



void MecanumDriveTrain::stop()

{

        drive->MecanumDrive(0.0, 0.0, 0.0);

}



void MecanumDriveTrain::setMotorSpeed(RobotMap::CanID motorID, float speed)

{        drive->setMotorSpeed(motorID,speed);



}





#endif

MecanumDriveTrain.h file

Code:

#ifndef MECANUM_DRIVE_TRAIN_H

#define MECANUM_DRIVE_TRAIN_H



#include "DriveTrain.h"



class CANTalon;

class Gyro;



class MecanumDriveTrain: public DriveTrain {

public:

        MecanumDriveTrain();

        virtual ~MecanumDriveTrain();

        void setMotorSpeed(RobotMap::CanID motorID, float speed);

        

        void move(float xValue, float yValue, float rotateValue);

        virtual void stop();



protected:

        CANTalon* frontLeftMotor;

        CANTalon* frontRightMotor;

        CANTalon* backLeftMotor;

        CANTalon* backRightMotor;



        Gyro* gyro;

};



#define MOVE_WITH_JOYSTICK\

        move(\

                oi->getAxisValue(logicalAxes::DriveX),\

                oi->getAxisValue(logicalAxes::DriveY),\

                oi->getAxisValue(logicalAxes::DriveRotate)\

        )



#endif

EncoderDrive.h file
#ifndef ENCODER_DRIVE_H

Code:

#define ENCODER_DRIVE_H



#include <RobotDrive.h>



#include "../../RobotMap.h"



#include <vector>



class SpeedController;

class CANTalon;

class Encoder;

class RatePIDController;

class PIDSource;



class EncoderDrive : public RobotDrive {



public:

        EncoderDrive(

                SpeedController* leftMotor,

                SpeedController* rightMotor,

                Encoder* leftEncoder,

                Encoder* rightEncoder

        );



        EncoderDrive(

                CANTalon* frontLeftMotor,

                CANTalon* backLeftMotor,

                CANTalon* frontRightMotor,

                CANTalon* backRightMotor

        );



        virtual void SetLeftRightMotorOutputs(float leftOutput, float rightOutput);

        virtual void MecanumDrive(double x, double y, double rotate, double angle=0.0);

        void setMaxSpeed(float speed);

        void setMotorSpeed(RobotMap::CanID motorID, float speed);

        void ignoreEncoders();



protected:

        float m_maxSpeed;



        bool m_brokenEncoder;



        std::vector<PIDSource*> sensors;

        std::vector<RatePIDController*> outputs;

        std::vector<SpeedController*> motors;

        std::vector<float> speeds;



        void equalizeMotors();

        void addMotor(PIDSource* sensor, SpeedController* motor);

};



#endif

EncoderDrive.cpp file

Code:

#include "EncoderDrive.h"



#include <cmath>

#include <iterator>

#include <algorithm>



#include <CANTalon.h>

#include <Encoder.h>



#include "RatePIDController.h"

#include "EncoderRatePIDSource.h"

#include "CANTalonRatePIDSource.h"



#include "../../RobotMap.h"



#include "../Netconsole.h"



EncoderDrive::EncoderDrive(

        SpeedController* leftMotor,

        SpeedController* rightMotor,

        Encoder* leftEncoder,

        Encoder* rightEncoder

): RobotDrive(leftMotor, rightMotor),

        m_maxSpeed(RobotMap::DriveBase::maxSpeed),

        m_brokenEncoder(false)

{

        addMotor(new EncoderRatePIDSource(leftEncoder), leftMotor);

        addMotor(new EncoderRatePIDSource(rightEncoder), rightMotor);



        speeds.resize(motors.size());

}



EncoderDrive::EncoderDrive(

        CANTalon* frontLeftMotor,

        CANTalon* backLeftMotor,

        CANTalon* frontRightMotor,

        CANTalon* backRightMotor

): RobotDrive(frontLeftMotor, backLeftMotor, frontRightMotor, backRightMotor),

        m_maxSpeed(RobotMap::DriveBase::maxSpeed),

        m_brokenEncoder(false)

{

        addMotor(new CANTalonRatePIDSource(frontLeftMotor), frontLeftMotor);

        addMotor(new CANTalonRatePIDSource(backLeftMotor), backLeftMotor);

        addMotor(new CANTalonRatePIDSource(frontRightMotor), frontRightMotor);

        addMotor(new CANTalonRatePIDSource(backRightMotor), backRightMotor);



        speeds.resize(motors.size());

}



void EncoderDrive::setMaxSpeed(float speed)

{

        m_maxSpeed = speed;

}



void EncoderDrive::SetLeftRightMotorOutputs(float leftOutput, float rightOutput)

{

        if (m_brokenEncoder)

        {

                RobotDrive::SetLeftRightMotorOutputs(leftOutput, rightOutput);

                return;

        }



        speeds[0] = leftOutput;

        speeds[1] = -rightOutput; // Should we set this motor inverted?

        equalizeMotors();

}





void EncoderDrive::MecanumDrive(double x, double y, double rotate, double angle)

{        

        



        if (m_brokenEncoder)

        {

                Netconsole::print<double>("X", x);

                Netconsole::print<double>("Y", y);

                Netconsole::print<double>("Rotate", rotate);

                

                RobotDrive::MecanumDrive_Cartesian(x, -y, rotate, angle);

                return;

        }



        static bool reported = false;

        if (!reported)

        {

                HALReport(

                        HALUsageReporting::kResourceType_RobotDrive,

                        motors.size(),

                        HALUsageReporting::kRobotDrive_MecanumCartesian

                );

                reported = true;

        }



        // Compensate for gyro angle.

        RotateVector(x, y, angle);



        speeds[kFrontLeftMotor] = x + y + rotate;

        speeds[kFrontRightMotor] = -x + y - rotate;

        speeds[kRearLeftMotor] = -x + y + rotate;

        speeds[kRearRightMotor] = x + y - rotate;



        // Normalize joystick values

        float maxSpeed = std::abs(speeds[kFrontLeftMotor]);

        for (int i = 1; i < kMaxNumberOfMotors; i++)

        {

                float currentSpeed = std::abs(speeds[i]);

                if (currentSpeed > maxSpeed)

                {

                        maxSpeed = currentSpeed;

                }

        }

        if (maxSpeed > 1.0)

        {

                for (int i=0; i < kMaxNumberOfMotors; i++)

                {

                        speeds[i] = speeds[i] / maxSpeed;

                }

        }



        equalizeMotors();

}



void EncoderDrive::addMotor(PIDSource* sensor, SpeedController* motor)

{

        sensors.push_back(sensor);

        motors.push_back(motor);



        RatePIDController* output = new RatePIDController(

                RobotMap::DriveBase::P,

                RobotMap::DriveBase::I,

                RobotMap::DriveBase::D,

                1,

                sensor,

                motor

        );

        output->SetInputRange(-m_maxSpeed, m_maxSpeed);

        output->SetPercentTolerance('1');

        output->Enable();

        outputs.push_back(output);

}





void EncoderDrive::equalizeMotors()

{

        int motorCount = motors.size();

        float maxInput = 1;

        float currentInput = 0;

        float peggedInput = 0;

        int peggedIndex = -1;



        // If one motor is pegged, don't let the others pass it

        // NOTE: Are there cases where motors could be pegged at different values?

        for (int i=0; i < motorCount; i++)

        {

                currentInput = std::abs(motors[i]->Get());

                if (currentInput > .99)

                {

                        peggedInput = std::abs(sensors[i]->PIDGet()) / m_maxSpeed;

                        if (peggedInput < maxInput)

                        {

                                maxInput = peggedInput;

                                peggedIndex = i;

                        }

                }

        }



        if (peggedIndex != -1)

        {

                for (int i=0; i < motorCount; i++)

                {

                        if (i == peggedIndex)

                        {

                                continue;

                        }



                        if (speeds[i] >= 0)

                        {

                                speeds[i] = std::min(speeds[i], maxInput);

                        }

                        else

                        {

                                speeds[i] = std::max(speeds[i], -maxInput);

                        }



                }

        }

        for (int i=0; i < motorCount; i++)

        {

                outputs[i]->SetSetpoint(speeds[i] * m_maxSpeed);

        }



        m_safetyHelper->Feed();

}



void EncoderDrive::ignoreEncoders()

{

        m_brokenEncoder = true;

}



void EncoderDrive::setMotorSpeed(RobotMap::CanID motorID, float speed)

{

        motors[motorID]->Set(speed);

}

Re: Talon SRX spamming forward and backward constantly

Also here is the link to our repository https://github.com/FRC2539/command-based/tree/rrush. Thanks all for the help.

Re: Talon SRX spamming forward and backward constantly

Arrived at the build site, here's what we're dealing with https://www.youtube.com/watch?v=_ALFjkahQuk. Thanks again for the help.

Re: Talon SRX spamming forward and backward constantly

Quote:

Originally Posted by Munchskull (Post 1437508)

Try calibrating it. That might solve the problem if it is not codes problem.

We looked into that but we're using CAN instead of PWM but calibration would only affect PWM. Thanks for the idea though.

Re: Talon SRX spamming forward and backward constantly

Quote:

Originally Posted by GeeTwo (Post 1437524)

We looked at the CAN addresses and there all different. Also when we enabled the robot we found that determining which wheels will turn on is never consistent. Thanks for the suggestion though.

Re: Talon SRX spamming forward and backward constantly

Quote:

Originally Posted by Christopher149 (Post 1437531)

Do you ever explicitly tell the motors to stop moving?

Yes in our autonomous program it's only set to run for half a second, however that never happens but thanks anyway. See the you-tube video above.

Re: Talon SRX spamming forward and backward constantly

We are having very similar problems with our drive system. We are running Talon SRX's using CAN and mechanum drive using C++ IterativeRobot. We have some motors not turning and others spin slowly in the wrong direction while other motors are running at full speed. We are also seeing some CAN errors from the PDB not responding on the DriverStation messages.

The Talons all have unique CAN IDs and have had a firmware update (from memory, I think they're running version 1.4). Same with the PDB (I think it's firmware level is 1.37). We are driving the motors using the RobotDrive.

2539: If you figure this out, please post your solution. I was going to try increasing the control period on the CANTalon's, but I'm not confident this will work.

Thanks

Re: Talon SRX spamming forward and backward constantly

There is definitely something wrong with that robot app. Running it on my bot it looks like two things are trying to drive all the Talons, it keeps switching between zero and nonzero values. I'll keep digging...

Re: Talon SRX spamming forward and backward constantly

Quote:

Originally Posted by ozrien (Post 1437938)

Wow thanks, over here we're fairly lost. If you're robot is working fine with your code, can you send us the drive part to see if it's in our code or hardware? And thanks again for all the help.

Re: Talon SRX spamming forward and backward constantly

Well to sanity checks things I grabbed the stock example at ...
http://wpilib.screenstepslive.com/s/...-mecanum-drive

...and tweaked it a bit. It is just mechanum drive with CANTalons and no gyro. I...

-changed the Talons to CANTalons and used 1,3,4,5 for the device IDs to match your bot. Although I did NOT cross-reference your code to make sure the number-to-positions matched (LF,LR,RF,RR).
-I increased the motor safety timeout to 500ms. Seems like the default is not long enough to cover the time between enabling and getting to the first TeleopOpLoop.
-Changed the joystick index to 0 from 1.

To use this example just create a new IterativeRobot and paste this into Robot.cpp.

Code:

#include "WPILib.h"/**

 * Simplest program to drive a robot with mecanum drive using a single Logitech

 * Extreme 3D Pro joystick and 4 drive motors connected as follows:

 *   - Digital Sidecar 1:

 *     - PWM 1 - Connected to front left drive motor

 *     - PWM 2 - Connected to rear left drive motor

 *     - PWM 3 - Connected to front right drive motor

 *     - PWM 4 - Connected to rear right drive motor

 */

class MecanumDefaultCode : public IterativeRobot

{

        CANTalon lf; /*left front */

        CANTalon lr;/*left rear */

        CANTalon rf; /*right front */

        CANTalon rr; /*right rear */

public:

        RobotDrive *m_robotDrive;                // RobotDrive object using PWM 1-4 for drive motors

        Joystick *m_driveStick;                        // Joystick object on USB port 1 (mecanum drive)public:

        /**

         * Constructor for this "MecanumDefaultCode" Class.

         */

        MecanumDefaultCode(void) : lf(1), lr(3), rf(4), rr(5)

        {

                /* Set every Talon to reset the motor safety timeout. */

                lf.Set(0);

                lr.Set(0);

                rf.Set(0);

                rr.Set(0);

                // Create a RobotDrive object using PWMS 1, 2, 3, and 4

                m_robotDrive = new RobotDrive(lf, lr, rf, rr);

                m_robotDrive->SetExpiration(0.5);

                // Define joystick being used at USB port #0 on the Drivers Station

                m_driveStick = new Joystick(0);

                // Twist is on Axis 3 for the Extreme 3D Pro

                m_driveStick->SetAxisChannel(Joystick::kTwistAxis, 3);

        }

        /**

         * Gets called once for each new packet from the DS.

         */

        void TeleopPeriodic(void)

        {

                m_robotDrive->MecanumDrive_Cartesian(m_driveStick->GetX(), m_driveStick->GetY(), m_driveStick->GetTwist());

        }

};

START_ROBOT_CLASS(MecanumDefaultCode);

Re: Talon SRX spamming forward and backward constantly

If you don't have any load on the drive wheels when you try to control the speed in closed-loop mode, you're very likely to get wildly oscillating wheels as they overshoot the target speed and try to correct.

Try giving them some load, like a patch of carpet pressed lightly against the rollers. You might also try adding some rate limiting on the motor power to keep it from thrashing so badly.

Re: Talon SRX spamming forward and backward constantly

Our head mentor uploaded some new code to get and it seemed to work. However the controls are off. See the video. https://www.youtube.com/watch?v=uDY6QJa7FKw Thanks everyone for there help and especially ozrien for putting the code on your robot. Tomorrow I'll post how he was able to fix it but for now sleep and class.

Re: Talon SRX spamming forward and backward constantly

Got this working on my bot. I used 3,1,4,5 for LeftFront, LeftRear, RightFront, RightRear respectively. Also added gyro an analog0 so holds absolute angle.
-I use the F350-similar gamepad (the wireless version), so I changed the three axes to X,Y,Z.
-Also I added button5 so that it will reset angle, thus allowing you to reselect the angle heading to servo to.
-Also inverted my right side since right-side motors have to drive negative (red) to move robot forward.

Actually I've been playing with this for about half an hour, driving it around the CTR office, so it seems pretty good.

Code:

#include "WPILib.h"

class MecanumDefaultCode : public IterativeRobot

{

        CANTalon lf; /*left front */

        CANTalon lr;/*left rear */

        CANTalon rf; /*right front */

        CANTalon rr; /*right rear */

public:

        RobotDrive *m_robotDrive;                // RobotDrive object using PWM 1-4 for drive motors

        Joystick *m_driveStick;                        // Joystick object on USB port 1 (mecanum drive)public:

        Gyro gyro;

        /**

         * Constructor for this "MecanumDefaultCode" Class.

         */

        MecanumDefaultCode(void) : lf(3), lr(1), rf(4), rr(5), gyro(0)

        {

                /* Set every Talon to reset the motor safety timeout. */

                lf.Set(0);

                lr.Set(0);

                rf.Set(0);

                rr.Set(0);

                // Create a RobotDrive object using PWMS 1, 2, 3, and 4

                m_robotDrive = new RobotDrive(lf, lr, rf, rr);

                m_robotDrive->SetExpiration(0.5);

                m_robotDrive->SetSafetyEnabled(false);

                // Define joystick being used at USB port #0 on the Drivers Station

                m_driveStick = new Joystick(0);

        }

        void TeleopInit()

        {

                gyro.Reset();

        }

        /** @return 10% deadband */

        double Db(double axisVal)

        {

                if(axisVal < -0.10)

                        return axisVal;

                if(axisVal > +0.10)

                        return axisVal;

                return 0;

        }

        /**

         * Gets called once for each new packet from the DS.

         */

        void TeleopPeriodic(void)

        {

                float angle = gyro.GetAngle();

                //std::cout << "Angle : " << angle << std::endl;

                m_robotDrive->MecanumDrive_Cartesian(        Db(m_driveStick->GetX()),

                                                                                                Db(m_driveStick->GetY()),

                                                                                                Db(m_driveStick->GetZ()),

                                                                                                angle);

                /* my right side motors need to drive negative to move robot forward */

                m_robotDrive->SetInvertedMotor(RobotDrive::kFrontRightMotor,true);

                m_robotDrive->SetInvertedMotor(RobotDrive::kRearRightMotor,true);

                /* on button 5, reset gyro angle to zero */

                if(m_driveStick->GetRawButton(5))

                        gyro.Reset();

        }

};

START_ROBOT_CLASS(MecanumDefaultCode);

Re: Talon SRX spamming forward and backward constantly

Quote:

Originally Posted by Wenry_Hill (Post 1438015)

but for now sleep and class.

(Off topic) Sorry, I couldn't let this pass by. Sleep and Class seem to go together, eh?

Re: Talon SRX spamming forward and backward constantly

Quote:

Originally Posted by GeeTwo (Post 1437524)

This cannot happen...

Quote:

Although the roboRIO Web-based Configuration is tolerant of “common ID” Talon SRXs to a
point, the robot programming API will not enable/control “common ID” Talons reliably.

...section 3.1 in Talon SRX Software Reference Manual.

Basically two Common-ID talons can't be enabled at the same time. One will win while the other stays disabled. Also there will be instances when both disable. When a Talon SRX is disabled it will blink orange each LED in an alternate pattern (think rail road crossing). See Talon User Guide for LED table.

Re: Talon SRX spamming forward and backward constantly

So it turned out that we only disabled half of our encoders. We had old code running the encoders even though we bypassed the joystick inputs. Whenever we tried to move our motors the old code kicked in and tried to bring the wheels back to 0, but overshot and just continued to do so. Thanks everyone for all the help! So mandrews281 make sure you have all your PID loops disabled.

Re: Talon SRX spamming forward and backward constantly

There are no PIDs in our code. It's only 4 TalonSRX, a robotdrive, a Gyro (disabled I think, need to check with my programming captain) and joystick. I saw 1452-Leo's solution here:

http://www.chiefdelphi.com/forums/sh...5&postcount=20

and I'm thinking that's the way to go. Our electrical team has our test base disassembled at the moment, I'll have to try this tomorrow.

Re: Talon SRX spamming forward and backward constantly

Quote:

We are also seeing some CAN errors from the PDB not responding on the DriverStation messages.

Are you still seeing this? Also, what does that mean exactly?

Is your problem that you are seeing motor safety errors in the DS AND as a result your motors are neutral (solid orange)? Please be specific.