C++ MotorSafetyHelper

[Team2246]

Hello,
We are having problems with the cRIO rebooting, we have narrowed it down to being a problem with the new MotorSafetyHelper. The cRIO works with the BulitinDefaultCode, but this years code has issues it seems. We tried our last years code and it had the same issues. Last year it worked fine, so it makes us think that the problem lies with MotorSafetyHelper. Thank you very much for any help you can provide. Below this years code, and information you can provide about MotorSafetyHelper would be great.

/*
 Jon Mardyla
 2012
 FIRST ROBOTICS 
 TEAM 2246 - ARMY OF SUM
 */
#include "WPILib.h"

DriverStationLCD *dsLCD = DriverStationLCD::GetInstance(); // create the instance of a driver station

const float AutoSpeed = .5; // robot speed (movement during auto mode)
const float Stop = 0;
const float CSspeed1 = .25; // speed for the auto cannon on 1st set of auto modes(1-3)
const float CSspeed2 = .5; // speed for the auto cannon on 2st set of auto modes(3-6)

float CannonSpeed = .5; // cannon speed for 

class ArmyOfSumRobot : public IterativeRobot
{
	DriverStation *Ds; //Driver Station 
	RobotDrive *MyRobot;
	Joystick *DriveStickLeft; // Controller 1 (Left Drive)
	Joystick *DriveStickRight; // Controller 2 (Right Drive)
	Joystick *Xbox; // Controller 3 (Shooter)

	Jaguar *DriveLeft;
	Jaguar *DriveRight;
	Victor *Cannon;
	Victor *CannonHeight;
	Victor *CannonDirection;

	Solenoid *CannonFeedOut;
	Solenoid *CannonFeedIn;
	Solenoid *BridgeArm;

	DigitalInput *HeightUp;
	DigitalInput *HeightDown;
	DigitalInput *LeftKaren;
	DigitalInput *RightKaren;
	DigitalInput *AutoSwitch1;
	DigitalInput *AutoSwitch2;
	DigitalInput *AutoSwitch3;

	Ultrasonic *WallDistance;

	Gyro *ShooterAngle;

	Compressor *compressor;

	Timer *timer;

public:
	ArmyOfSumRobot(void)
	{
		Ds = DriverStation::GetInstance();

		MyRobot = new RobotDrive(1,2);

		Xbox = new Joystick(2);
		Xbox->SetAxisChannel(Joystick::kTwistAxis, 3);
		Xbox->SetAxisChannel(Joystick::kXAxis, 1);
		Xbox->SetAxisChannel(Joystick::kYAxis, 2);

		Cannon = new Victor(3);
		CannonHeight = new Victor(4);
		CannonDirection = new Victor(5);

		CannonFeedOut = new Solenoid(1);
		CannonFeedIn = new Solenoid(2);
		BridgeArm = new Solenoid(3);

		HeightUp = new DigitalInput(1);
		HeightDown = new DigitalInput(2);
		LeftKaren = new DigitalInput(3);
		RightKaren = new DigitalInput(4);
		AutoSwitch1 = new DigitalInput(5);
		AutoSwitch2 = new DigitalInput(6);
		AutoSwitch3 = new DigitalInput(7);

		WallDistance = new Ultrasonic(8,9);

		ShooterAngle = new Gyro(1);

		compressor = new Compressor(10,1);

		timer = new Timer;
	}

	void AutonomousContinuous(void)
	{
		compressor->Start();
		Cannon->Set(CannonSpeed);

		//Varible Values
		dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "US: %4.1f",
				WallDistance->GetRangeInches()); // display the rangefinder
		dsLCD->Printf(DriverStationLCD::kUser_Line2, 1, "CannonSpeed: %4.1f",
				Cannon->Get()); // display the cannon speed
		dsLCD->Printf(DriverStationLCD::kUser_Line3, 1, "Gyro: %4.1f",
				ShooterAngle->GetAngle()); // display the cannon angle
		dsLCD->UpdateLCD();

		if (AutoSwitch1->Get() == 1 && AutoSwitch2->Get() == 1
				&& AutoSwitch3->Get() == 1)
		{
			// this auto mode will be the right side (no movement) and will turn cannon left and shoot twice
			// and rotate back to 0 degrees.
			/*mti*/Cannon->Set(CSspeed1); // cannon speed for auto modes(1-3)

			if (ShooterAngle->GetAngle() < 14.5) //detect the cannon angle

			{
				CannonDirection->Set(.25); // move the cannon
			}
			else if (ShooterAngle->GetAngle()>15.5)
			{
				CannonDirection->Set(-.25);
			}
			else
			{
				timer->Start();
				CannonDirection->Set(Stop);
				if (timer->Get() >= 1 && timer->Get() < 3) // shoot ball at 1 sec

				{
					CannonFeedOut->Set(true);
					CannonFeedIn->Set(false);
				}
				if (timer->Get() >= 3 && timer->Get() < 5) // prepare cannon for another ball
				{
					CannonFeedOut->Set(false);
					CannonFeedIn->Set(true);
				}
				if (timer->Get() >= 5 && timer->Get() < 7) // prepare cannon for another ball & teleop

				{
					CannonFeedOut->Set(true);
					CannonFeedIn->Set(false);
				}
				if (timer->Get() >= 7) // shoot 2nd ball at another 1 sec

				{
					CannonFeedOut->Set(true);
					CannonFeedIn->Set(false);
				}
			}
		}

	if(AutoSwitch1->Get() == 1 && AutoSwitch2->Get() == 1 && AutoSwitch3->Get() == 0)
	{
		//this auto mode will shoot twice (no movement)
	}
	if(AutoSwitch1->Get() == 1 && AutoSwitch2->Get() == 0 && AutoSwitch3->Get() == 0)
	{
		// this auto mode will be the left side (no movement) and will turn cannon right and shoot twice
		// and rotate back to 0 degrees.
	}
	if(AutoSwitch1->Get() == 0 && AutoSwitch2->Get() == 1 && AutoSwitch3->Get() == 1)
	{
		// this auto mode will be the right side (with movement) and will turn cannon left and shoot twice
		// and rotate back to 0 degrees.
	}
	if(AutoSwitch1->Get() == 0 && AutoSwitch2->Get() == 0 && AutoSwitch3->Get() == 1)
	{
		//this auto mode will shoot twice (with movement)
	}
	if(AutoSwitch1->Get() == 1 && AutoSwitch2->Get() == 0 && AutoSwitch3->Get() == 1)
	{
		// this auto mode will be the left side (with movement) and will turn cannon right and shoot twice
		// and rotate back to 0 degrees.
	}
	if(AutoSwitch1->Get() == 0 && AutoSwitch2->Get() == 1 && AutoSwitch3->Get() == 0)
	{
		// this auto mode will move the robot back X distance from the wall
	}
	if(AutoSwitch1->Get() == 0 && AutoSwitch2->Get() == 0 && AutoSwitch3->Get() == 0)
	{
		// this won't be an auto mode, it will sit still and do nothing
	}
}

void TeleopContinuous(void)
{
	//Varible Values
	dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "US: %4.1f", WallDistance->GetRangeInches()); // display the rangefinder
	dsLCD->Printf(DriverStationLCD::kUser_Line2, 1, "CannonSpeed: %4.1f", Cannon->Get()); // display the cannon speed
	dsLCD->Printf(DriverStationLCD::kUser_Line3, 1, "Gyro: %4.1f", ShooterAngle->GetAngle()); // display the cannon angle
	dsLCD->UpdateLCD();

	MyRobot->TankDrive(.25, .25); // create tank drive with the left & right stick
	Cannon->Set(CannonSpeed); //set the default cannon speed for teleop
	//--------------------------------------------------------------------------------------------------------------
	if(Xbox->GetRawButton(3)) // (button X) spinning lazy karen left

	{
		CannonDirection->Set(1.0);
		if(LeftKaren->Get() == 1)
		{
			CannonDirection->Set(Stop);
		}
	}
	else if(Xbox->GetRawButton(2)) // (button b) spinning lazy karen left

	{
		CannonDirection->Set(-1.0);
		if(RightKaren->Get() == 1)
		{
			CannonDirection->Set(Stop);
		}
	}
	else // do not rotate if buttons are not pushed

	{
		CannonDirection->Set(0);
	}
	//--------------------------------------------------------------------------------------------------------------	
	if(Xbox->GetRawButton(4)) // (button y) raise height

	{
		CannonHeight->Set(1.0);
		if(HeightUp->Get() == 1)
		{
			CannonHeight->Set(Stop);
		}
	}
	else if(Xbox->GetRawButton(1)) // (button a) lowering height

	{
		CannonHeight->Set(-1.0);
		if(HeightDown->Get() == 1)
		{
			CannonHeight->Set(Stop);
		}
	}
	else // stop cannon height if no button is pushed

	{
		CannonHeight->Set(Stop);
	}
	//--------------------------------------------------------------------------------------------------------------
	if(Xbox->GetRawButton(6)) // speed up the cannonspeed

	{
		CannonSpeed+=.01;
		if(CannonSpeed>=1)
			CannonSpeed=1;
	}
	else if(Xbox->GetRawButton(5)) // lower the cannonspeed

	{
		CannonSpeed-=.01;
		if(CannonSpeed<0.25)
			CannonSpeed=0.25;
	}
	else if (Xbox->GetRawButton(10))
	{
		CannonSpeed=.5;
	}
	Cannon->Set(CannonSpeed);
	//--------------------------------------------------------------------------------------------------------------
	if(Xbox->GetTwist()> .5) // shoot ball when bumber is pushed in

	{
		CannonFeedOut->Set(true);
		CannonFeedIn->Set(false);
	}
	else // when bumper is released reverse action

	{
		CannonFeedOut->Set(false);
		CannonFeedIn->Set(true);
	}
	//--------------------------------------------------------------------------------------------------------------
	if(Xbox->GetTwist() < -.5) // when the other bumber is pushed deploy the bridge arm

	{
		BridgeArm->Set(true);
	}
	else // when the bumper is released reverse action

	{
		BridgeArm->Set(false);
	}
}

void DisabledPeriodic(void)
{
	//		GetWatchdog().Feed(); // feed the user watchdog at every period when disabled
}

void TeleopPeriodic(void)
{
	//		GetWatchdog().Feed(); // feed the user watchdog at every period when in autonomous 
}

void DisabledContinuous(void)
{
	//		GetWatchdog().Feed();	// feed the user watchdog at every period when in autonomous 
}

void AutonomousPeriodic(void)
{
	//		GetWatchdog().Feed();	// feed the user watchdog at every period when in autonomous 
}
/********************************** Init Routines *************************************/
void DisabledInit(void)
{
	DriveLeft->SetSafetyEnabled(false);
	DriveRight->SetSafetyEnabled(false);
	Cannon->SetSafetyEnabled(false);
	CannonHeight->SetSafetyEnabled(false);
	CannonDirection->SetSafetyEnabled(false);
	MyRobot->SetSafetyEnabled(false);
	GetWatchdog().Feed(); // feed the user watchdog at every period when in autonomous 

	ShooterAngle->Reset();
}

void AutonomousInit(void)
{
	DriveLeft->SetSafetyEnabled(false);
	DriveRight->SetSafetyEnabled(false);
	Cannon->SetSafetyEnabled(false);
	CannonHeight->SetSafetyEnabled(false);
	CannonDirection->SetSafetyEnabled(false);
	MyRobot->SetSafetyEnabled(false);

}

void TeleopInit(void)
{
	DriveLeft->SetSafetyEnabled(false);
	DriveRight->SetSafetyEnabled(false);
	Cannon->SetSafetyEnabled(false);
	CannonHeight->SetSafetyEnabled(false);
	CannonDirection->SetSafetyEnabled(false);
	MyRobot->SetSafetyEnabled(false);

}

void RobotInit(void)
{

	// Create and set up a camera instance. First wait for the camera to start
	// if the robot was just powered on. This gives the camera time to boot.
	/*
	 AxisCamera &camera = AxisCamera::GetInstance();
	 camera.WriteResolution(AxisCamera::kResolution_320x240);
	 camera.WriteCompression(60);
	 camera.WriteBrightness(0);
	 */
	// set up gyro
	ShooterAngle->Reset();
	ShooterAngle->SetSensitivity(.01); //change later to a  diffrent sensitvity

	MyRobot->SetExpiration(20.0);
	MyRobot->SetSafetyEnabled(true);
}

};

START_ROBOT_CLASS(ArmyOfSumRobot)
;

[dcherba]

For every motor in the telop period and autonomous period you must make sure to use the set function for every motor. This insures that the motor is under control. If it does not receive a set frequently enough acts like a watchdog timer event.

[RufflesRidge]

Originally Posted by Team2246

Hello,
We are having problems with the cRIO rebooting, we have narrowed it down to being a problem with the new MotorSafetyHelper.

The MotorSafetyHelper is not new, it was added last year. What is new is it sending messages back to the Driver station in C++ and Java. Last year it only did so in LabVIEW.