void Drive (float speed, float turn, float strafe) { Dlf->Set(range(speed + turn + strafe)*250, 2); Dlb->Set(range(speed + turn - strafe)*250, 2); Drf->Set(range(-speed + turn + strafe)*250, 2); Drb->Set(range(-speed + turn - strafe)*250, 2); //CANJaguar::UpdateSyncGroup(2); }
void DoctaEight::leftyrighty(double left, double right)//set drive motors on either side { REDRUM; righty.Set(negate*right*.9); rightyB.Set(negate*right*.9); lefty.Set(negate*left*.9); leftyB.Set(negate*left*.9); }
void setMotors(float left,float right) { left=-left; right=-right; left*=300; right*=300; frontLeft->Set(-left); frontRight->Set(right); backRight->Set(right); backLeft->Set(-left); //CANJaguar::UpdateSyncGroup(2); }
void OperatorControl(void) { while(!IsDisabled()) { GetWatchdog().Feed(); float speed = stick.GetRawAxis(2); float strafe = -1*stick.GetRawAxis(1); float turn = -1*stick.GetRawAxis(3); Dlf->Set(speed + turn + strafe); Dlb->Set(speed + turn - strafe); Drf->Set(-speed + turn + strafe); Drb->Set(-speed + turn - strafe); Wait(.05); } }
void RawControl::armAngle(float angle) { //angle/=300.0f; arm->Set(angle); //cout<<"angle= "<<angle<<"\n"; }
void AutonomousInit() { init(); currentDistance = 0; goalDistance = 6.0*12.0; // 6 feet autonomousShooterDelay = 0; shooterMotor->Set(0); loaderMotor->Set(Relay::kOff); }
void StartWheels() { if (!spinFastNow) { printf(">>> StartWheels\n"); Log(LOG_START, 0, 0); spinFastNow = true; // start shooter wheels in %vbus mode, max output #ifdef HAVE_TOP_WHEEL #ifdef HAVE_TOP_CAN1 jagVbus(topWheel1, maxOutput); Log(LOG_MODE, 1, 1); #endif #ifdef HAVE_TOP_PWM1 topWheel1->Set(maxOutput); Log(LOG_MODE, 1, 1); #endif #ifdef HAVE_TOP_CAN2 jagVbus(topWheel2, maxOutput); Log(LOG_MODE, 2, 1); #endif #endif #ifdef HAVE_BOTTOM_WHEEL #ifdef HAVE_BOTTOM_CAN1 jagVbus(bottomWheel1, maxOutput); Log(LOG_MODE, 3, 1); #endif #ifdef HAVE_BOTTOM_PWM1 bottomWheel1->Set(maxOutput); Log(LOG_MODE, 3, 1); #endif #ifdef HAVE_BOTTOM_CAN2 jagVbus(bottomWheel2, maxOutput); Log(LOG_MODE, 4, 1); #endif #endif topPID = bottomPID = false; // reset reporting counter report = 0; printf("<<< StartWheels\n"); } }
/** * Initialization code for test mode should go here. * * Use this method for initialization code which will be called each time * the robot enters test mode. */ void TestInit() { printf(">>> TestInit\n"); #ifdef HAVE_COMPRESSOR compressor->Start(); #endif #ifdef HAVE_ARM arm->Set(DoubleSolenoid::kOff); #endif #ifdef HAVE_INJECTOR injectorL->Set(DoubleSolenoid::kOff); injectorR->Set(DoubleSolenoid::kOff); #endif #ifdef HAVE_EJECTOR ejector->Set(false); #endif #ifdef HAVE_LEGS legs->Set(false); #endif #ifdef HAVE_TOP_WHEEL #ifdef HAVE_TOP_CAN1 jagVbus(topWheel1, 0.0); #endif #ifdef HAVE_TOP_PWM1 topWheel1->Set(0.0); #endif #ifdef HAVE_TOP_CAN2 jagVbus(topWheel2, 0.0); #endif #endif #ifdef HAVE_BOTTOM_WHEEL #ifdef HAVE_BOTTOM_CAN1 jagVbus(bottomWheel1, 0.0); #endif #ifdef HAVE_BOTTOM_PWM1 bottomWheel1->Set(0.0); #endif #ifdef HAVE_BOTTOM_CAN2 jagVbus(bottomWheel2, 0.0); #endif #endif printf("<<< TestInit\n"); }
void TeleopPeriodic() { SmartDashboard::PutNumber("joystickX",stick.GetX()); SmartDashboard::PutNumber("joystickY",stick.GetY()); //SmartDashboard::PutBoolean("f*****g buttons", stick.GetRawButton(1)); //SmartDashboard::PutNumber("potentiometer voltage", pot.GetVoltage()); SmartDashboard::PutBoolean("infra",infra.Get()); SmartDashboard::PutNumber("accelX",accel.GetX()); SmartDashboard::PutNumber("accelY",accel.GetY()); SmartDashboard::PutNumber("accelZ",accel.GetZ()); servo.Set( trueMap(stick.GetX(), 1, -1, 1, 0) // trueMap allows use of entire joystick ); SmartDashboard::PutNumber("servo", servo.Get()); jag1.Set(stick.GetY()); jag2.Set(stick.GetY()); //tal1.Set(stick.GetY()); SmartDashboard::PutNumber("jag1", jag1.Get()); SmartDashboard::PutNumber("jag2", jag2.Get()); /*SmartDashboard::PutNumber("encpos", enc.Get()); SmartDashboard::PutNumber("encspd", enc.GetRate());*/ if (stick.GetRawButton(1) && !actuatePressed) { pistonVal=!pistonVal; piston.Set(pistonVal ? DoubleSolenoid::kForward : DoubleSolenoid::kReverse); actuatePressed = true; } else if (!stick.GetRawButton(1)) actuatePressed = false; SmartDashboard::PutBoolean("piston forward", piston.Get() == DoubleSolenoid::kForward); }
void Claw(double joy) { if(joy < 10) joy = 10; if(joy > 230) joy = 230; int location = EncClaw.Get(); double speed = PIDClaw(joy, location); //if(location < 15) speed *= .2; if(speed > .32) speed = .32; if(speed < -.32) speed = -.32; if(speed < .1 && speed > -.1) speed = 0; arm2->Set(speed,2); }
/** * Run the closed loop position controller on the Jag. */ void OperatorControl() { printf("In OperatorControl\n"); GetWatchdog().SetEnabled(true); while (IsOperatorControl() && !IsDisabled()) { GetWatchdog().Feed(); // Set the desired setpoint speedJag.Set(stick.GetAxis(axis) * 150.0); UpdateDashboardStatus(); Wait(0.05); } }
void initRobot () { cerr << "running init\n"; Dlf->EnableControl(0); Dlb->EnableControl(0); Drf->EnableControl(0); Drb->EnableControl(0); arm1->EnableControl(); arm1_sec->EnableControl(); arm2->EnableControl(); Dlf->ConfigEncoderCodesPerRev(250); Dlf->SetPID(1,0,0); Dlb->ConfigEncoderCodesPerRev(250); Dlb->SetPID(1,0,0); Drf->ConfigEncoderCodesPerRev(250); Drf->SetPID(1,0,0); Drb->ConfigEncoderCodesPerRev(250); Drb->SetPID(1,0,0); Wait(.1); if(robotInted==false) { int count=220; arm2->Set(-.3); while(count-->0 && LimitClaw.Get() == 1) Wait(.005); arm2->Set(.15); while(count-->0 && LimitClaw.Get() == 0) Wait(.005); arm2->Set(0); if(count>0) EncClaw.Reset(); arm1->Set(-.3); arm1_sec->Set(-.3); while(count-->0 && LimitArm.Get() == 1) Wait(.005); arm1->Set(.5); arm1_sec->Set(.5); while(count-->0 && LimitArm.Get() == 0) Wait(.005); if(count>0) EncArm.Reset(); arm1->Set(0); arm1_sec->Set(0); robotInted = true; } }
void Arm(double joy) { int location = EncArm.Get(); /* if(location < 10 && joy < 0) joy = 0; if(location > 110 && joy > 0) joy = 0; arm1->Set(joy); arm1_sec->Set(joy); return; */ if(joy < -10) joy = -10; if(joy > 110) joy = 110; double speed = PIDArm(joy, location); if(speed > .5) speed = .5; if(speed < -.3) speed = -.3; if(speed < 0 && location < 10) speed = 0; if(speed > 0 && location > 110) speed = 0; speed = LowArm(speed); if(speed < .01 && speed > -.01) speed = 0; arm1->Set(speed,3); arm1_sec->Set(speed,3); }
void AutonomousPeriodic() { currentDistance = leftDriveEncoder->GetDistance(); shooterMotor->Set(-shooterMotorVolts); if (currentDistance >= goalDistance){ drive->setLeft(0); drive->setRight(0); loaderMotor->Set(Relay::kForward); } else { drive->setLeft(.49); drive->setRight(.5); } //log->info("LRD %f %f", // leftDriveEncoder->GetDistance(), // rightDriveEncoder->GetDistance()); //log->print(); }
void DoctaEight::OperatorControl(void) { REDRUM; LTop.SetSafetyEnabled(0); LTop.EnableControl(); LBot.SetSafetyEnabled(0); LBot.EnableControl(); while (IsOperatorControl()) { REDRUM; output(); if (pilot.GetRawButton(6)) { arm.Set (.3); } else if (pilot.GetRawButton(5)) { arm.Set (-.3); } else arm.Set (0); //Set Shooter state and reset RPMoffset if necessary if (copilot.GetRawButton(1)) //BUTTON A { REDRUM; if(ShooterState != 1) { REDRUM; ShooterState = 1; RPMoffset = 0; } } else if (copilot.GetRawButton(2)) //BUTTON B { REDRUM; if(ShooterState != 2) { REDRUM; ShooterState = 2; RPMoffset = 0; } } else if (copilot.GetRawButton(3)) //BUTTON X { REDRUM; if(ShooterState != 3) { REDRUM; ShooterState = 3; RPMoffset = 0; } } else if (copilot.GetRawButton(4)) //BUTTON Y { REDRUM; if(ShooterState != 4) { REDRUM; ShooterState = 4; RPMoffset = 0; } } //increment or decrement RPMoffset if(copilot.GetRawButton(5)) //BUTTON LB { REDRUM; FlagB5 = true; } else if(copilot.GetRawButton(6)) //BUTTON RB { REDRUM; FlagB6 = true; } if(FlagB5 == true && copilot.GetRawButton(5) == false) { REDRUM; RPMoffset -= 50; FlagB5 = false; } else if(FlagB6 && !copilot.GetRawButton(6)) { REDRUM; RPMoffset += 50; FlagB6 = false; } if (pilot.GetRawButton(1) && !cycle) { cycle = 1; negate*=-1; } else { cycle=0; } //prepare shooter/launcher ouput speed if(ShooterState == 1) { REDRUM; //BUTTON A LTopSpeed = MAX/12; LBotSpeed = RPMMid + RPMoffset; } else if(ShooterState == 2) { //BUTTON B REDRUM; LTopSpeed = MAX/12; LBotSpeed = RPMLow + RPMoffset; } else if(ShooterState == 3) { //BUTTON X REDRUM; LTopSpeed = MAX/12; LBotSpeed = RPMHigh + RPMoffset; } else if(ShooterState == 4) { //BUTTON Y REDRUM; LTopSpeed = 0; LBotSpeed = 0; } //set shooter launcher speed to CANJags LTop.Set(LTopSpeed); LBot.Set(-LBotSpeed); //move simple platform arm leftyrighty(-pilot.GetY(), -pilot.GetRawAxis(4)); intake.Set(-copilot.GetY()); } //stops encoders }
float setSpeed(float newMagnitude) { tarTheta = atan2(yVector, xVector); curTheta = -(posEncoder->GetVoltage() - FLOFFSET ) / 5 * 2 * PI; // Code Snippet diffTheta = tarTheta - curTheta; if (diffTheta > PI) { diffTheta -= 2*PI; } else if (diffTheta < -PI) { diffTheta += 2*PI; } if (diffTheta > PI/2) { diffTheta -= PI; mag = mag * -1; } else if (diffTheta < -PI/2) { diffTheta += PI; mag = mag * -1; } turnVel = diffTheta / (PI/2); if (0 < turnVel && turnVel < .25) { turnVel = .25; } if (0 > turnVel && turnVel > -.25) { turnVel = -.25; } if (fabs(diffTheta) < PI/45 ) { turnVel = 0; } if (((turnVel > 0 && prevTurnVel < 0) || (turnVel < 0&& prevTurnVel> 0)) && !changeSign) { changeSign = true; // moveTime = baneTimer.Get() + .1; **FIX BANETIMER } if (changeSign) { turnVel = 0; // if (moveTime < baneTimer.Get()) { changeSign = false; } } // /Code Snippet if (!(xVector == 0 && yVector == 0)) { turnWheel->Set(turnVel); moveWheel->Set(mag); } else { turnWheel->Set(0); moveWheel->Set(0); } }
void RunWheels() { // uint32_t t0, t1, t2, t3; // schedule updates to avoid overloading CAN bus or CPU switch (report++) { case 12: // 240 milliseconds report = 0; // reset counter case 0: // Update PID parameters double newP = SmartDashboard::GetNumber("Shooter P"); double newI = SmartDashboard::GetNumber("Shooter I"); double newD = SmartDashboard::GetNumber("Shooter D"); if (newP != kP || newI != kI || newD != kD) { kP = newP; kI = newI; kD = newD; #ifdef HAVE_TOP_WHEEL if (topPID) { #ifdef HAVE_TOP_CAN1 ; // topWheel1->SetPID( kP, kI, kD ); #endif #ifdef HAVE_TOP_CAN2 topWheel2->SetPID( kP, kI, kD ); #endif } #endif #ifdef HAVE_BOTTOM_WHEEL if (bottomPID) { #ifdef HAVE_BOTTOM_CAN1 ; // bottomWheel1->SetPID( kP, kI, kD ); #endif #ifdef HAVE_BOTTOM_CAN2 bottomWheel2->SetPID( kP, kI, kD ); #endif } #endif } break; case 4: // 80 milliseconds #ifdef HAVE_TOP_WHEEL //t0 = GetFPGATime(); // Get top output voltage, current and measured speed #ifdef HAVE_TOP_CAN1 double topI1 = topWheel1->GetOutputCurrent(); #endif #ifdef HAVE_TOP_CAN2 double topI2 = topWheel2->GetOutputCurrent(); topJagSpeed = topWheel2->GetSpeed(); #endif //t1 = GetFPGATime(); topTachSpeed = topTach->PIDGet(); #ifdef HAVE_TOP_CAN1 // stupid floating point! Log(LOG_CURRENT, 1, (uint32_t)(topI1 * 1000 + 0.5)); #endif #ifdef HAVE_TOP_CAN2 Log(LOG_CURRENT, 2, (uint32_t)(topI2 * 1000 + 0.5)); Log(LOG_SPEED, 2, (uint32_t)(topJagSpeed + 0.5)); #endif // Send values to SmartDashboard #ifdef HAVE_TOP_CAN1 SmartDashboard::PutNumber("Top Current 1", topI1); #endif #ifdef HAVE_TOP_CAN2 SmartDashboard::PutNumber("Top Current 2", topI2); SmartDashboard::PutNumber("Top Jag ", topJagSpeed); #endif SmartDashboard::PutNumber("Top Tach ", topTachSpeed); // Get setpoint topSpeed = SmartDashboard::GetNumber("Top Set "); //t2 = GetFPGATime(); if (spinFastNow) { if (topPID) { if (topJagSpeed < topSpeed * vbusThreshold) { topPID = false; // below threshold: switch both motors to full output #ifdef HAVE_TOP_CAN1 jagVbus(topWheel1, maxOutput); Log(LOG_MODE, 1, 1); #endif #ifdef HAVE_TOP_PWM1 topWheel1->Set(maxOutput); Log(LOG_MODE, 1, 1); #endif #ifdef HAVE_TOP_CAN2 jagVbus(topWheel2, maxOutput); Log(LOG_MODE, 2, 1); #endif } else { ; // above threshold: run motor 1 off, PID on motor 2 #ifdef HAVE_TOP_CAN1 topWheel1->Set(0.0); #endif #ifdef HAVE_TOP_PWM1 topWheel1->Set(0.0); #endif #ifdef HAVE_TOP_CAN2 topWheel2->Set(topSpeed); #endif } } else { if (topJagSpeed >= topSpeed * pidThreshold) { ; // above threshold: switch motor 1 off, motor 2 PID topPID = true; #ifdef HAVE_TOP_CAN1 topWheel1->Set(0.0); #endif #ifdef HAVE_TOP_PWM1 topWheel1->Set(0.0); #endif #ifdef HAVE_TOP_CAN2 jagPID(topWheel2, topSpeed); Log(LOG_MODE, 2, 2); #endif } else { ; // below threshold: run both motors at full output #ifdef HAVE_TOP_CAN1 topWheel1->Set(maxOutput); #endif #ifdef HAVE_TOP_PWM1 topWheel1->Set(maxOutput); #endif #ifdef HAVE_TOP_CAN2 topWheel2->Set(maxOutput); #endif } } //t3 = GetFPGATime(); //printf("%10u %10u %10u\n", (uint32_t)(t1 - t0), (uint32_t)(t2 - t1), (uint32_t)(t3 - t2)); } #endif break; case 8: // 160 milliseconds #ifdef HAVE_BOTTOM_WHEEL // Get bottom output voltage, current and measured speed //t0 = GetFPGATime(); #ifdef HAVE_BOTTOM_CAN1 double bottomI1 = bottomWheel1->GetOutputCurrent(); #endif #ifdef HAVE_BOTTOM_CAN2 double bottomI2 = bottomWheel2->GetOutputCurrent(); bottomJagSpeed = bottomWheel2->GetSpeed(); #endif //t1 = GetFPGATime(); bottomTachSpeed = bottomTach->PIDGet(); #ifdef HAVE_BOTTOM_CAN1 Log(LOG_CURRENT, 3, (uint32_t)(bottomI1 * 1000 + 0.5)); #endif #ifdef HAVE_BOTTOM_CAN2 Log(LOG_CURRENT, 4, (uint32_t)(bottomI2 * 1000 + 0.5)); Log(LOG_SPEED, 4, (uint32_t)(bottomJagSpeed + 0.5)); #endif // Send values to SmartDashboard #ifdef HAVE_BOTTOM_CAN1 SmartDashboard::PutNumber("Bottom Current 1", bottomI1); #endif #ifdef HAVE_BOTTOM_CAN2 SmartDashboard::PutNumber("Bottom Current 2", bottomI2); SmartDashboard::PutNumber("Bottom Jag ", bottomJagSpeed); #endif SmartDashboard::PutNumber("Bottom Tach ", bottomTachSpeed); // Get setpoint bottomSpeed = SmartDashboard::GetNumber("Bottom Set "); //t2 = GetFPGATime(); if (spinFastNow) { if (bottomPID) { if (bottomJagSpeed < bottomSpeed * vbusThreshold) { bottomPID = false; // below threshold: switch both motors to full output #ifdef HAVE_BOTTOM_CAN1 jagVbus(bottomWheel1, maxOutput); Log(LOG_MODE, 3, 1); #endif #ifdef HAVE_BOTTOM_PWM1 bottomWheel1->Set(maxOutput); Log(LOG_MODE, 3, 1); #endif #ifdef HAVE_BOTTOM_CAN2 jagVbus(bottomWheel2, maxOutput); Log(LOG_MODE, 4, 1); #endif } else { ; // above threshold: run motor 1 off, PID on motor 2 #ifdef HAVE_BOTTOM_CAN1 bottomWheel1->Set(0.0); #endif #ifdef HAVE_BOTTOM_PWM1 bottomWheel1->Set(0.0); #endif #ifdef HAVE_BOTTOM_CAN2 bottomWheel2->Set(bottomSpeed); #endif } } else { if (bottomJagSpeed >= bottomSpeed * pidThreshold) { // above threshold: switch motor 1 off, motor 2 PID bottomPID = true; #ifdef HAVE_BOTTOM_CAN1 bottomWheel1->Set(0.0); #endif #ifdef HAVE_BOTTOM_PWM1 bottomWheel1->Set(0.0); #endif #ifdef HAVE_BOTTOM_CAN2 jagPID(bottomWheel2, bottomSpeed); Log(LOG_MODE, 4, 2); #endif } else { ; // below threshold: run both motors at full output #ifdef HAVE_BOTTOM_CAN1 bottomWheel1->Set(maxOutput); #endif #ifdef HAVE_BOTTOM_PWM1 bottomWheel1->Set(maxOutput); #endif #ifdef HAVE_BOTTOM_CAN2 bottomWheel2->Set(maxOutput); #endif } } //t3 = GetFPGATime(); //printf("%10u %10u %10u\n", (uint32_t)(t1 - t0), (uint32_t)(t2 - t1), (uint32_t)(t3 - t2)); } #endif break; } }
void TeleopPeriodic() { displayCount++; //tbs change button number if ((control->gamepadButton(9) && control->gamepadButton(10)) || // start climbState != NotInitialized) { // continue // Do we want a manual abort here? ClimbPeriodic(); return; } // drive drive->setLeft(control->left()); // control->setRightScale(.95); drive->setRight(control->right()); drive->setScale(control->throttle()); //drive->setLowShift(control->button(1)); // right trigger drive->setReversed(control->toggleButton(11)); // right JS button 11 //turn light on or off //lightRing->Set(control->gamepadToggleButton(4) ? Relay::kForward : Relay::kOff ); blowerMotor->Set(control->gamepadButton(6) ? 1.0 : 0.0 ); // For the loader, if we are rotating, wait for at least 100 counts before // checking the switch or adjusting motor power if (loading) { //if (loaderSwitch->Get()) { //loaderDisengageDetected = true; //} loadSwitchDelay++; // If already rotating, and the switch trips, power down the motor if (/*loaderDisengageDetected*/ loaderSwitch->Get() != loadSwitchOldState) { loaderMotor->Set(Relay::kOff); loading = false; loadSwitchOldState = loaderSwitch->Get(); } } else { // If not rotating and the gamepad button is set, start rotating if (control->gamepadButton(7)) { loadSwitchDelay = 0; loadCount++; loaderMotor->Set(Relay::kForward); loading = true; loaderDisengageDetected = false; } } if (control->gamepadToggleButton(4)){ if (control->gamepadRightVertical() > 0.05 || control->gamepadRightVertical() < -0.05) { shooterMotorVolts += control->gamepadRightVertical(); if (shooterMotorVolts < 6.0) shooterMotorVolts = 6.0; if (shooterMotorVolts > 12.0) shooterMotorVolts = 12.0; } } // For the shooter, spin it up or down based on the toggle // if (control->gamepadToggleButton(8)) { shooterMotor->Set(-shooterMotorVolts);// negative because motor is wired backwerds log->info("shooter on"); } else { shooterMotor->Set(0.0); log->info("shooter off"); } if (control->gamepadLeftVertical() > 0.05 || control->gamepadLeftVertical() < -0.05) { cameraElevateAngle += control->gamepadLeftVertical()*5; if (cameraElevateAngle < cameraElevateMotor->GetMinAngle()) cameraElevateAngle = cameraElevateMotor->GetMinAngle(); if (cameraElevateAngle > cameraElevateMotor->GetMaxAngle()) cameraElevateAngle = cameraElevateMotor->GetMaxAngle(); } if (control->gamepadLeftHorizontal() > 0.05 || control->gamepadLeftHorizontal() < -0.05) { cameraPivotAngle += control->gamepadLeftHorizontal()*5; if (cameraPivotAngle < cameraPivotMotor->GetMinAngle()) cameraPivotAngle = cameraPivotMotor->GetMinAngle(); if (cameraPivotAngle > cameraPivotMotor->GetMaxAngle()) cameraPivotAngle = cameraPivotMotor->GetMaxAngle(); } cameraPivotMotor->SetAngle(cameraPivotAngle); cameraElevateMotor->SetAngle(cameraElevateAngle); if (control->gamepadToggleButton(1)) { // If the climber lower limit switch is set, and the distance is >0, set it to 0 if (!leftClimber->lowerLimitSwitch->Get()) { leftClimber->encoder->Reset(); leftClimber->encoder->Start(); // Only allow forward motion if (control->gamepadRightVertical() > 0) { leftClimber->motor->Set(control->gamepadRightVertical()); } else { leftClimber->motor->Set(0.0); } } else { leftClimber->motor->Set(control->gamepadRightVertical()); } } if (control->gamepadToggleButton(3)) { // If the climber lower limit switch is set, and the distance is >0, set it to 0 if (!rightClimber->lowerLimitSwitch->Get()) { rightClimber->encoder->Reset(); rightClimber->encoder->Start(); // Only allow forward motion if (control->gamepadRightVertical() > 0) { rightClimber->motor->Set(control->gamepadRightVertical()); } else { rightClimber->motor->Set(0.0); } } else { rightClimber->motor->Set(control->gamepadRightVertical()); } } // assorted debug //log->info("Shift %s", control->toggleButton(8) // ? "low" : "high"); //log->info("ArmPot: %.0f", arm->encoderValue()); //log->info("pidf: %.2f", arm->pidFactor()); //log->info("piden: %s", arm->isPidEnabled() ? "true" : "false"); //double myTest = drive->rightPosition(); //log->info("renc: %f", myTest); //myTest = drive->leftPosition(); //log->info("lenc: %f", myTest); //log->info("gplh %f %f", control->gamepadLeftHorizontal(), cameraPivotAngle); //log->info("gplv %f %f", control->gamepadLeftVertical(), cameraElevateAngle); //log->info("gprh %f", control->gamepadRightHorizontal()); //log->info("gprv %f", control->gamepadRightVertical()); if (display()) { //log->info("lg %d ldd %d", loading, loaderDisengageDetected); //log->info("BCD: %d", bcd->value()); log->info("SHV: %f", shooterMotorVolts); //log->info("LdCDS: %d %d %d", loadCount, loaderDisengageDetected, loaderSwitch->Get()); log->info("ena: %c%c%c%c", " L"[control->gamepadToggleButton(1)], " R"[control->gamepadToggleButton(3)], " J"[control->gamepadToggleButton(2)], " S"[control->gamepadToggleButton(4)]); log->info("LRC %f %f", leftClimber->encoder->GetDistance(), rightClimber->encoder->GetDistance()); log->info("LRD %f %f", leftDriveEncoder->GetDistance(), rightDriveEncoder->GetDistance()); log->info("LRL %d %d %d %d %d %d %d", leftClimber->lowerLimitSwitch->Get(), leftClimber->lowerHookSwitch->Get(), leftClimber->upperHookSwitch->Get(), rightClimber->lowerLimitSwitch->Get(), rightClimber->lowerHookSwitch->Get(), rightClimber->upperHookSwitch->Get(), loaderSwitch->Get()); log->print(); } }
void TeleopInit() { init(); drive->setShiftMode(Drive::Manual); shooterMotor->Set(0.0); loaderMotor->Set(Relay::kOff); }
void CANJaguarConfiguration :: Configure ( CANJaguar & Jag ) { Jag.DisableControl (); Jag.ConfigMaxOutputVoltage ( MaxOutputVoltage ); Jag.SetVoltageRampRate ( VoltageRampRate ); Jag.ConfigFaultTime ( FaultTime ); Jag.ConfigNeutralMode ( NeutralMode ); Jag.ConfigLimitMode ( LimitMode ); Jag.ConfigForwardLimit ( ForwardPositionLimit ); Jag.ConfigForwardLimit ( ReversePositionLimit ); switch ( Feedback ) { case kFeedbackType_None: switch ( Mode ) { case CANSpeedController :: kPercentVbus: Jag.SetPercentMode (); break; case CANSpeedController :: kCurrent: Jag.SetCurrentMode ( P, I, D ); Jag.Set ( 0 ); Jag.EnableControl (); break; case CANSpeedController :: kVoltage: Jag.SetVoltageMode (); break; default: Jag.SetVoltageMode ( CANJaguar :: Encoder, EncoderCounts ); Jag.ConfigMaxOutputVoltage ( 0.0 ); break; } break; case kFeedbackType_Encoder: switch ( Mode ) { case CANSpeedController :: kPercentVbus: Jag.SetPercentMode ( CANJaguar :: Encoder, EncoderCounts ); break; case CANSpeedController :: kCurrent: Jag.SetCurrentMode ( CANJaguar :: Encoder, EncoderCounts, P, I, D ); Jag.Set ( 0 ); Jag.EnableControl (); break; case CANSpeedController :: kSpeed: Jag.SetSpeedMode ( CANJaguar :: Encoder, EncoderCounts, P, I, D ); Jag.Set ( 0 ); Jag.EnableControl (); break; case CANSpeedController :: kVoltage: Jag.SetVoltageMode ( CANJaguar :: Encoder, EncoderCounts ); break; default: Jag.SetVoltageMode ( CANJaguar :: Encoder, EncoderCounts ); Jag.ConfigMaxOutputVoltage ( 0.0 ); break; } break; case kFeedbackType_QuadEncoder: switch ( Mode ) { case CANSpeedController :: kPercentVbus: Jag.SetPercentMode ( CANJaguar :: QuadEncoder, EncoderCounts ); break; case CANSpeedController :: kCurrent: Jag.SetCurrentMode ( CANJaguar :: QuadEncoder, EncoderCounts, P, I, D ); Jag.Set ( 0 ); Jag.EnableControl (); break; case CANSpeedController :: kSpeed: Jag.SetSpeedMode ( CANJaguar :: QuadEncoder, EncoderCounts, P, I, D ); Jag.Set ( 0 ); Jag.EnableControl (); break; case CANSpeedController :: kPosition: Jag.SetPositionMode ( CANJaguar :: QuadEncoder, EncoderCounts, P, I, D ); Jag.Set ( 0 ); Jag.EnableControl (); break; case CANSpeedController :: kVoltage: Jag.SetVoltageMode ( CANJaguar :: QuadEncoder, EncoderCounts ); Jag.Set ( 0 ); Jag.EnableControl (); break; default: Jag.SetVoltageMode ( CANJaguar :: Encoder, EncoderCounts ); Jag.ConfigMaxOutputVoltage ( 0.0 ); break; } break; case kFeedbackType_Potentiometer: switch ( Mode ) { case CANSpeedController :: kPercentVbus: Jag.SetPercentMode ( CANJaguar :: Potentiometer ); break; case CANSpeedController :: kCurrent: Jag.SetCurrentMode ( CANJaguar :: Potentiometer, P, I, D ); Jag.Set ( 0 ); Jag.EnableControl (); break; case CANSpeedController :: kPosition: Jag.SetPositionMode ( CANJaguar :: Potentiometer, P, I, D ); Jag.Set ( 0 ); Jag.EnableControl (); break; case CANSpeedController :: kVoltage: Jag.SetVoltageMode ( CANJaguar :: Potentiometer ); break; default: Jag.SetVoltageMode ( CANJaguar :: Encoder, EncoderCounts ); Jag.ConfigMaxOutputVoltage ( 0.0 ); break; } break; } };