RobotSystem(void):
robotInted(false)
,stick(1) // as they are declared above.
,stick2(2)
,line1(10)
,line2(11)
,line3(12)
//,camera(AxisCamera::GetInstance())
,updateCAN("CANUpdate",(FUNCPTR)UpdateCAN)
,cameraTask("CAMERA", (FUNCPTR)CameraTask)
,compressor(14,1)
,EncArm(2,3)
,EncClaw(5,6)
,PIDArm(.04,0,0) // .002, .033
,PIDClaw(.014,.0000014,0)
,LowArm(.1)
/*
,MiniBot1(4)
,MiniBot2(2)
,ClawGrip(3)
*/
,MiniBot1a(8,1)
,MiniBot1b(8,2)
,MiniBot2a(8,3)
,MiniBot2b(8,4)
,ClawOpen(8, 8)
,ClawClose(8,7)
,LimitClaw(7)
,LimitArm(13)
{
// myRobot.SetExpiration(0.1);
GetWatchdog().SetEnabled(false);
GetWatchdog().SetExpiration(1);
compressor.Start();
debug("Waiting to init CAN");
Wait(2);
Dlf = new CANJaguar(6,CANJaguar::kSpeed);
Dlb = new CANJaguar(3,CANJaguar::kSpeed);
Drf = new CANJaguar(7,CANJaguar::kSpeed);
Drb = new CANJaguar(2,CANJaguar::kSpeed);
arm1 = new CANJaguar(5);
arm1_sec = new CANJaguar(8);
arm2 = new CANJaguar(4);
EncArm.SetDistancePerPulse(.00025);
EncClaw.SetDistancePerPulse(.00025);
EncClaw.SetReverseDirection(false);
EncArm.SetReverseDirection(true);
EncArm.Reset();
EncClaw.Reset();
updateCAN.Start((int)this);
//cameraTask.Start((int)this);
EncArm.Start();
EncClaw.Start();
debug("done initing");
}
Robot() :
timer(),
robotDrive(Constants::driveFrontLeftPin, Constants::driveRearLeftPin, Constants::driveFrontRightPin, Constants::driveRearRightPin),//tells the robot where everything is plugged in
i2c(I2C::kOnboard,Constants::ledAddress),
driveStick(Constants::driveStickChannel),
grabStick(Constants::grabStickChannel),
prox(Constants::driveUltrasonicPin),
grabTalon(Constants::grabTalonPin),
grabInnerLimit(Constants::grabInnerLimitPin),
grabOuterLimit(Constants::grabOuterLimitPin),
liftTalon(Constants::liftTalonPin),
liftEncoder(Constants::liftEncoderAPin, Constants::liftEncoderBPin),
liftUpperLimit(Constants::liftUpperLimitPin),
liftLowerLimit(Constants::liftLowerLimitPin),
grabEncoder(Constants::grabEncoderAPin, Constants::grabEncoderBPin),
pdp(),
gyro(Constants::driveGyroRate),
pickup(grabTalon, grabInnerLimit, grabOuterLimit, liftTalon, liftEncoder, liftUpperLimit, liftLowerLimit, pdp)
{
robotDrive.SetExpiration(0.1); // safety feature
robotDrive.SetInvertedMotor(RobotDrive::kFrontRightMotor, true); // make the motors go the right way
robotDrive.SetInvertedMotor(RobotDrive::kRearRightMotor, true);
robotDrive.SetInvertedMotor(RobotDrive::kRearLeftMotor, true);
liftEncoder.SetReverseDirection(Constants::liftEncoderReverse);
}
Robot()
{
#if BUILD_VERSION == COMPETITION
liftMotor = new CANTalon(CHAN_LIFT_MOTOR);
liftMotor2 = new CANTalon(CHAN_LIFT_MOTOR2);
#else
liftMotor = new CANJaguar(CHAN_LIFT_MOTOR);
liftMotor2 = new CANJaguar(CHAN_LIFT_MOTOR2);
#endif
liftEncoder = new Encoder(CHAN_LIFT_ENCODER_A, CHAN_LIFT_ENCODER_B, false, Encoder::EncodingType::k4X);
liftEncoder->SetDistancePerPulse(LIFT_ENCODER_DIST_PER_PULSE);
liftEncoder->SetPIDSourceParameter(liftEncoder->kDistance);
#if BUILD_VERSION == COMPETITION
liftEncoder->SetReverseDirection(true);
#endif
controlLift = new PIDController(PID_P, PID_I, PID_D, liftEncoder, liftMotor);
controlLift->SetContinuous(true); //treat input to controller as continuous; true by default
controlLift->SetOutputRange(PID_OUT_MIN, PID_OUT_MAX);
controlLift->Disable(); //do not enable until in holding position mode
controlLift2 = new PIDController(PID_P, PID_I, PID_D, liftEncoder, liftMotor2);
controlLift2->SetContinuous(true); //treat input to controller as continuous; true by default
controlLift2->SetOutputRange(PID_OUT_MIN, PID_OUT_MAX);
controlLift2->Disable(); //do not enable until in holding position mode
liftLimitSwitchMin = new DigitalInput(CHAN_LIFT_LOW_LS);
liftLimitSwitchMax = new DigitalInput(CHAN_LIFT_HIGH_LS);
joystick = new Joystick(CHAN_JS);
}
PnumaticArmTest (void):
stick(1)
,up(7,7)
,down(7,8)
,enc(1,2)
,pid(.045,.00000,.16)
,low(.05)
{
enc.SetReverseDirection(true);
enc.Start();
}
RobotDemo(void):
leftDriveMotor(LEFT_DRIVE_PWM),
rightDriveMotor(RIGHT_DRIVE_PWM),
myRobot(&leftDriveMotor, &rightDriveMotor), // these must be initialized in the same order
stick(1), // as they are declared above.
stick2(2),
gamepad(3),
collectorMotor(PICKUP_PWM),
indexerMotor(INDEX_PWM),
shooterMotor(SHOOTER_PWM),
armMotor (ARM_PWM),
leftDriveEncoder(LEFT_DRIVE_ENC_A, LEFT_DRIVE_ENC_B),
shifter(SHIFTER_A,SHIFTER_B),
greenClaw(CLAW_1_LOCKED, CLAW_1_UNLOCKED),
yellowClaw(CLAW_2_LOCKED, CLAW_2_UNLOCKED),
potentiometer(ARM_ROTATION_POT),
indexSwitch(INDEXER_SW),
greenClawLockSwitch(CLAW_1_LOCK_SENSOR),
yellowClawLockSwitch(CLAW_2_LOCK_SENSOR),
compressor(COMPRESSOR_PRESSURE_SW, COMPRESSOR_SPIKE),
jogTimer(),
shooterTimer()
{
m_collectorMotorRunning = false;
m_shooterMotorRunning = false;
m_jogTimerRunning = false;
m_shiftCount = MAX_SHIFTS;
dsLCD = DriverStationLCD::GetInstance();
dsLCD->Clear();
dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, "2013 " NAME);
dsLCD->PrintfLine(DriverStationLCD::kUser_Line2, __DATE__ " "__TIME__);
dsLCD->UpdateLCD();
myRobot.SetExpiration(0.1);
shifter.Set(DoubleSolenoid::kReverse);
leftDriveEncoder.SetDistancePerPulse(DRIVE_ENCODER_DISTANCE_PER_PULSE);
leftDriveEncoder.SetMaxPeriod(1.0);
leftDriveEncoder.SetReverseDirection(true); // change to true if necessary
leftDriveEncoder.Start();
}
FRC2994_2014():
leftFrontDrive(LEFT_FRONT_DRIVE_PWM),
leftRearDrive(LEFT_REAR_DRIVE_PWM),
rightFrontDrive(RIGHT_FRONT_DRIVE_PWM),
rightRearDrive(RIGHT_REAR_DRIVE_PWM),
leftCenterDrive(CENTER_LEFT_DRIVE_PWM),
rightCenterDrive(CENTER_RIGHT_DRIVE_PWM),
intake(INTAKE_MOTOR_PWM),
rightWinch(RIGHT_WINCH_MOTOR_PWM),
leftWinch(LEFT_WINCH_MOTOR_PWM),
robotDrive(leftFrontDrive, leftRearDrive, leftCenterDrive, rightCenterDrive, rightFrontDrive, rightRearDrive),
rightStick(RIGHT_DRIVE_STICK),
leftStick(LEFT_DRIVE_STICK),
gamepad(GAMEPAD_PORT),
shifters(SHIFTER_A, SHIFTER_B),
arm(ARM_A, ARM_B),
eject(EJECT_A, EJECT_B),
winchSwitch(WINCH_SWITCH),
leftDriveEncoder(LEFT_ENCODER_A, LEFT_ENCODER_B),
rightDriveEncoder(RIGHT_ENCODER_A, RIGHT_ENCODER_B),
compressor(COMPRESSOR_PRESSURE_SW, COMPRESSOR_SPIKE),
// Robot starts off in a loaded state so a ball can be fit in
loaded(true),
loading(false),
armDown(false)
{
// Print an I'M ALIVE message before anything else. NOTHING ABOVE THIS LINE.
dsLCD = DriverStationLCD::GetInstance();
dsLCD->Clear();
dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, ROBOT_NAME);
dsLCD->PrintfLine(DriverStationLCD::kUser_Line3, __DATE__ " " __TIME__);
dsLCD->UpdateLCD();
ds = DriverStation::GetInstance();
// Set the experation to 1.5 times the loop speed.
robotDrive.SetExpiration(LOOP_PERIOD*1.5);
leftDriveEncoder.SetReverseDirection(true);
}
void TeleopInit()
{
// Initialize the encoder
sampleEncoder = new Encoder(0, 1, false, Encoder::EncodingType::k4X);
sampleEncoder->SetMaxPeriod(.1);
sampleEncoder->SetMinRate(10);
sampleEncoder->SetDistancePerPulse(5);
sampleEncoder->SetReverseDirection(true);
sampleEncoder->SetSamplesToAverage(7);
// Initialize the joystick
joystick = new Joystick(0);
// Initialize the motor
motor = new Victor(9);
// Initialize the gear tooth counter
toothTrigger = new AnalogTrigger(3);
toothTrigger->SetLimitsRaw(250, 3600);
gearToothCounter = new Counter(toothTrigger);
// gearToothCounter->SetUpDownCounterMode();
}
/**
* Set the direction sensing for this encoder.
* This sets the direction sensing on the encoder so that it couldl count in the correct
* software direction regardless of the mounting.
*
* @param aSlot The digital module slot for the A Channel on the encoder
* @param aChannel The channel on the digital module for the A Channel of the encoder
* @param bSlot The digital module slot for the B Channel on the encoder
* @param bChannel The channel on the digital module for the B Channel of the encoder
* @param reverseDirection true if the encoder direction should be reversed
*/
void SetEncoderReverseDirection(UINT32 aSlot, UINT32 aChannel, UINT32 bSlot, UINT32 bChannel, bool reverseDirection)
{
Encoder *encoder = AllocateEncoder(aSlot, aChannel, bSlot, bChannel);
if (encoder != NULL)
encoder->SetReverseDirection(reverseDirection);
}