| Team2246 |
14-02-2012 14:41 |
C++ MotorSafetyHelper
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.
Code:
/*
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)
;
|
