View Single Post
  #15   Spotlight this post!  
Unread 05-02-2015, 21:27
ozrien's Avatar
ozrien ozrien is offline
Omar Zrien
AKA: Omar
no team
Team Role: Mentor
 
Join Date: Sep 2006
Rookie Year: 2003
Location: Sterling Heights, MI
Posts: 531
ozrien has a reputation beyond reputeozrien has a reputation beyond reputeozrien has a reputation beyond reputeozrien has a reputation beyond reputeozrien has a reputation beyond reputeozrien has a reputation beyond reputeozrien has a reputation beyond reputeozrien has a reputation beyond reputeozrien has a reputation beyond reputeozrien has a reputation beyond reputeozrien has a reputation beyond repute
Re: [C++] Use CAN Talons in RobotDrive

The problem looks like it's motor safety tripping. I reproduced the symptom with a similar project. Either the robot hits the motor safety trip once it boots up, or it fires during disable-to-enable transitions. When this happens the driver station will throw the MotorSafetyHelper assert (Section 16.14 in Talon SRX software reference manual), and the self-test will report "No-drive ".

I bet if you just called m_robotDrive->SetLeftRightMotorOutputs(0,0) in the disabled loop, that will keep feeding the motor safety object often enough to prevent your problem.

Or alternatively just TURN OFF the feature with...
m_robotDrive->SetSafetyEnabled(false)

Using dummy PWM objects seems like a drastic workaround.

Here's what I tested. If you comment out DisabledPeriodic(), you can reproduce the problem.

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)
	{
		// Create a RobotDrive object using PWMS 1, 2, 3, and 4
		m_robotDrive = new RobotDrive(lf, lr, rf, rr);

		/* alternatively you can disable safety features if you are not source-level debugging.
		 * uncomment this line to disable safety features. */
		//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;
	}

	void DisabledPeriodic(void)
	{
		/* while we are disabled, keep calling robotDrive's
		 * set routine to keep feeding the safety helper */
		m_robotDrive->SetLeftRightMotorOutputs(0,0);
	}
	/**
	 * Gets called once for each new packet from the DS.
	 */
	void TeleopPeriodic(void)
	{
		/* grab angle */
		float angle = gyro.GetAngle();

		/* printf for debugging - leave commented so it does affect performance. */
		//std::cout << "Angle : " << angle << std::endl;

		/* use angle with mec drive */
		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);
	}
};
START_ROBOT_CLASS(MecanumDefaultCode);