/**
* Drive left & right motors for 2 seconds, enabled by a jumper (jumper
* must be in for autonomous to operate).
*/
void Autonomous(void) {
myRobot->SetSafetyEnabled(false);
if (ds->GetDigitalIn(ENABLE_AUTONOMOUS) == 1) // only run the autonomous program if jumper is in place
{
myRobot->Drive(0.5, 0.0); // drive forwards half speed
Wait(2.0); // for 2 seconds
myRobot->Drive(0.0, 0.0); // stop robot\
}
myRobot->SetSafetyEnabled(true);
}
void OperatorControl(void)
{
bool buttonABool;
bool buttonBBool;
bool buttonXBool;
bool buttonYBool;
bool buttonLBBool;
bool buttonRBBool;
bool buttonBackBool;
bool buttonStartBool;
float leftTrigger;
float rightTrigger;
float leftYAxis;
float rightYAxis;
float leftXAxis;
float rightXAxis;
myRobot.SetSafetyEnabled(true);
while (IsOperatorControl())
{
leftYAxis = newStick.GetLeftYAxis();
rightYAxis = newStick.GetRightYAxis();
SmartDashboard::PutNumber("Left Y Axis", leftYAxis);
SmartDashboard::PutNumber("Right Y Axis", rightYAxis);
leftXAxis = newStick.GetLeftXAxis();
rightXAxis = newStick.GetRightXAxis();
SmartDashboard::PutNumber("Left X Axis", leftXAxis);
SmartDashboard::PutNumber("Right X Axis", rightXAxis);
leftTrigger = newStick.GetLeftThrottle();
SmartDashboard::PutNumber("left trigger", leftTrigger);
rightTrigger = newStick.GetRightThrottle();
SmartDashboard::PutNumber("right trigger", rightTrigger);
buttonABool = newStick.GetButtonA();
SmartDashboard::PutNumber("buttonA",buttonABool);
buttonBBool = newStick.GetButtonB();
SmartDashboard::PutNumber("buttonB",buttonBBool);
buttonXBool = newStick.GetButtonX();
SmartDashboard::PutNumber("buttonX",buttonXBool);
buttonYBool = newStick.GetButtonY();
SmartDashboard::PutNumber("buttonY",buttonYBool);
buttonLBBool = newStick.GetButtonLB();
SmartDashboard::PutNumber("buttonLB",buttonLBBool);
buttonRBBool = newStick.GetButtonRB();
SmartDashboard::PutNumber("buttonRB",buttonRBBool);
buttonBackBool = newStick.GetButtonBack();
SmartDashboard::PutNumber("buttonBack",buttonBackBool);
buttonStartBool = newStick.GetButtonStart();
SmartDashboard::PutNumber("buttonStart",buttonStartBool);
Wait(0.005);
}
}
void OperatorControl(void)
{
NetTest();
return;
myRobot.SetSafetyEnabled(true);
digEncoder.Start();
const double ppsTOrpm = 60.0/250.0; //Convert from Pos per Second to Rotations per Minute by multiplication
// (See the second number on the back of the encoder to replace 250 for different encoders)
const float VoltsToIn = 41.0; // Convert from volts to cm by multiplication (volts from ultrasonic).
// This value worked for distances between 1' and 10'.
while (IsOperatorControl())
{
if (stick.GetRawButton(4)) {
myRobot.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(), -1);
}
else if (stick.GetRawButton(5))
{
myRobot.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(), 1);
}
else
{
myRobot.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(), 0);
}
myRobot.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(), 0);
SmartDashboard::PutNumber("Digital Encoder RPM", abs(digEncoder.GetRate()*ppsTOrpm));
SmartDashboard::PutNumber("Ultrasonic Distance inch", (double) ultra.GetAverageVoltage()*VoltsPerInch);
SmartDashboard::PutNumber("Ultrasonic Voltage", (double) ultra.GetAverageVoltage());
Wait(0.1);
}
digEncoder.Stop();
}
/**
* Drive left & right motors for 2 seconds then stop
*/
void Autonomous(void)
{
myRobot.SetSafetyEnabled(false);
myRobot.Drive(0.5, 0.0); // drive forwards half speed
Wait(2.0); // for 2 seconds
myRobot.Drive(0.0, 0.0); // stop robot
}
/**
* Runs the motors with Mecanum drive.
*/
void OperatorControl()
{
robotDrive.SetSafetyEnabled(false);
while (IsOperatorControl() && IsEnabled())
{
bool collisionDetected = false;
double curr_world_linear_accel_x = ahrs->GetWorldLinearAccelX();
double currentJerkX = curr_world_linear_accel_x - last_world_linear_accel_x;
last_world_linear_accel_x = curr_world_linear_accel_x;
double curr_world_linear_accel_y = ahrs->GetWorldLinearAccelY();
double currentJerkY = curr_world_linear_accel_y - last_world_linear_accel_y;
last_world_linear_accel_y = curr_world_linear_accel_y;
if ( ( fabs(currentJerkX) > COLLISION_THRESHOLD_DELTA_G ) ||
( fabs(currentJerkY) > COLLISION_THRESHOLD_DELTA_G) ) {
collisionDetected = true;
}
SmartDashboard::PutBoolean( "CollisionDetected", collisionDetected);
try {
/* Use the joystick X axis for lateral movement, */
/* Y axis for forward movement, and Z axis for rotation. */
/* Use navX MXP yaw angle to define Field-centric transform */
robotDrive.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(),
stick.GetZ(),ahrs->GetAngle());
} catch (std::exception ex ) {
std::string err_string = "Error communicating with Drive System: ";
err_string += ex.what();
DriverStation::ReportError(err_string.c_str());
}
Wait(0.005); // wait 5ms to avoid hogging CPU cycles
}
}
// Real Autonomous
// * Code to be run autonomously for the first ten (10) seconds of the match.
// * Launch catapult
// * Drive robot forward ENCODER_DIST ticks.
void Autonomous()
{
robotDrive.SetSafetyEnabled(false);
// STEP 1: Set all of the states.
// SAFETY AND SANITY - SET ALL TO ZERO
loaded = winchSwitch.Get();
loading = false;
intake.Set(0.0);
winch.Set(0.0);
// STEP 2: Move forward to optimum shooting position
Drive(-AUTO_DRIVE_SPEED, SHOT_POSN_DIST);
// STEP 3: Drop the arm for a clean shot
arm.Set(DoubleSolenoid::kForward);
Wait(1.0); // Ken
// STEP 4: Launch the catapult
LaunchCatapult();
Wait (1.0); // Ken
// Get us fully into the zone for 5 points
Drive(-AUTO_DRIVE_SPEED, INTO_ZONE_DIST - SHOT_POSN_DIST);
// SAFETY AND SANITY - SET ALL TO ZERO
intake.Set(0.0);
winch.Set(0.0);
}
//Choose which auto to use
void AutonomousInit() {
chainLift.SetSpeed(0);
canGrabber.SetSpeed(0);
robotDrive.MecanumDrive_Cartesian(0, 0, 0);
SmartDashboard::PutString("STATUS:", "STARTING AUTO");
robotDrive.SetSafetyEnabled(false);
chainLift.SetSafetyEnabled(false);
SmartDashboard::PutBoolean("Auto switch A: ", autoSwitch1.Get());
SmartDashboard::PutBoolean("Auto switch B: ", autoSwitch2.Get());
//Select auto type
if (autoSwitch1.Get()) {
if (autoSwitch2.Get())
AutonomousType4();
else
//1 on 2 grab n back
AutonomousType8();
} else {
if (autoSwitch2.Get())
//1 off, 2 on: grab n turn
AutonomousType10();
else {
SmartAutoPicker();
}
//Do Nothing
}
}
/*this fuction will need to be updated for the final robot*/
void FunctionBot::configDrive() {
drive->SetInvertedMotor(RobotDrive::kFrontLeftMotor, true);
drive->SetInvertedMotor(RobotDrive::kRearLeftMotor, true);
//drive->SetInvertedMotor(RobotDrive::kFrontLeftMotor,true);
drive->SetSafetyEnabled(false);
//not sure on this value at all
drive->SetMaxOutput(333);
}
/**
* Runs the motors with arcade steering.
*/
void OperatorControl(void)
{
myRobot.SetSafetyEnabled(true);
while (IsOperatorControl())
{
myRobot.ArcadeDrive(stick); // drive with arcade style (use right stick)
Wait(0.005); // wait for a motor update time
}
}
Wheels()
{
drive = new RobotDrive(0,2,4,3); //frontLeft, rearLeft, frontRight, rearRight
drive->SetInvertedMotor(RobotDrive::kFrontLeftMotor, true); // 0 is front left wheel
drive->SetInvertedMotor(RobotDrive::kRearLeftMotor, true); // 2 is back left wheel
drive->SetExpiration(0.1);
drive->SetSafetyEnabled(true);
driveSafety = new MotorSafetyHelper(drive);
}
void TeleopInit() override {
drive->SetExpiration(200000);
drive->SetSafetyEnabled(false);
liftdown->Set(false);
intake_hold = false;
lastLiftPos = 0;
manual = true;
}
/**
* Drive based upon joystick inputs, and automatically control
* motors if the robot begins tipping.
*/
void OperatorControl()
{
robotDrive.SetSafetyEnabled(false);
while (IsOperatorControl() && IsEnabled()) {
double xAxisRate = stick.GetX();
double yAxisRate = stick.GetY();
double pitchAngleDegrees = ahrs->GetPitch();
double rollAngleDegrees = ahrs->GetRoll();
if ( !autoBalanceXMode &&
(fabs(pitchAngleDegrees) >=
fabs(kOffBalanceThresholdDegrees))) {
autoBalanceXMode = true;
}
else if ( autoBalanceXMode &&
(fabs(pitchAngleDegrees) <=
fabs(kOnBalanceThresholdDegrees))) {
autoBalanceXMode = false;
}
if ( !autoBalanceYMode &&
(fabs(pitchAngleDegrees) >=
fabs(kOffBalanceThresholdDegrees))) {
autoBalanceYMode = true;
}
else if ( autoBalanceYMode &&
(fabs(pitchAngleDegrees) <=
fabs(kOnBalanceThresholdDegrees))) {
autoBalanceYMode = false;
}
// Control drive system automatically,
// driving in reverse direction of pitch/roll angle,
// with a magnitude based upon the angle
if ( autoBalanceXMode ) {
double pitchAngleRadians = pitchAngleDegrees * (M_PI / 180.0);
xAxisRate = sin(pitchAngleRadians) * -1;
}
if ( autoBalanceYMode ) {
double rollAngleRadians = rollAngleDegrees * (M_PI / 180.0);
yAxisRate = sin(rollAngleRadians) * -1;
}
try {
// Use the joystick X axis for lateral movement, Y axis for forward movement, and Z axis for rotation.
robotDrive.MecanumDrive_Cartesian(xAxisRate, yAxisRate,stick.GetZ());
} catch (std::exception ex ) {
std::string err_string = "Drive system error: ";
err_string += ex.what();
DriverStation::ReportError(err_string.c_str());
}
Wait(0.005); // wait 5ms to avoid hogging CPU cycles
}
}
/**
* Runs the motors with arcade steering.
*/
void OperatorControl()
{
cout << "Hello operator!" << endl;
myRobot.SetSafetyEnabled(true);
while (IsOperatorControl())
{
myRobot.ArcadeDrive(stick); // drive with arcade style (use right stick)
Wait(0.005); // wait for a motor update time
}
cout << "Bye operator!" << endl;
}
/**
* Code to be run during the remaining 2:20 of the match (after Autonomous())
*/
void OperatorControl()
{
/* TODO: Investigate. At least year's (GTR East) competition, we reached the conclusion that disabling this was
* the only way we could get out robot code to work (reliably). Should this be set to false?
*/
robotDrive.SetSafetyEnabled(true);
while (IsOperatorControl())
{
robotDrive.ArcadeDrive(rightStick);
Wait(0.005);
}
}
// Test Autonomous
void TestAutonomous()
{
robotDrive.SetSafetyEnabled(false);
// STEP 1: Set all of the states.
// SAFETY AND SANITY - SET ALL TO ZERO
loaded = winchSwitch.Get();
loading = false;
intake.Set(0.0);
winch.Set(0.0);
// STEP 2: Move forward to optimum shooting position
Drive(-AUTO_DRIVE_SPEED, SHOT_POSN_DIST);
// STEP 3: Drop the arm for a clean shot
arm.Set(DoubleSolenoid::kForward);
Wait(1.0); // Ken
// STEP 4: Launch the catapult
LaunchCatapult();
Wait (1.0); // Ken
if (ds->GetDigitalIn(0))
{
// STEP 5: Start the intake motor and backup to our origin position to pick up another ball
InitiateLoad();
intake.Set(-INTAKE_COLLECT);
while (CheckLoad());
Drive(AUTO_DRIVE_SPEED, SHOT_POSN_DIST);
Wait(1.0);
// STEP 6: Shut off the intake, bring up the arm and move to shooting position
intake.Set(0.0);
arm.Set(DoubleSolenoid::kReverse);
Wait (1.0);
Drive(-AUTO_DRIVE_SPEED, SHOT_POSN_DIST);
// Step 7: drop the arm for a clean shot and shoot
arm.Set(DoubleSolenoid::kForward);
// UNTESTED KICKED OFF FIELD
Wait(1.0); // Ken
LaunchCatapult();
}
// Get us fully into the zone for 5 points
Drive(-AUTO_DRIVE_SPEED, INTO_ZONE_DIST - SHOT_POSN_DIST);
// SAFETY AND SANITY - SET ALL TO ZERO
intake.Set(0.0);
winch.Set(0.0);
}
/**
* Runs the motors with arcade steering.
*/
void OperatorControl(void)
{
myRobot.SetSafetyEnabled(true);
while (IsOperatorControl())
{
myRobot.ArcadeDrive(stick.getAxisLeftY(), stick.getAxisLeftX()); // drive with arcade style (use left stick)
dsLCD->Printf(DriverStationLCD::kUser_Line2, 1, "Move: %f4", stick.getAxisLeftY());
dsLCD->Printf(DriverStationLCD::kUser_Line3, 1, "Rotate: %f4", stick.getAxisLeftX());
dsLCD->UpdateLCD();
Wait(0.005); // wait for a motor update time
}
}
// Code to be run during the remaining 2:20 of the match (after Autonomous())
//
// OperatorControl
// * Calls all the above methods
void OperatorControl()
{
// SAFETY AND SANITY - SET ALL TO ZERO
intake.Set(0.0);
winch.Set(0.0);
arm.Set(DoubleSolenoid::kReverse);
/* TODO: Investigate. At least year's (GTR East) competition, we reached the conclusion that disabling this was
* the only way we could get out robot code to work (reliably). Should this be set to false?
*/
robotDrive.SetSafetyEnabled(false);
Timer clock;
int sanity = 0;
int bigSanity = 0;
loading = false;
loaded = winchSwitch.Get();
RegisterButtons();
gamepad.Update();
leftStick.Update();
compressor.Start();
while (IsOperatorControl() && IsEnabled())
{
clock.Start();
HandleDriverInputs();
HandleShooter();
HandleArm();
// HandleEject();
while (!clock.HasPeriodPassed(LOOP_PERIOD)); // add an IsEnabled???
clock.Reset();
sanity++;
if (sanity >= 100)
{
bigSanity++;
sanity = 0;
dsLCD->PrintfLine(DriverStationLCD::kUser_Line4, "%d", bigSanity);
}
gamepad.Update();
leftStick.Update();
dsLCD->UpdateLCD();
}
// SAFETY AND SANITY - SET ALL TO ZERO
intake.Set(0.0);
winch.Set(0.0);
}
/**
* Runs the motors with Mecanum drive.
*/
void OperatorControl()
{
robotDrive.SetSafetyEnabled(false);
while (IsOperatorControl() && IsEnabled())
{
// Use the joystick X axis for lateral movement, Y axis for forward movement, and Z axis for rotation.
// This sample does not use field-oriented drive, so the gyro input is set to zero.
robotDrive.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(), stick.GetZ());
Wait(0.005); // wait 5ms to avoid hogging CPU cycles
}
}
void OperatorControl()
{
myRobot.SetSafetyEnabled(true);
while (IsOperatorControl() && IsEnabled())
{
//MECANUM DRIVE
float yValue = stick->GetY();
float xValue = stick->GetX();
float rotation = stick->GetTwist();
myRobot.MecanumDrive_Cartesian(xValue, yValue, rotation, 0.0);
Wait(0.005);
}
}
/**
* Do auto stuff
*/
void Autonomous()
{
cout << "Hello autonomous!" << endl;
myRobot.SetSafetyEnabled(false);
myRobot.Drive(-0.125, 0.0); // drive forwards half speed
Wait(5.0); // for 5 seconds
myRobot.Drive(0.0, 0.0); // stop robot
Wait(1.0); // Wait 1 sec
myRobot.Drive(-0.125, 0.25); // Drive fwd @ 1/2 speed, 0.25 curve (right?)
Wait(1.0);
myRobot.Drive(0, 0);
cout << "Bye autonomous!" << endl;
}
void OperatorControl(void)
{
myRobot.SetSafetyEnabled(true);
gamepad.EnableButton(BUTTON_COLLECTOR_FWD);
gamepad.EnableButton(BUTTON_COLLECTOR_REV);
gamepad.EnableButton(BUTTON_SHOOTER);
gamepad.EnableButton(BUTTON_CLAW_1_LOCKED);
gamepad.EnableButton(BUTTON_CLAW_2_LOCKED);
gamepad.EnableButton(BUTTON_CLAW_1_UNLOCKED);
gamepad.EnableButton(BUTTON_CLAW_2_UNLOCKED);
gamepad.EnableButton(BUTTON_STOP_ALL);
gamepad.EnableButton(BUTTON_JOG_FWD);
gamepad.EnableButton(BUTTON_JOG_REV);
stick2.EnableButton(BUTTON_SHIFT);
// Set inital states for all switches and buttons
gamepad.Update();
indexSwitch.Update();
greenClawLockSwitch.Update();
yellowClawLockSwitch.Update();
stick2.Update();
// Set initial states for all pneumatic actuators
shifter.Set(DoubleSolenoid::kReverse);
greenClaw.Set(DoubleSolenoid::kReverse);
yellowClaw.Set(DoubleSolenoid::kReverse);
compressor.Start ();
while (IsOperatorControl())
{
gamepad.Update();
stick2.Update();
indexSwitch.Update();
greenClawLockSwitch.Update();
yellowClawLockSwitch.Update();
HandleCollectorInputs();
HandleDriverInputsManual();
HandleArmInputs();
HandleShooterInputs();
HandleResetButton();
UpdateStatusDisplays();
dsLCD->UpdateLCD();
Wait(0.005); // wait for a motor update time
}
}
void Test(void)
{
compressor->Start();
myRobot.SetSafetyEnabled(true);
GetWatchdog().SetEnabled(true);
GetWatchdog().SetExpiration(1);
GetWatchdog().Feed();
while(IsTest())
{
GetWatchdog().Feed();
Wait(0.1);
}
}
/**
* This autonomous (along with the chooser code above) shows how to select between different autonomous modes
* using the dashboard. The sendable chooser code works with the Java SmartDashboard. If you prefer the LabVIEW
* Dashboard, remove all of the chooser code and uncomment the GetString line to get the auto name from the text box
* below the Gyro
*
* You can add additional auto modes by adding additional comparisons to the if-else structure below with additional strings.
* If using the SendableChooser make sure to add them to the chooser code above as well.
*/
void Autonomous()
{
std::string autoSelected = *((std::string*)chooser->GetSelected());
//std::string autoSelected = SmartDashboard::GetString("Auto Selector", autoNameDefault);
std::cout << "Auto selected: " << autoSelected << std::endl;
if(autoSelected == autoNameCustom){
//Custom Auto goes here
std::cout << "Running custom Autonomous" << std::endl;
myRobot.SetSafetyEnabled(false);
myRobot.Drive(-0.5, 1.0); // spin at half speed
Wait(2.0); // for 2 seconds
myRobot.Drive(0.0, 0.0); // stop robot
} else {
//Default Auto goes here
std::cout << "Running default Autonomous" << std::endl;
myRobot.SetSafetyEnabled(false);
myRobot.Drive(-0.5, 0.0); // drive forwards half speed
Wait(2.0); // for 2 seconds
myRobot.Drive(0.0, 0.0); // stop robot
}
}
/**
* Runs the motors with arcade steering.
*/
void OperatorControl()
{
myRobot.SetSafetyEnabled(true);
while (IsOperatorControl() && IsEnabled())
{
leftIntake.Set(0.8);
rightIntake.Set(0.8);
leftLift.Set(operatorStick.GetRawAxis(1));
rightLift.Set(operatorStick.GetRawAxis(1));
myRobot.TankDrive(driverStick.GetRawAxis(5), driverStick.GetRawAxis(1), true); // drive with arcade style (use right stick)
Wait(0.005); // wait for a motor update time
}
}
void OperatorControl(void)
{
myRobot->SetSafetyEnabled(false);
LEDLights (true); //turn camera lights on
shooterspeedTask->Start((UINT32)this); //start counting shooter speed
kickerTask->Start((UINT32)this); //turns on the kicker task
kicker_in_motion = false;
while (IsOperatorControl() && !IsDisabled())
{
if (ControllBox->GetDigital(3)) //turn tracking on with switch 3 on controll box
{
tracking(ControllBox->GetDigital(7));
}
else
{
myRobot->TankDrive(leftstick, rightstick); //if tracking is off, enable human drivers
Wait(0.005); // wait for a motor update time
}
Shooter_onoff=ControllBox->GetDigital(4); //shoot if switch 4 is on
ballgatherer(ControllBox->GetDigital(5), rightstick->GetRawButton(10));
kicker_onoff=lonelystick->GetRawButton(1);
bridgeboot(ControllBox->GetDigital(6));
kicker_cancel=lonelystick->GetRawButton(2);
//kicker_down=rightstick->GetRawButton(11));
}
LEDLights (false);
shooterspeedTask->Stop();
kickerTask->Stop();
ballgatherer(false, false);
kickermotor->Set(Relay::kOff);
}
/**
* Runs the motors with Mecanum drive.
*/
void OperatorControl()
{
robotDrive.SetSafetyEnabled(false);
while (IsOperatorControl() && IsEnabled())
{
bool reset_yaw_button_pressed = stick.GetRawButton(1);
if ( reset_yaw_button_pressed ) {
ahrs->ZeroYaw();
}
bool rotateToAngle = false;
if ( stick.GetRawButton(2)) {
turnController->SetSetpoint(0.0f);
rotateToAngle = true;
} else if ( stick.GetRawButton(3)) {
turnController->SetSetpoint(90.0f);
rotateToAngle = true;
} else if ( stick.GetRawButton(4)) {
turnController->SetSetpoint(179.9f);
rotateToAngle = true;
} else if ( stick.GetRawButton(5)) {
turnController->SetSetpoint(-90.0f);
rotateToAngle = true;
}
double currentRotationRate;
if ( rotateToAngle ) {
turnController->Enable();
currentRotationRate = rotateToAngleRate;
} else {
turnController->Disable();
currentRotationRate = stick.GetTwist();
}
try {
/* Use the joystick X axis for lateral movement, */
/* Y axis for forward movement, and the current */
/* calculated rotation rate (or joystick Z axis), */
/* depending upon whether "rotate to angle" is active. */
robotDrive.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(),
currentRotationRate ,ahrs->GetAngle());
} catch (std::exception ex ) {
std::string err_string = "Error communicating with Drive System: ";
err_string += ex.what();
DriverStation::ReportError(err_string.c_str());
}
Wait(0.005); // wait 5ms to avoid hogging CPU cycles
}
}
/**
* Runs the motors with arcade steering.
*/
void OperatorControl()
{
myRobot.SetSafetyEnabled(true);
while (IsOperatorControl() && IsEnabled())
{
myRobot.ArcadeDrive(joystick); // drive with arcade style (use right stick)
if(joystick.GetRawButton(1)) {
crochets.Set(-0.6);
}
else if(joystick.GetRawButton(2)) {
crochets.Set(0.6);
}
Wait(0.005); // wait for a motor update time
}
}
Robot() :
robotDrive(Motor1, Motor2), // these must be initialized in the same order
stick(5), // as they are declared above.
lw(LiveWindow::GetInstance()),
autoLoopCounter(0),
Motor1(21),
Motor2(12),
Slave1(20),
Slave2(14),
t_motor(13),
arm_Motor(23),
finger_Motor(22),
intake_Spin_Motor(11),
intake_Winch_Motor(13),
stick2(4),
autoLoopCounter2(0)
{
robotDrive.SetExpiration(0.1);
robotDrive.SetSafetyEnabled(false);
Slave1.SetControlMode(CANSpeedController::kFollower);
Slave1.Set(21);
Slave2.SetControlMode(CANSpeedController::kFollower);
Slave2.Set(12);
Motor2.SetInverted(true); //12
Slave2.SetInverted(true);//14
arm_Motor.SetInverted(false);//23
t_motor.SetInverted(true);//23
// t_motor.SetControlMode(CANSpeedController::kVoltage);
// t_motor.Set(0);
// CameraServer::GetInstance()->SetQuality(50);
// CameraServer::GetInstance()->SetSize(2);
// //the camera name (ex "cam0") can be found through the roborio web interface
// CameraServer::GetInstance()->StartAutomaticCapture("cam0");
t_motor.SetControlMode(CANSpeedController::kPercentVbus);
// t_motor.SetVoltageCompensationRampRate(24.0);
t_motor.SetFeedbackDevice(CANTalon::QuadEncoder);
t_motor.SetPosition(0);
// t_motor.SetPID(1, 0, 0);
arm_Motor.SetControlMode(CANSpeedController::kPercentVbus);
finger_Motor.SetControlMode(CANSpeedController::kPercentVbus);
// ourRangefinder = new AnalogInput(0);
}
void Autonomous()
{
compressor.Start();//start compressor
myRobot.SetSafetyEnabled(false);
myRobot.Drive(-0.5, 0.0); // drive forwards half speed
Wait(2.0); // for 2 seconds
myRobot.Drive(0.0, 0.0); // stop robot
while (!shoot.Get()){//while shooter isnt cocked pull it back
shooter.SetSpeed(1); //set motor
}
shooterSole.Set(shooterSole.kForward);//Sets Solenoid forward, shoot ball
shooterSole.Set(shooterSole.kReverse);//Sets Solenoid backward
myRobot.Drive(1.0, -1.0);//turn around for 0.5 seconds, we have to check to see if it takes that long
Wait(0.5);//wait for 0.5 seconds
myRobot.Drive(0.0, 0.0);//stop robot
}
void Test() {
robotDrive.SetSafetyEnabled(false);
uint8_t toSend[10];//array of bytes to send over I2C
uint8_t toReceive[10];//array of bytes to receive over I2C
uint8_t numToSend = 1;//number of bytes to send
uint8_t numToReceive = 0;//number of bytes to receive
toSend[0] = 7; //send 0 to arduino
i2c.Transaction(toSend, numToSend, toReceive, numToReceive);
bool isSettingUp = true;
pickup.setGrabber(-1);
pickup.setLifter(1);
while (isSettingUp) {
isSettingUp = false;
if (grabOuterLimit.Get() == false) {
pickup.setGrabber(0);
}
else {
isSettingUp = true;
}
if (liftLowerLimit.Get()) {
pickup.setLifter(0);
}
else {
isSettingUp = true;
}
}
gyro.Reset();
liftEncoder.Reset();
grabEncoder.Reset();
toSend[0] = 8;
i2c.Transaction(toSend, numToSend, toReceive, numToReceive);
while(IsTest() && IsEnabled());
toSend[0] = 0;
i2c.Transaction(toSend, numToSend, toReceive, numToReceive);
}