void TeleopPeriodic() { drive_mode_t new_mode = drive_mode_chooser.GetSelected(); SmartDashboard::PutString("current mode", new_mode == TANK_DRIVE ? "Tank" : "Arcade"); if (new_mode != drive_mode) SetDriveMode(new_mode); if (drive_mode == TANK_DRIVE) { left_speed = accel(left_speed, pilot->LeftY(), TICKS_TO_ACCEL); right_speed = accel(right_speed, pilot->RightY(), TICKS_TO_ACCEL); drive->TankDrive(left_speed, right_speed); } else { rot_speed = accel(rot_speed, pilot->RightX(), TICKS_TO_ACCEL); SmartDashboard::PutNumber("rotation speed", rot_speed); rot_speed = pilot->RightX(); move_speed = accel(move_speed, pilot->LeftY(), TICKS_TO_ACCEL); drive->ArcadeDrive(move_speed * MOVE_SPEED_LIMIT, -rot_speed * MOVE_SPEED_LIMIT, false); } SmartDashboard::PutBoolean("clamp open", clamp->isOpen()); SmartDashboard::PutBoolean("sword in", clamp->isSwordIn()); SmartDashboard::PutData("gyro", gyro); // for (uint8 i = 0; i <= 15; ++i) // SmartDashboard::PutNumber(std::string("current #") + std::to_string(i), pdp->GetCurrent(i)); SmartDashboard::PutNumber("Current", pdp->GetTotalCurrent()); if (pilot->ButtonState(GamepadF310::BUTTON_A)) { clamp->open(); } else if (pilot->ButtonState(GamepadF310::BUTTON_B)){ clamp->close(); } clamp->update(); SmartDashboard::PutNumber("accelerometer Z", acceler->GetZ()); SmartDashboard::PutNumber("Encoder", encoder->Get()); flywheel->Set(pilot->RightTrigger()); if (pilot->LeftTrigger() != 0) flywheel->Set(-pilot->LeftTrigger()); SmartDashboard::PutNumber("Left Trigger:", pilot->LeftTrigger()); if (pilot->ButtonState(GamepadF310::BUTTON_X)) { cameraFeeds-> changeCam(cameraFeeds->kBtCamFront); } if (pilot->ButtonState(GamepadF310::BUTTON_Y)){ cameraFeeds-> changeCam(cameraFeeds->kBtCamBack); } cameraFeeds->run(); }
void AutonomousPeriodic() { if((Auto==1)||(Auto==2)) { while(gyro.GetAngle()<ang) { motorspeed=(1/90)*abs(gyro.GetAngle()-ang); myRobot.TankDrive(motorspeed/2,motorspeed/-2); } } if((Auto==3)||(Auto==4)) { while(gyro.GetAngle()>ang) { motorspeed=(1/90)*abs(gyro.GetAngle()-ang); myRobot.TankDrive(motorspeed/-2,motorspeed/2); } } while(EncDist.GetDistance()<EncVal1) { myRobot.Drive(-0.5, 0.0); } ang = -ang; if((Auto==1)||(Auto==2)) { while(gyro.GetAngle()<ang) { motorspeed=(1/90)*abs(gyro.GetAngle()-ang); myRobot.TankDrive(motorspeed/2,motorspeed/-2); } } if((Auto==3)||(Auto==4)) { while(gyro.GetAngle()>ang) { motorspeed=(1/90)*abs(gyro.GetAngle()-ang); myRobot.TankDrive(motorspeed/-2,motorspeed/2); } } launcher.Set(1.0); sonic.SetAutomaticMode(true); while(sonic.GetRangeInches()>DefToShot) { myRobot.Drive(-0.5,0.0); } myRobot.Drive(0.0,0.0); intake.Set(1.0); }
void RobotInit() { frame = imaqCreateImage(IMAQ_IMAGE_RGB, 0); //camera.reset(new AxisCamera("axis-camera.local")); table = NetworkTable::GetTable("GRIP/myContoursReport"); chooser = new SendableChooser(); chooser->AddDefault(autoNameDefault, (void*)&autoNameDefault); for (std::map<std::string, bool (*)()>::const_iterator it = Autonomous::crossFunctions.begin(); it!= Autonomous::crossFunctions.end(); it++) { chooser->AddObject(it->first, (void*)&(it->first)); } SmartDashboard::PutData("Auto Modes", chooser); posChooser = new SendableChooser(); posChooser->AddDefault(posDefault, (void*)&posToDegrees[stoi(posDefault)]); for (int i = 1; i < 6; i++) { posChooser->AddObject(std::to_string(i), (void*)&posToDegrees[i]); } SmartDashboard::PutData("Position", posChooser); shootChooser = new SendableChooser(); shootChooser->AddDefault(shootDefault, (void*)&shootDefault); shootChooser->AddObject("No", (void*)"No"); SmartDashboard::PutData("Shoot", shootChooser); drive = new RobotDrive(2,3,0,1); //drive = new RobotDrive(frontLeft, backLeft, frontRight, backRight); drive->SetInvertedMotor(RobotDrive::MotorType::kFrontLeftMotor, true); drive->SetInvertedMotor(RobotDrive::MotorType::kRearLeftMotor, true); drive->SetInvertedMotor(RobotDrive::MotorType::kFrontRightMotor, true); drive->SetInvertedMotor(RobotDrive::MotorType::kRearRightMotor, true); drive->SetExpiration(0.2); drive->SetMaxOutput(0.5); driveStick = new Joystick(0); shootStick = new Joystick(1); launchPiston = new Solenoid(3); //tiltPiston = new DoubleSolenoid(0,1); defensePiston = new DoubleSolenoid(0,1); launch1 = new VictorSP(4); launch2 = new VictorSP(5); launch1->SetInverted(true); winch = new VictorSP(6); otherWinch = new Victor(7); gyro = new AnalogGyro(1); leftEnc = new Encoder(2, 3, false, Encoder::EncodingType::k1X); rightEnc = new Encoder(0,1, false, Encoder::EncodingType::k1X); //Configure for inches.t551 leftEnc->SetDistancePerPulse(-0.06); rightEnc->SetDistancePerPulse(0.06); launcherSensor = new DigitalInput(9); Autonomous::init(drive, gyro, leftEnc, rightEnc); timer = new Timer(); defenseUp = false; debounce = false; //if (fork() == 0) { // system("/home/lvuser/grip &"); //} launcherDown = false; }
void TeleopPeriodic() { //camera->GetImage(frame); //imaqDrawShapeOnImage(frame, frame, { 10, 10, 100, 100 }, DrawMode::IMAQ_DRAW_VALUE, ShapeMode::IMAQ_SHAPE_OVAL, 0.0f); //CameraServer::GetInstance()->SetImage(frame); printf("Left Encoder: %i, Right Encoder: %i, Gyro: %f\n", leftEnc->Get(), rightEnc->Get(), gyro->GetAngle()); drive->ArcadeDrive(driveStick); drive->SetMaxOutput((1-driveStick->GetThrottle())/2); //printf("%f\n", (1-stick->GetThrottle())/2); //leftMotor->Set(0.1); //rightMotor->Set(0.1); if (shootStick->GetRawAxis(3) > 0.5) { launch1->Set(1.0); launch2->Set(1.0); } else if (shootStick->GetRawAxis(2) > 0.5) { printf("Power Counter: %i\n", powerCounter); if (powerCounter < POWER_MAX) { powerCounter++; launch1->Set(-0.8); launch2->Set(-0.8); } else { launch1->Set(-0.6); launch2->Set(-0.6); } } else { launch1->Set(0.0); launch2->Set(0.0); powerCounter = 0.0; } //use this button to spin only one winch, to lift up. if (shootStick->GetRawButton(7)) { otherWinch->Set(0.5); } else if (shootStick->GetRawButton(8)) { otherWinch->Set(-0.5); } else { otherWinch->Set(0.0); } if (shootStick->GetRawButton(5)) { winch->Set(-0.7); if (!shootStick->GetRawButton(7) && !shootStick->GetRawButton(8)) { // otherWinch->Set(-0.5); } } else if (shootStick->GetRawButton(6)) { winch->Set(0.7); if (!shootStick->GetRawButton(7) && !shootStick->GetRawButton(8)) { // otherWinch->Set(0.5); } } else { winch->Set(0.0); if (!shootStick->GetRawButton(7) && !shootStick->GetRawButton(8)) { //, otherWinch->Set(0.0); } } if (shootStick->GetRawButton(1)) { launchPiston->Set(1); } else { launchPiston->Set(0); } if (shootStick->GetRawButton(3)) { Autonomous::alignWithGoal(drive, launch1, launch2, winch, otherWinch, table, timer); } if (shootStick->GetRawButton(3) && debounce == false) { debounce = true; if (defenseUp) { defensePiston->Set(DoubleSolenoid::Value::kReverse); defenseUp = false; } else { defenseUp =true; defensePiston->Set(DoubleSolenoid::Value::kForward); } } else if (!shootStick->GetRawButton(3)){ debounce = false; } }
void AutonomousPeriodic() { drive->SetMaxOutput(1.0); printf("Distance: %f\n", rightEnc->GetDistance()); // if (!launcherDown) { // if (timer->Get() > 1.0) { // winch->Set(0.5); // otherWinch->Set(0.5); // } else if (timer->Get() < 3.0) { // winch->Set(0.5); // otherWinch->Set(0.0); // } else { // winch->Set(0.0); // otherWinch->Set(0.0); // launcherDown = true; // } // } if(done) { winch->Set(0.0); otherWinch->Set(0.0); drive->ArcadeDrive(0.0,0.0); if (autoCounter > 10) { launchPiston->Set(0); launch1->Set(0.0); launch2->Set(0.0); } else { autoCounter++; if (shoot == "Yes") { launch1->Set(1.0); launch2->Set(1.0); } } } else { if (autoSelected == "Approach Only") { done = Autonomous::approachOnly(); launch1->Set(0.0); launch2->Set(0.0); winch->Set(0.0); otherWinch->Set(0.0); } else if (!defenseCrossed) { if(Autonomous::crossFunctions.find(autoSelected) != Autonomous::crossFunctions.end()) { bool (*crossFunction)() = Autonomous::crossFunctions.at(autoSelected); defenseCrossed = crossFunction(); launch1->Set(0.0); launch2->Set(0.0); winch->Set(0.0); otherWinch->Set(0.0); } else { done = true; launch1->Set(0.0); launch2->Set(0.0); winch->Set(0.0); otherWinch->Set(0.0); drive->ArcadeDrive(0.0,0.0); } timer->Reset(); } else { if (autoSelected == "Spy Bot") { rotation = 90; } //after we cross... float difference = gyro->GetAngle() - rotation; if (difference > 10) { launch1->Set(0.0); launch2->Set(0.0); winch->Set(0.0); otherWinch->Set(0.0); drive->ArcadeDrive(0.0,difference * 0.3); timer->Reset(); } else { if (goal == "Yes") { if (!Autonomous::alignWithGoal(drive, launch1, launch2, winch, otherWinch, table, timer)) { launchPiston->Set(0); } else { launch1->Set(0.0); launch2->Set(0.0); winch->Set(0.0); otherWinch->Set(0.0); drive->ArcadeDrive(0.0,0.0); launchPiston->Set(1); done = true; } } else { launch1->Set(0.0); launch2->Set(0.0); winch->Set(0.0); otherWinch->Set(0.0); done = true; } } } } }
void TeleopPeriodic() { if(stick.GetRawButton(3)) { sm2kl.Set(1.0); sm2kr.Set(1.0); } if(rSwitch.Get()) { sm2kr.Set(0.0); sm2kl.Set(0.0); SmartDashboard::PutNumber("Tests",675); } if(stick.GetRawButton(5)) { sm2kl.Set(-1.0); sm2kr.Set(-1.0); SmartDashboard::PutNumber("Tests",675); } if((lEnc.GetDistance() > 1080)||(rEnc.GetDistance() > 1080)) { sm2kl.Set(0.0); sm2kr.Set(0.0); } if(stick.GetRawButton(10)) { actintake.Set(1.0); } else if(stick.GetRawButton(9)) { actintake.Set(-1.0); } else { actintake.Set(0.0); } xvalue = -stick.GetZ()*.5; yvalue = -stick.GetY(); if(stick.GetRawButton(7)&&!toggle) { latch=!latch; toggle=true; } if(!stick.GetRawButton(7)&&toggle) { toggle=false; } if(latch) { yvalue=-yvalue; } if(stick.GetRawButton(1)) { launcher.Set(1.0); } if(stick.GetRawButton(11)) { intake.Set(1.0); } myRobot.ArcadeDrive(yvalue,xvalue); SmartDashboard::PutNumber("rSwitch",rSwitch.Get()); SmartDashboard::PutBoolean("latch",latch); SmartDashboard::PutNumber("Button",stick.GetRawButton(5)); //myRobot.ArcadeDrive(stick); // drive with arcade style (use right stick) }