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)
}
