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.
Can you post the code you’re using to control said motors? (Just to eliminate the possibility of the code being the issue).
Try calibrating it. That might solve the problem if it is not codes problem.
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.
Do you ever explicitly tell the motors to stop moving?
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.
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.
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*);
if (currentSpeed > maxSpeed)
{
maxSpeed = currentSpeed;
}
}
if (maxSpeed > 1.0)
{
for (int i=0; i < kMaxNumberOfMotors; i++)
{
speeds* = speeds* / 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*->Get());
if (currentInput > .99)
{
peggedInput = std::abs(sensors*->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* >= 0)
{
speeds* = std::min(speeds*, maxInput);
}
else
{
speeds* = std::max(speeds*, -maxInput);
}
}
}
for (int i=0; i < motorCount; i++)
{
outputs*->SetSetpoint(speeds* * m_maxSpeed);
}
m_safetyHelper->Feed();
}
void EncoderDrive::ignoreEncoders()
{
m_brokenEncoder = true;
}
void EncoderDrive::setMotorSpeed(RobotMap::CanID motorID, float speed)
{
motors[motorID]->Set(speed);
}
Also here is the link to our repository https://github.com/FRC2539/command-based/tree/rrush. Thanks all for the help.
Arrived at the build site, here’s what we’re dealing with https://www.youtube.com/watch?v=_ALFjkahQuk. Thanks again for the help.
We looked into that but we’re using CAN instead of PWM but calibration would only affect PWM. Thanks for the idea though.
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.
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.
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
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…
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.
Well to sanity checks things I grabbed the stock example at …
http://wpilib.screenstepslive.com/s/4485/m/13810/l/241862-driving-a-robot-using-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.
#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);
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.
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.
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.
#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);