// WHen robot is enabled
void init() {
log->info("initializing");
log->print();
climber = NULL;
leftDriveEncoder->Reset();
leftDriveEncoder->SetDistancePerPulse(DriveDistancePerPulse);
leftDriveEncoder->Start();
rightDriveEncoder->Reset();
rightDriveEncoder->SetDistancePerPulse(DriveDistancePerPulse);
rightDriveEncoder->Start();
//leftClimber->motorController->Disable();
leftClimber->encoder->Reset();
leftClimber->encoder->Start();
//rightClimber->motorController->Disable();
rightClimber->encoder->Reset();
rightClimber->encoder->Start();
bcdValue = bcd->value();
loadSwitchOldState = loaderSwitch->Get();
#if 0
bool leftDone = false;
bool rightDone = false;
// Only do this for some BCD values?
while (!leftDone || !rightDone){
if (!leftDone)
leftDone = leftClimber->UpdateState(-1.0, -30);
if (!rightDone)
rightDone = rightClimber->UpdateState(-1.0, -30);
log->info("Wait: Ll Rl: %d %d",
leftClimber->lowerLimitSwitch->Get(),
rightClimber->lowerLimitSwitch->Get());
log->print();
}
#endif
climbState = NotInitialized;
cameraElevateAngle =
(cameraElevateMotor->GetMaxAngle()-cameraElevateMotor->GetMinAngle()) * 2/3;
cameraPivotAngle = 0;
cameraPivotMotor->SetAngle(cameraPivotAngle);
cameraElevateMotor->SetAngle(cameraElevateAngle);
loading = false;
loaderDisengageDetected = false;
//This is a rough guess of motor power it should be based on voltage
shooterMotorVolts = 8.0; // volts as a fraction of 12V
loadCount = 0;
}
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();
}
}
