Talon SRX spamming forward and backward constantly

[Wenry_Hill]

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.

[Asymons]

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

[Munchskull]

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

[GeeTwo]

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.

[Christopher149]

Do you ever explicitly tell the motors to stop moving?

[restouffer]

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:

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:

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.

[ozrien]

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.

[Wenry_Hill]

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

#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

#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

#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

#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

#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

#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);
}

[Wenry_Hill]

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

[Wenry_Hill]

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

[Wenry_Hill]

Originally Posted by Munchskull

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.

[Wenry_Hill]

Originally Posted by GeeTwo

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.

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.

[Wenry_Hill]

Originally Posted by Christopher149

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.

[mandrews281]

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

[ozrien]

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...