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