Chief Delphi - [C++] Use CAN Talons in RobotDrive

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

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

- - [C++] Use CAN Talons in RobotDrive (http://www.chiefdelphi.com/forums/showthread.php?t=133804)

1452-Leo

31-01-2015 19:40

[C++] Use CAN Talons in RobotDrive

We are using CAN for the first time and after a few hours of learning how to do it, wiring, and formatting the Talons, our drive code didn't work. I tried controlling each motor individually and it works perfectly, but we would like to use RobotDrive because we are using Mecanum wheels with a gyro. Here is the non-working code:

Code:

class Robot: public SampleRobot {

        Joystick driveStick;

        CANTalon leftFront;

        CANTalon leftBack;

        CANTalon rightFront;

        CANTalon rightBack;

        RobotDrive drive;

        Gyro gyro;

public:

        Robot() :

                driveStick(1),

                leftFront(1),

                leftBack(5),

                rightFront(2),

                rightBack(6),

                drive(leftFront, leftBack, rightFront, rightBack),

                gyro(0)

{}

        void OperatorControl() {

                gyro.InitGyro();

                gyro.Reset();

                while (IsOperatorControl() && IsEnabled()) {

                        drive.MecanumDrive_Cartesian(driveStick.GetX(),driveStick.GetY(),driveStick.GetThrottle(),gyro.GetAngle());

                }

        }

};

And here is the working code (I used tank drive to make it easier):

Code:

class Robot: public SampleRobot {

        Joystick driveStick;

        CANTalon leftFront;

        CANTalon leftBack;

        CANTalon rightFront;

        CANTalon rightBack;

        Gyro gyro;

public:

        Robot() :

                driveStick(1),

                leftFront(1),

                leftBack(5),

                rightFront(2),

                rightBack(6),

                gyro(0)

{}

        void OperatorControl() {

                gyro.InitGyro();

                gyro.Reset();

                while (IsOperatorControl() && IsEnabled()) {

                        leftFront.Set(-driveStick.GetY());

                        leftBack.Set(-driveStick.GetY());

                        rightFront.Set(driveStick.GetThrottle());

                        rightBack.Set(driveStick.GetThrottle());

                }

        }

};

Does anyone know why this doesn't work? Thanks!

viggy96

31-01-2015 19:53

Re: [C++] Use CAN Talons in RobotDrive

Have you tried connecting the TalonSRXs via the PWM? That may alleviate your problem. Could also check if your TalonSRXs have the latest firmware.

Check here: http://crosstheroadelectronics.com/T...7s%20Guide.pdf

ozrien

31-01-2015 20:09

Re: [C++] Use CAN Talons in RobotDrive

Maybe grab the self test from the Rio web based configuration? Maybe its not in percent throttle mode. I don't have the implementation of robot drive in front of me but I think it just uses Set().

James Kuszmaul

31-01-2015 20:10

Re: [C++] Use CAN Talons in RobotDrive

What sort of problems are you encountering when you try to run the mecanum drive code? Does it simply not do anything at all, does it move the motors around but not in the way you expect, or something else? If the motors are running but the robot is responding how you expect, you should double check that everything is plugged into the right ports, that all the motors are turning the right direction, etc.

1452-Leo

31-01-2015 20:28

Re: [C++] Use CAN Talons in RobotDrive

Quote:

Originally Posted by viggy96 (Post 1436749)

I updated all the Talons to 1.4 (the newest I think) and we're trying to use CAN instead of PWM. Using PWM would involve soldering the CAN wires which we don't want to do just yet.

Quote:

Originally Posted by ozrien (Post 1436753)

Maybe grab the self test from the Rio web based configuration? Maybe its not in percent throttle mode. I don't have the implementation of robot drive in front of me but I think it just uses Set().

I actually did do this earlier, but I had no idea what is means. When in drive, they are set to "No Drive" but otherwise set to "Throttle." This seems like it could be the problem if I knew more about it.

Quote:

Originally Posted by James Kuszmaul (Post 1436755)

Motors are not doing anything at all, but works fine when not using RobotDrive.

James Kuszmaul

31-01-2015 20:48

Re: [C++] Use CAN Talons in RobotDrive

try calling
robotDrive.SetSafetyEnabled(false);
before the while loop in OperatorControl.
Edit: actually, try calling that in the Robot() constructor. It's possible that the Talons are getting disabled before you start running the robot but that they aren't getting properly re-enabled.

1452-Leo

31-01-2015 20:54

Re: [C++] Use CAN Talons in RobotDrive

Quote:

Originally Posted by James Kuszmaul (Post 1436767)

try calling
robotDrive.SetSafetyEnabled(false);
before the while loop in OperatorControl.

I actually tried that earlier when I had some motor controllers set up wrong, but I forgot to try it again! I'll check back tomorrow and report if it worked or not.

ozrien

31-01-2015 20:57

Re: [C++] Use CAN Talons in RobotDrive

If you are seeing kNoDrive then most likely set is not being called. See software reference manual for more details.

1452-Leo

31-01-2015 21:12

Re: [C++] Use CAN Talons in RobotDrive

Quote:

Originally Posted by ozrien (Post 1436769)

If you are seeing kNoDrive then most likely set is not being called. See software reference manual for more details.

Can I just call Set() once at the beginning of teleop or is there something I have to change in RobotDrive? Unfortunately, I can't test until Monday.

TimTheGreat

31-01-2015 22:48

Re: [C++] Use CAN Talons in RobotDrive

Are you calling Wait(.01) in your code? That may cause the code to do nothing.

viggy96

01-02-2015 00:09

Re: [C++] Use CAN Talons in RobotDrive

Could also try creating a new project. Sometimes projects get messed up. On the surface, it may not seem like it matters, but it could be worth a try.

I reccomend you switch from using the SampleRobot project to using IterativeRobot and try the code again.

viggy96

01-02-2015 00:45

Re: [C++] Use CAN Talons in RobotDrive

This should work, but as I don't have a bot with TalonSRXs, I can't confirm this, but you could try it. I have used the IterativeRobot class.

Code:

class Robot: public IterativeRobot

{

private:

        LiveWindow *lw;



        CANTalon leftFront = new CANTalon(1);

        CANTalon leftBack = new CANTalon(5);

        CANTalon rightFront = new CANTalon(2);

        CANTalon rightBack = new CANTalon(6);



        RobotDrive drive = new RobotDrive(leftFront, leftBack, rightFront, rightBack);



        Joystick stick = new Joystick(1);



        Gyro gyro = new gyro(0);



        void RobotInit()

        {

                lw = LiveWindow::GetInstance();

        }



        void AutonomousInit()

        {



        }



        void AutonomousPeriodic()

        {



        }



        void TeleopInit()

        {



        }



        void TeleopPeriodic()

        {

                drive.MecanumDrive_Cartesian(stick.getX(), stick.getY(), stick.getThrottle(), gyro.getAngle());

        }



        void TestPeriodic()

        {

                lw->Run();

        }

};

1452-Leo

01-02-2015 16:29

Re: [C++] Use CAN Talons in RobotDrive

Quote:

Originally Posted by viggy96 (Post 1436847)

This should work, but as I don't have a bot with TalonSRXs, I can't confirm this, but you could try it. I have used the IterativeRobot class.

How would this be different than the code I started with? Iterative basically just adds a function to be called at the beginning, but doesn't really change what is actually going on in the program.

virtuald

02-02-2015 01:19

Re: [C++] Use CAN Talons in RobotDrive

Quote:

Originally Posted by 1452-Leo (Post 1437039)

How would this be different than the code I started with? Iterative basically just adds a function to be called at the beginning, but doesn't really change what is actually going on in the program.

Iterative waits for each driver station packet for each iteration, whereas your original code runs in a tight loop without waiting for input. If you added a 20ms sleep in your loop, it would be vaguely equivalent to the iterative version.

Either way, you should add some sleep to your loop, otherwise you'll starve the rest of the threads on your system and you'll encounter problems. I'd recommend using iterative robot instead though, then you don't have to worry about things like that.

1452-Leo

03-02-2015 10:14

Re: [C++] Use CAN Talons in RobotDrive

Quote:

Originally Posted by virtuald (Post 1437179)

Thanks, I didn't realize there was an actual difference in the how the program ran for iterative, so we will probably switch over to it today.

kmateti

05-02-2015 10:09

Re: [C++] Use CAN Talons in RobotDrive

Please post your progress... we have the same exact need, and are hard coding our drive code currently... CAN has so many benefits, but so does using RobotDrive...

Thanks!

1452-Leo

05-02-2015 10:15

Re: [C++] Use CAN Talons in RobotDrive

Quote:

Originally Posted by kmateti (Post 1438686)

Please post your progress... we have the same exact need, and are hard coding our drive code currently... CAN has so many benefits, but so does using RobotDrive...

Thanks!

We haven't had any time to test really, so we have just hardcoded the Mecanum drive with no gyro. I hope that we can get RobotDrive sorted out later today, but I'm not too sure. Of course I'll post the solution here though if we find it!

ozrien

05-02-2015 11:43

Re: [C++] Use CAN Talons in RobotDrive

This might help, I ran this code on my mec bot, with gyro and CANTalons. I use IterativeRobot (it's just my preference).

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

1452-Leo

05-02-2015 12:08

Re: [C++] Use CAN Talons in RobotDrive

Quote:

Originally Posted by ozrien (Post 1438725)

This might help, I ran this code on my mec bot, with gyro and CANTalons. I use IterativeRobot (it's just my preference).

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

Thanks! I'll have to try this later. I did just get RobotDrive working by putting this before the drive function:

Code:

leftFront->SetControlMode(CANSpeedController::kPercentVbus);

leftBack->SetControlMode(CANSpeedController::kPercentVbus);

rightFront->SetControlMode(CANSpeedController::kPercentVbus);

rightBack->SetControlMode(CANSpeedController::kPercentVbus);

There are problems with this solution though. For example, I can't reverse the motors anymore (it doesn't drive as soon as I add SetInvertedMotor in). I have another idea though that I will try later.

1452-Leo

05-02-2015 20:41

Re: [C++] Use CAN Talons in RobotDrive

Ok I got it working! It's kind of an unusual workaround, but it works fine for us. Basically, create 4 fake motor controllers on unused PWM ports, and pass those to RobotDrive. From there, set the values of the real motor controllers to the values of the fake ones. Here is part of the code we used:

Code:

CANTalon *leftFront = new CANTalon(1); //Real ones

CANTalon *leftBack = new CANTalon(5);

CANTalon *rightFront = new CANTalon(2);

CANTalon *rightBack = new CANTalon(6);



TalonSRX *PWMlf = new TalonSRX(0); //Fake ones

TalonSRX *PWMlb = new TalonSRX(1);

TalonSRX *PWMrf = new TalonSRX(2);        

TalonSRX *PWMrb = new TalonSRX(3);        



void TeleopPeriodic()

{

        drive->MecanumDrive_Cartesian(driveStick->GetX(), driveStick->GetY(), driveStick->GetZ(), gyro->GetAngle());

        leftFront->Set(PWMlf->Get());

        leftBack->Set(PWMlb->Get());

        rightFront->Set(PWMrf->Get());

        rightBack->Set(PWMrb->Get());

}

ozrien

05-02-2015 21:27

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

1452-Leo

05-02-2015 23:19

Re: [C++] Use CAN Talons in RobotDrive

I tried turning the safeties off already and it didn't work, but I'll try the disabled loop tomorrow and get back. I agree that dummy PWMs is a bad solution, but we're going all CAN this year so it shouldn't really affect anything, and we will probably revert to it if your solution doesn't work because we've spent too long trying to get this to work. Thanks for the help though!

ozrien

07-02-2015 21:14

Re: [C++] Use CAN Talons in RobotDrive

Quote:

Originally Posted by 1452-Leo (Post 1439072)

Turning off the safeties in the CANTalons is not sufficient. They are already off (they default off). The problem is RobotDrive has it's own safety enable.

Anyway if you are still seeing the EXACT symptom of "...Motor Safety Timeout..." errors in the DS then you need to remedy it. Even with the PWM workaround (eek) you're still generating errors in your DS log. That's going to prevent your drive team from seeing other reported errors (because they will get used to seeing errors in that area of the DS".

m_robotDrive->SetSafetyEnabled(false); will absolutely prevent MS errors. If you are still seeing them then something is very wrong, and it will likely be a problem throughout the season.

BradAMiller

13-02-2015 17:54

Re: [C++] Use CAN Talons in RobotDrive

Definitely turn off the motor safety on the RobotDrive object, not the individual motor controllers and see if that fixes the problem. In the library, RobotDrive is the only place where the motor safety stuff is enabled by default, and as Omar said, it is part of the RobotDrive class as a whole. The thought was that it would stopping the runaway program from driving on it's own was the highest priority.

Use something like:
drive.SetSafetyEnabled(false);

Please let us know if does it for you.

All times are GMT -5. The time now is 21:12.