Robot Code Not Working

[Arrowhead]

So we recently got our new CRio in, and after getting it set up, we started testing our code. We have just got the code uploaded to the CRio, but when we run it, none of our Victors do anything, and our code doesnt seem to be making any changes to anything. We built our code without errors. Here is our code, and here is the messages we get in the driver station diagnostics menu

#include "WPILib.h"


class RobotDemo : public SimpleRobot
{



Victor laquanda; //front left motor:V1
Victor anabel; //back left motor:V2
Victor rotciv; //front right motor:V3
Victor shanaynay; //back right motor:V4

RobotDrive rDrive; //declare robot drive

Victor shooterVictor; //declare victor for shooter
Victor shooterVictor2;

Servo loaderArm; //declare servo for loader arm

Joystick driverStick; //declares all 3 joysticks
Joystick driverStick2;
Joystick shooterStick;

Victor climbingArm1; //declare victors for climbing arms
Victor climbingArm2;

Solenoid SC1; //declares solenoids for climbing arm
Solenoid SC2;

DigitalInput Disk1; //declares limit switches for hopper
DigitalInput Disk2;
DigitalInput Disk3;
DigitalInput Disk4;



public:


RobotDemo(void):

laquanda(1), //assign drive motors PWM ports
anabel(2),
rotciv(3),
shanaynay(4),

rDrive(laquanda, anabel, rotciv, shanaynay), //assigns robot drive talons theiw PWM ports

shooterVictor(6), //assigns victor its PWM port
shooterVictor2(7),

loaderArm(5), //assign servo its PWM port

driverStick(1), //assign joysticks their USB ports
driverStick2(2),
shooterStick(3),

climbingArm1(8), //assigns climbing victors PWM ports
climbingArm2(9),

SC1(1), //assigns climbing pneumatics digital output ports
SC2(2),

Disk1(3), //assigns limit switches digital input ports
Disk2(4),
Disk3(5),
Disk4(6)

{

Watchdog().SetEnabled(false);

rDrive.SetExpiration(0.1);
rDrive.SetSafetyEnabled(true); //sets motor saftey on for all motors

shooterVictor.SetExpiration(0.1);
shooterVictor.SetSafetyEnabled(true);

shooterVictor2.SetExpiration(0.1);
shooterVictor2.SetSafetyEnabled(true);

loaderArm.SetExpiration(0.1);
loaderArm.SetSafetyEnabled(true);

climbingArm1.SetExpiration(0.1);
climbingArm1.SetSafetyEnabled(false);

climbingArm2.SetExpiration(0.1);
climbingArm2.SetSafetyEnabled(false);

}


void Autonomous(void)
{

DriverStationLCD *ds = DriverStationLCD::GetInstance();

double AutoTime = 0; //used to measure time during autonomous


rDrive.SetSafetyEnabled(false); //turn of motor saftey timers during autonomous for convience

shooterVictor.SetSafetyEnabled(false);
shooterVictor2.SetSafetyEnabled(false);

loaderArm.SetSafetyEnabled(false);

ds->PrintfLine(DriverStationLCD::kUser_Line1, "Program Started Successfully!");
ds->PrintfLine(DriverStationLCD::kUser_Line2, "Robot Is in Autonomous");

ds->UpdateLCD();

while(IsAutonomous()){


/**
//start at center autonomous
if(AutoTime>=0.0 && AutoTime<0.5){
shooterVictor.Set(1.0);
shooterVictor2.Set(1.0);
rDrive.MecanumDrive_Cartesian( 0.0 , 0.0 , -0.5 );
}

if(AutoTime>=0.5 && AutoTime<1.0){
loaderArm.SetAngle(180);
}

if(AutoTime>=1.0 && AutoTime<1.5){
loaderArm.SetAngle(0);
}

if(AutoTime>=1.5 && AutoTime<2.0){
loaderArm.SetAngle(180);
}

if(AutoTime>=2.0 && AutoTime<2.5){
loaderArm.SetAngle(0);
}

if(AutoTime>=2.5 && AutoTime<3.0){
loaderArm.SetAngle(180);
}

if(AutoTime>=3.0 && AutoTime<3.5){
loaderArm.SetAngle(0);
shooterVictor.Set(0.0);
shooterVictor2.Set(0.0);
}

if(AutoTime>=3.5 && AutoTime<4.0){
rDrive.MecanumDrive_Cartesian( 0.0, 0.0 , 0.5 );
}

if(AutoTime>=4.0 && AutoTime<7.0){
rDrive.MecanumDrive_Cartesian( 1.0 , 0.0 , 0.0 );
}

if(AutoTime>=7.0 && AutoTime<9.0){
rDrive.MecanumDrive_Cartesian( 0.0 , 1.0 , 0.0 );
}

if(AutoTime>=9.0 && AutoTime<10.0){
rDrive.MecanumDrive_Cartesian( 0.0 , 0.0 , 0.5 );
}

if(AutoTime>=10.0 && AutoTime<10.5){
rDrive.MecanumDrive_Cartesian( 0.7 , 0.0 , 0.0 );
}
*/


//start at left or right autonomous
if(AutoTime>=0.0 && AutoTime<0.5){
shooterVictor.Set(1.0);
shooterVictor2.Set(1.0);
loaderArm.SetAngle(180);
}

if(AutoTime>=0.5 && AutoTime<1.0){
loaderArm.SetAngle(0);
}

if(AutoTime>=1.0 && AutoTime<1.5){
loaderArm.SetAngle(180);
}

if(AutoTime>=1.5 && AutoTime<2.0){
loaderArm.SetAngle(0);
}

if(AutoTime>=2.0 && AutoTime<2.5){
loaderArm.SetAngle(180);
}

if(AutoTime>=2.5 && AutoTime<3.0){
loaderArm.SetAngle(0);
shooterVictor.Set(0.0);
shooterVictor2.Set(0.0);
}

if(AutoTime>=3.0 && AutoTime<6.0){
rDrive.MecanumDrive_Cartesian(1.0, 0.0 , 0.0 );
}

if(AutoTime>=6.0 && AutoTime<7.0){
rDrive.MecanumDrive_Cartesian( 0.0 , 0.0 , -0.5 ); //left autonomous = -0.5 turn right autonomous = 0.5 turn
}

if(AutoTime>=7.0 && AutoTime<9.0){
rDrive.MecanumDrive_Cartesian( 0.7 , 0.0 , 0.0 );
}

AutoTime=AutoTime+0.02;
Wait(0.02);
}
}


void operatorControl(){

rDrive.SetSafetyEnabled(true); //turn motor saftey timers back on

shooterVictor.SetSafetyEnabled(true);
shooterVictor2.SetSafetyEnabled(true);

loaderArm.SetSafetyEnabled(true);

DriverStationLCD *ds = DriverStationLCD::GetInstance();

ds->PrintfLine(DriverStationLCD::kUser_Line6, "Pinkie Pie Is Best Pony");

while (IsOperatorControl()){

ds->PrintfLine(DriverStationLCD::kUser_Line2, "Robot Is In TeleOp");

rDrive.MecanumDrive_Cartesian(driverStick.GetX(), driverStick.GetY(), driverStick2.GetX()); //gets input for robot drive
//from joysticks 1/2
ShooterControl(); //runs shooter control function

LoaderControl(); //runs servo control function

//Climbing(); //runs pryamid climbing function

//DiskCount(); //runs diskcount function

ds->UpdateLCD();

Wait(0.02); //waits 0.02 seconds
}
}


void ShooterControl(){ //shooter control function
DriverStationLCD *ds = DriverStationLCD::GetInstance();
if(shooterStick.GetRawButton(1)){
shooterVictor.Set(1.0); //if trigger is pressed, turn on shooter
shooterVictor2.Set(1.0);
ds->PrintfLine(DriverStationLCD::kUser_Line3, "Shooter Is On");
}
else{
shooterVictor.Set(0.0); //if trigger isnt pressed, turn off shooter
shooterVictor2.Set(0.0);
ds->PrintfLine(DriverStationLCD::kUser_Line3, "Shooter Is Off");
}
}


void LoaderControl(){ //servo control function
DriverStationLCD *ds = DriverStationLCD::GetInstance();
if(shooterStick.GetRawButton(3)){
loaderArm.SetAngle(180); //if middle button is pressed, extend servo
ds->PrintfLine(DriverStationLCD::kUser_Line4, "Loader Servo Is Extended");
}
else{
loaderArm.SetAngle(0); //if middle button isnt pressed, retract servo
ds->PrintfLine(DriverStationLCD::kUser_Line4, "Loader Servo Is Retracted");
}
}


void Climbing(){ //pyramid climbing function, happens automatically
DriverStationLCD *ds = DriverStationLCD::GetInstance();
if(shooterStick.GetRawButton(6) && driverStick.GetRawButton(6)){ //if button 6 pressed on both sticks, starts climb

ds->PrintfLine(DriverStationLCD::kUser_Line2, "Robot Is Climbing");

ds->UpdateLCD();

rDrive.SetSafetyEnabled(false); //turn of motor saftey timers during climbing for convience

shooterVictor.SetSafetyEnabled(false);
shooterVictor2.SetSafetyEnabled(false);

loaderArm.SetSafetyEnabled(false);

bool climbingRunning=true; //checks if robot continues to climb
double CTime = 0;
double VCTime=0;

while(climbingRunning=true){

if(CTime>=0.0 && CTime<1.0){
climbingArm1.Set(0.4); //raise climbing arms
climbingArm2.Set(0.4);
VCTime=VCTime+0.05;
}

if(CTime>=1.0 && CTime<1.5){
climbingArm1.Set(0.0); //extend climbing arms
climbingArm2.Set(0.0);
SC1.Set(true);
SC2.Set(true);
}


if(CTime>=1.5 && CTime<2.0){
rDrive.MecanumDrive_Cartesian( 0.2 , 0.0 , 0.0 ); //move towards bar
}

if(CTime>=2.0 && CTime<2.5){
SC1.Set(false); //retract climbing arms
SC2.Set(false);
}


if(shooterStick.GetRawButton(7)){
climbingRunning=false; //if button 7 pressed, stops climbing
climbingArm1.Set(-0.4);
climbingArm2.Set(-0.4);
Wait(VCTime);
SC1.Set(false);
SC2.Set(false);

rDrive.SetSafetyEnabled(true); //turn motor saftey timers back on

shooterVictor.SetSafetyEnabled(true);
shooterVictor2.SetSafetyEnabled(true);

loaderArm.SetSafetyEnabled(true);
}


Wait (0.05);

CTime=CTime+0.05;
}
}
}


void DiskCount(){ //function that counts disk in the hopper and displays it
DriverStationLCD *ds = DriverStationLCD::GetInstance();
int disknum = 0; //variable that counts disks in hopper

if(Disk1.Get()){
disknum=disknum+1; //disk has activated a limit switch, add 1 to # of disks
}
if(Disk2.Get()){
disknum=disknum+1;
}
if(Disk3.Get()){
disknum=disknum+1;
}
if(Disk4.Get()){
disknum=disknum+1;
}

ds->PrintfLine(DriverStationLCD::kUser_Line5, "Disk's in Hopper: %d", disknum); //print out the number of disks in hopper


}

};

START_ROBOT_CLASS(RobotDemo); //starts the class running

Warning <Code> 44002 occurred at Ping Results: link-GOOD, DS radio(.4)-bad, robot radio(.1)-bad, cRIO(.2)-GOOD, FMS-bad Driver Station
<time>2/6/2013 5:28:08 AM<unique#>104
FRC: Driver Station ping status has changed.
ERROR: A timeout has been exceeded: PWM 5 on module 1... Output not updated often enough. ...in Check() in C:/WindRiver/workspace/WPILib/MotorSafetyHelper.cpp at line 117


ERROR: A timeout has been exceeded: PWM 7 on module 1... Output not updated often enough. ...in Check() in C:/WindRiver/workspace/WPILib/MotorSafetyHelper.cpp at line 117


ERROR: A timeout has been exceeded: PWM 6 on module 1... Output not updated often enough. ...in Check() in C:/WindRiver/workspace/WPILib/MotorSafetyHelper.cpp at line 117


ERROR: A timeout has been exceeded: RobotDrive... Output not updated often enough. ...in Check() in C:/WindRiver/workspace/WPILib/MotorSafetyHelper.cpp at line 117
ERROR: A timeout has been exceeded: PWM 5 on module 1... Output not updated often enough. ...in Check() in C:/WindRiver/workspace/WPILib/MotorSafetyHelper.cpp at line 117
[/CODE]

[jwakeman]

The code looks pretty good. I like your Victor names. Did you write all of it without testing any of it? Or has this just become a problem after getting your new cRio? It's hard to tell from all that code where exactly the problem might be. You might want to try using NetConsole and add some print statements to your code to see where it is getting locked up...the "output not updated" errors suggest your code is locked up somewhere. If you are using the WPILib PIDController class there are some defects that cause task deadlocks.

[Hjax]

You forgot to disable motor safety.
Motor safety kills the robot if you don't pass an update to the motor often enough (if i remember correctly)

Just put this line:
myRobot.SetSafetyEnabled(false);
before this line:
while (isOperatorControl()){

This will disable the motor safety (not a bad thing in this case to keep your motors spinning and your code working)

[Arrowhead]

The code looks pretty good. I like your Victor names. Did you write all of it without testing any of it? Or has this just become a problem after getting your new cRio? It's hard to tell from all that code where exactly the problem might be. You might want to try using NetConsole and add some print statements to your code to see where it is getting locked up...the "output not updated" errors suggest your code is locked up somewhere. If you are using the WPILib PIDController class there are some defects that cause task deadlocks.

This is the first time we are testing the code, as we havent had a working CRio before now.
What is this net console you talk about?
Also, what is the PIDController class?
It looks like i have some reading to do

[Arrowhead]

You forgot to disable motor safety.
Motor safety kills the robot if you don't pass an update to the motor often enough (if i remember correctly)

Just put this line:
myRobot.SetSafetyEnabled(false);
before this line:
while (isOperatorControl()){

This will disable the motor safety (not a bad thing in this case to keep your motors spinning and your code working)

we have tried that, the code still does nothing, and the only difference is that we don't get timeout errors
Also, we shouldn't be getting timeout errors, as they should be getting updated often enough.

[Alan Anderson]

All those diagnostic messages to the DS "LCD" window might be clogging the network communication and causing enough lag to trip the motor safeties. Take them out temporarily just to see if that's the cause of your problem.

[jwakeman]

This is the first time we are testing the code, as we havent had a working CRio before now.
What is this net console you talk about?
Also, what is the PIDController class?
It looks like i have some reading to do

If you haven't tested any of the code before I would recommend starting small and then adding in one piece at a time verifying that things work as expected as you go. Just comment out everything in your while(IsAutonomous()) loop and while(IsOperatorControl()) loop and then uncomment one line at a time testing each time. Another way of saying this is you should do 'unit testing' to verify each function is working as intended, then do 'integration testing' to verify all the functions/units work together correctly.

You may not need to use NetConsole since you are already sending print statements to the LCD but is here is a link to the documentation http://wpilib.screenstepslive.com/s/3120/m/8851/l/79731-using-netconsole.

PIDController is one of the classes available in the WPILib that is used for doing closed-loop PID control. Here's the link for that http://wpilib.screenstepslive.com/s/3120/m/7912/l/79828-operating-the-robot-with-feedback-from-sensors-pid-control

[Arrowhead]

So we tried running the simple robot template code on the robot, and it didnt run, so right now we are looking at the ribbon cable running between the CRio and the digital sidecar as the culprit.
We should be bale to get another one within the next 2 days or so.

[Arrowhead]

So we switched out the CRio module for an older one and it now runs the simple robot code perfectly.
We then tried running our code. We disabled motor safety timers and still get errors about motors not being updated enough. When we start running the code in tele op after deploying it nothing happens. However, when we run autonomous the motors move, but dont stop, and dont seem to be doing anything specified in our code. If we then run the robot in tele op after running it in autonomous, the same motors stay on and dont reply to any user input.

[kingroosevelt]

Arrowhead,

Team 4243 has been having the exact same problem. Out of the box Simple Robot and Default Robot templates work out of the box on an 8-slot cRIO but fails with timeout errors on a 4-slot cRIO.

This is our second year and the same problem annihilated our rookie performance last year. At that time we managed to disable the timeouts but our code effectively executed at 3 Hz instead of 50 Hz resulting in pretty embarrassing performance.

Fortunately we had an 8-slot cRIO Team 342 had lent us to get some programming knowledge before last year's build season started. And, thanks to this thread, I changed out our 4-slot cRIO for the 8-slot cRIO, and the Simple Robot Template compiled and executed flawlessly!!! I repeated the experience with the Default Robot template. What a relief!!

Given all the related threads on MotorSafetyHelper timeout messages, I think it is clear WPILib is not fully compatible with the 4-slot cRIO. Not last year and not this year.

[Arrowhead]

Oh, we fixed the problem we were having, the problem was the "O" in OperatorControl wasnt capitalized :/