void TeleopPeriodic() { if(!isWait) { //drive drive->drive(oi->joyDrive->GetRawAxis(FORWARD_Y_AXIS)), setTurnSpeed(oi->joyDrive->GetRawAxis(TURN_X_AXIS)); drive->shift(oi->joyDrive->GetRawButton(8), oi->joyDrive->GetRawButton(9)); //manipulator manip->moveArm(oi->joyManip->GetRawButton(6), oi->joyManip->GetRawButton(7)); manip->intakeBall(oi->joyManip->GetRawButton(INTAKE_BUTTON), oi->joyManip->GetRawButton(OUTTAKE_BUTTON), (oi->getManipJoystick()->GetThrottle()+1)/2); catapult->launchBall(); manip->toggleCompressor(oi->joyDrive->GetRawButton(COMPRESSOR_BUTTON)); } //camera motor mount if(oi->joyManip->GetRawButton(10)) { bCameraLatch = true; } else if(oi->joyManip->GetRawButton(11)) { bCameraLatch = false; } manip->toggleCameraPosition(bCameraLatch); }
/** Tests that the encoder measurements are being correctly translated into wheel speeds */ void tests::measurements() { Drive md; EncoderMeasurement measure(md); md.setMLSpeed(400); md.setMRSpeed(-400); uint32_t prevTime; int i =0; while(true) { uint32_t currTime = micros(); if (currTime - prevTime >= PERIOD_MICROS) { prevTime = currTime; measure.update(); i++; if(i % 5 == 0) { Serial.print(prevTime); Serial.print("\t"); Serial.print(measure.mVR); Serial.print("\t"); Serial.println(measure.mVL); } } } }
Drive* DriveManager::getDrive(std::string type, int index) { Drive* drive = createDrive(type, index); drive->updateFreeSpace(); drive->doQuickSMARTCheck(); return drive; }
Robot() { log = new Log(this); bcd = new BcdSwitch(new DigitalInput(2,11), new DigitalInput(2,12), new DigitalInput(2,13), new DigitalInput(2,14)); bcdValue = bcd->value(); control = new Control( new Joystick(1), new Joystick(2), new Joystick(3), Control::Tank, log); control->setLeftScale(-1); control->setRightScale(-1); control->setGamepadScale(-1); drive = new Drive(this, 1,2); drive->addMotor(Drive::Left, 2, 1); drive->addMotor(Drive::Left, 3, 1); drive->addMotor(Drive::Right, 4, 1); drive->addMotor(Drive::Right, 5, 1); rightDriveEncoder = new Encoder(1,1, 1,2, true, Encoder::k2X); leftDriveEncoder = new Encoder(1,3, 1,4, false, Encoder::k2X); //compressor = new Compressor(2,9, 2,3); //press, relay //compressor->Start(); leftClimber = new Climber( this, "leftClimber", new CANJaguar(6), new Encoder(1,5, 1,6, true), ClimberDistancePerPulse, new DigitalInput(2,1),//lower limit switch new DigitalInput(2,2),//lower hook switch new DigitalInput(2,3) );//upper hook switch rightClimber = new Climber( this, "rightClimber", new CANJaguar(7), new Encoder(1,7, 1,8, true), ClimberDistancePerPulse, new DigitalInput(2,4),//lower limit switch new DigitalInput(2,5),//lower hook switch new DigitalInput(2,6) );//upper hook switch loaderMotor = new Relay(2,1); loaderSwitch = new DigitalInput(2,8); shooterMotor = new CANJaguar(10, CANJaguar::kVoltage); blowerMotor = new CANJaguar(9); cameraPivotMotor = new Servo(1,9); cameraElevateMotor = new Servo(1,10); lightRing = new Relay(2,4); }
// // Main Tele Operator Mode function. This function is called once, therefore a while loop that checks IsOperatorControl and IsEnabled is used // to maintain control until the end of tele operator mode // void OperatorControl() { //myRobot.SetSafetyEnabled(true); timer->Start(); Relay* reddlight = new Relay(4); //Timer* lighttimer = new Timer(); //lighttimer->Start(); while (IsOperatorControl() && IsEnabled()) { reddlight->Set(reddlight->kForward); /* if (lighttimer->Get()<=0.5) { reddlight->Set(reddlight->kForward); } else if(lighttimer->Get()<1){ reddlight->Set(reddlight->kOff); } else { lighttimer->Reset(); } */ // // Get inputs // driverInput->GetInputs(); drive->GetInputs(); catapult->GetInputs(); feeder->GetInputs(); // // Pass values between components as necessary // //catapult->SetSafeToFire(feeder->GetAngle()<95); // // Execute one step on each component // drive->ExecStep(); catapult->ExecStep(); feeder->ExecStep(); // // Set Outputs on all components // catapult->SetOutputs(); feeder->SetOutputs(); // // Wait for step timer to expire. This allows us to control the amount of time each step takes. Afterwards, restart the // timer for the next loop // while (timer->Get()<(PERIOD_IN_SECONDS)); timer->Reset(); } }
void TeleopPeriodic() { // Comment the next line out to disable movement drive->doDrive(stick->GetX(), -stick->GetY()); intake->periodic(); shooter->periodic(); camSystem->periodic(); }
void tests::servo_drive_interaction() { VS11 servo(3); servo.setGoal(0); delay(1000); servo.setGoal(M_PI/2); delay(1000); Serial.println("0"); { Drive d; d.setMLSpeed(100); d.setMRSpeed(100); delay(1000); servo.setGoal(0); delay(1000); d.setMLSpeed(-100); d.setMRSpeed(-100); delay(1000); servo.setGoal(M_PI/2); delay(1000); d.setMLSpeed(0); d.setMRSpeed(0); } }
bool Agent::fromBottle(Bottle b) { if (!this->Object::fromBottle(b)) return false; if (!b.check("belief")||!b.check("emotions")) return false; m_belief.clear(); Bottle* beliefs = b.find("belief").asList(); for(int i=0; i<beliefs->size() ; i++) { Bottle* bRelation = beliefs->get(i).asList(); Relation r(*bRelation); m_belief.push_back(r); } m_emotions_intrinsic.clear(); Bottle* emotions = b.find("emotions").asList(); for(int i=0; i<emotions->size() ; i++) { Bottle* bEmo = emotions->get(i).asList(); string emotionName = bEmo->get(0).asString().c_str(); double emotionValue = bEmo->get(1).asDouble(); m_emotions_intrinsic[emotionName.c_str()] = emotionValue; } m_drives.clear(); Bottle* drivesProperty = b.find("drives").asList(); string drivesDebug = drivesProperty->toString().c_str(); for(int i=0; i<drivesProperty->size() ; i++) { Bottle* bD = drivesProperty->get(i).asList(); string drivesDebug1 = bD->toString().c_str(); Drive currentDrive; currentDrive.fromBottle(*bD); m_drives[currentDrive.name] = currentDrive; } Bottle* bodyProperty = b.find("body").asList(); m_body.fromBottle(*bodyProperty); return true; }
const std::string PncKeyGenerator::generate_key(const Drive& drive) { const auto drive_serial_number = drive.get_fru_info().get_serial_number(); if (!drive_serial_number.has_value()) { throw KeyValueMissingError("Serial number is missing."); } return generate_key_base(drive) + drive_serial_number.value(); }
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(); }
/** Tests that the encoders work when the motors are spun BY HAND */ void tests::encoder_wiring() { Drive md; Serial.println("Turn the motors by hand"); uint32_t lastr, lastl; while(true) { uint32_t encr = md.getMREncoder(); uint32_t encl = md.getMLEncoder(); if(encr != lastr || encl != lastl) { Serial.print(encr); Serial.print(", "); Serial.print(encl); Serial.println(); } delay(10); lastl = encl; lastr = encr; } }
int main() { Drive d; cout << d.show() <<endl; cout << d.show_protected_data() <<endl; Drive *dd = new Drive; Base *b = dd; //静态是Base动态是Drive b->v_func(); //结果是d data: 0 指针是所以调用动态函数但是参数是静态 就是base的 0 b->no_virtual();//结果是Base类的函数 Drive_private *pd = new Drive_private; cout << pd->show_protected_data() <<endl; //pd->acessy_pubilc_inhirit(); //error 不能被获取 }
/** Tests that the PID controllers work */ void tests::motor_feedback() { Drive md; MotorController cont(md); EncoderMeasurement measure(md); uint32_t prevTime; int i =0; while(true) { uint32_t currTime = micros(); if (currTime - prevTime >= PERIOD_MICROS) { prevTime = currTime; measure.update(); cont.controlMR(0.1, measure.mVR); // right motor PI control cont.controlML(-0.1, measure.mVL); //left motor PI control i++; if(i % 100 == 0) { Serial.print(measure.encoderRCount); Serial.print(", "); Serial.println(measure.encoderLCount); Serial.print(": "); Serial.print(measure.mVR, 4); Serial.print(", "); Serial.println(measure.mVL, 4); } if(md.faulted()) { Serial.println("Motor fault"); md.setMLSpeed(0); md.setMRSpeed(0); delay(100); md.clearFault(); } } } }
void TeleopPeriodic() { manip->toggleCompressor(oi->joyDrive->GetRawButton(COMPRESSOR_BUTTON)); //drive drive->drive(oi->joyDrive->GetRawAxis(FORWARD_Y_AXIS), oi->joyDrive->GetRawAxis(TURN_X_AXIS)); //drive->shift(oi->joyDrive->GetRawButton(8), oi->joyDrive->GetRawButton(9)); //manipulator // TJF: Replaced "magic numbers" with named constants manipArm->moveArm(oi->joyDrive->GetRawButton(UP_INTAKE_BUTTON), oi->joyDrive->GetRawButton(DOWN_INTAKE_BUTTON)); //manipArm->moveArm(oi->joyDrive->GetRawButton(6), oi->joyDrive->GetRawButton(7)); manip->intakeBall(oi->joyDrive->GetRawButton(INTAKE_BUTTON), oi->joyDrive->GetRawButton(OUTTAKE_BUTTON), (oi->joyDrive->GetThrottle()+1)/2); // TJF: removed only because it doesn't work yet catapult->launchBall(); printSmartDashboard(); // 01/18/2016 moved this function because it needed to be updated constantly instead of initializing it only once. }
bool Changer::LoadCartridge( Drive & drive, Cartridge & cartridge, Error & error) { boost::lock_guard<boost::mutex> lock(detail_->mutex); int slotIDSrc; if ( ! cartridge.GetSlotID(slotIDSrc,error) ) { return false; } int slotIDDst; if ( ! drive.GetSlotID(slotIDDst,error) ) { return false; } return detail_->MoveCartridge(slotIDSrc,slotIDDst,cartridge,error); }
void printSmartDashboard() { SmartDashboard::PutNumber("Drive Y-Value", oi->joyDrive->GetRawAxis(FORWARD_Y_AXIS)); //oi->getDashboard()->PutNumber("Drive Y-Value", oi->joyDrive->GetRawAxis(FORWARD_Y_AXIS)); SmartDashboard::PutNumber("Drive X-Value", oi->joyDrive->GetRawAxis(TURN_X_AXIS));//oi->getDashboard()->PutNumber("Drive X-Value", oi->joyDrive->GetRawAxis(TURN_X_AXIS)); SmartDashboard::PutBoolean("Compressor On?", manip->compressorState()); //oi->getDashboard()->PutBoolean("Compressor On?", manip->compressorState()); SmartDashboard::PutNumber("Gyro Value", drive->navX->GetAngle()); //Returns Encoder Position(with tick) //deleted two front motors because encoder is attached only to the back talons SmartDashboard::PutNumber("Left Encoder Position", drive->rearLeftMotor->GetEncPosition()); SmartDashboard::PutNumber("Right Encoder Position", drive->rearRightMotor->GetEncPosition()); //Returns how fast the wheel is spinning //deleted two front motors because encoder is attached only to the back talons SmartDashboard::PutNumber("Left Encoder Speed", drive->rearLeftMotor->GetEncVel()); //gives value for both back and front left encoder SmartDashboard::PutNumber("Right Encoder Speed", drive->rearRightMotor->GetEncVel()); //TODO: Does not return right encoder value SmartDashboard::PutNumber("Average Encoder Value", drive->getAvgEncVal()); //Is working 1/21/2016 }
int _tmain(int argc, _TCHAR* argv[]) { WindowsDriveManager dm; Drive* drive = dm.getDrive("USB", 0); assert(0 == drive->getDriveName().compare("WMI USB")); assert(0 != drive->getDriveName().compare("WMI default")); assert(0 != drive->getDriveName().compare("WMI SATA")); assert(0 != drive->getDriveName().compare("Linux USB")); assert(0 != drive->getDriveName().compare("Linux default")); assert(0 != drive->getDriveName().compare("Linux SATA")); delete drive; drive = dm.getDrive("SAS", 0); assert(NULL == drive); delete drive; return 0; }
bool homeostaticModule::addNewDrive(string driveName) { cout << "adding a default drive..." << endl; Drive* drv = new Drive(driveName); drv->setHomeostasisMin(drv->homeostasisMin); drv->setHomeostasisMax(drv->homeostasisMax); drv->setDecay(drv->decay); drv->setValue((drv->homeostasisMax + drv->homeostasisMin) / 2.); drv->setGradient(false); manager.addDrive(drv); cout << "default drive added. Opening ports..."<<endl; openPorts(driveName); cout << "new drive created successfully!"<<endl; return true; }
bool homeostaticModule::addNewDrive(string driveName, yarp::os::Bottle& grpHomeostatic) { Drive* drv = new Drive(driveName); drv->setHomeostasisMin(grpHomeostatic.check((driveName + "-homeostasisMin"), Value(drv->homeostasisMin)).asDouble()); drv->setHomeostasisMax(grpHomeostatic.check((driveName + "-homeostasisMax"), Value(drv->homeostasisMax)).asDouble()); cout << "H1 " << grpHomeostatic.check((driveName + "-decay"), Value(drv->decay)).asDouble() <<endl; drv->setDecay(grpHomeostatic.check((driveName + "-decay"), Value(drv->decay)).asDouble()); drv->setValue((drv->homeostasisMax + drv->homeostasisMin) / 2.); drv->setGradient(grpHomeostatic.check((driveName + "-gradient"), Value(drv->gradient)).asBool()); cout << "h2 " << drv->decay << endl; //cout << drv->name << " " << drv->homeostasisMin << " " << drv->homeostasisMax << " " << drv->decay << " " <<drv->gradient << endl; //cout << d << endl; //cout << grpHomeostatic.toString()<<endl; manager.addDrive(drv); cout << "h2 " << manager.drives[0]->decay << endl; openPorts(driveName); return true; }
void TeleopPeriodic() { drive->doDrive(input.GetX(), input.GetY(), input.GetRotation()); input.GetGrabberInput(); input.GetArmInput(); }
/** * Periodic code for autonomous mode should go here. * * Use this method for code which will be called periodically at a regular * rate while the robot is in autonomous mode. */ void RobotDemo::AutonomousPeriodic() { shooter->handle(); intake->handle(); switch(curAutoState) { case AUTO_EXTEND: if(curAutoState != preAutoState) { preAutoState = curAutoState; intake->move(INTAKE_EXTEND_POWER); printf("extending, pre %d, cur %d, \n", preAutoState, curAutoState); } if(intake->getCurrentPosition() >= EXTENDER_MAX_DISTANCE) { intake->move(STOP); curAutoState = AUTO_DRIVE; printf("pre %d, cur %d, stopped extender \n", preAutoState, curAutoState); } break; case AUTO_DRIVE: if(curAutoState != preAutoState) { preAutoState = curAutoState; drive->InitAutoDrive(AUTO_DRIVE_DISTANCE_RIGHT, AUTO_DRIVE_DISTANCE_LEFT, AUTO_DRIVE_SPEED); printf("We're moving! pre %d, cur %d, \n", preAutoState, curAutoState); } /* if(HotAutoSensor->Get()) { HotAutoStarts = true; } */ if(drive->autoDrive() == true) { curAutoState = AUTO_STOP_DRIVING; } break; case AUTO_STOP_DRIVING: if(curAutoState != preAutoState) { preAutoState = curAutoState; printf("now stopping pre %d, cur %d, \n", preAutoState, curAutoState); drive->TeleopDrive(AUTO_STOPPING_SPEED_LEFT, AUTO_STOPPING_SPEED_RIGHT); } if(drive->getSpeed() > 0.01) { drive->TeleopDrive(AUTO_STOPPED_SPEED_LEFT, AUTO_STOPPED_SPEED_RIGHT); // if(HotAutoStarts || HotAutoTimer->Get() > 5.0) // { curAutoState = AUTO_SHOOT; // } } break; case AUTO_SHOOT: if(curAutoState != preAutoState) { preAutoState = curAutoState; printf("now shooting pre %d, cur %d, \n", preAutoState, curAutoState); shooter->initShot(SHOT_HIGH_ANGLE, SHOT_HIGH_SPEED); } if(!shooter->isShooting()) { curAutoState = AUTO_STOP; } break; /* case AUTO_MOBILITY: if(curAutoState != preAutoState) { drive->InitAutoDrive(AUTO_SETUP_SHOT_DISTANCE_RIGHT, AUTO_SETUP_SHOT_DISTANCE_LEFT, AUTO_SETUP_SHOT_SPEED); } if(drive->autoDrive()) { curAutoState = AUTO_STOP; } break; */ case AUTO_STOP: preAutoState = curAutoState; printf("auto is done pre %d, cur %d, \n", preAutoState, curAutoState); break; // HotAutoTimer->Stop(); default: break; } }
/** * Periodic code for teleop mode should go here. * * Use this method for code which will be called periodically at a regular * rate while the robot is in teleop mode. */ void RobotDemo::TeleopPeriodic() { drive->TeleopDrive(driverPad->GetLeftY() *DRIVER_LEFT_DIRECTION, driverPad->GetRightY() *DRIVER_RIGHT_DIRECTION); shooter->handle(); shooter->debug(); //shooter->setShooterPower(operatorPad->GetRightY()); if((intakeMode == IN_INTAKE_MANUAL_MODE) && operatorPad->GetNumberedButton(INTAKE_AUTO_MODE_BUTTON)) { intakeMode = IN_INTAKE_AUTO_MODE; printf ("set into auto roller mode"); intake->setExtenderMode(IN_INTAKE_AUTO_MODE); } //sets intake mode to auto if previously in manual and operator presses auto button else if((intakeMode == IN_INTAKE_AUTO_MODE) && operatorPad->GetNumberedButton(INTAKE_MANUAL_MODE_BUTTON)) { intakeMode = IN_INTAKE_MANUAL_MODE; printf ("set into manual roller mode"); intake->setExtenderMode(IN_INTAKE_MANUAL_MODE); } intake->handle(); if(intakeMode == IN_INTAKE_MANUAL_MODE) { intake->move(operatorPad->GetLeftY()*OPERATOR_LEFT_DIRECTION); } else { //Extender DPad control curPadDir = operatorPad->GetDPad(); if(lastPadDir != curPadDir) { switch(curPadDir) { case Gamepad::kUp: intake->setExtenderTarget(FULL_EXTEND_DISTANCE); break; case Gamepad::kDown: intake->setExtenderTarget(FULL_RETRACT_DISTANCE); break; default: break; } } lastPadDir = curPadDir; } //intake->spinRoller(operatorPad->GetRightY()*OPERATOR_RIGHT_DIRECTION); intake->getCurrentPosition(); //if operator taps in button roller continually spins inward if(operatorPad->GetNumberedButton(ROLLER_IN_BUTTON)) { intakeCont = INTAKE_ROLLER_IN_STATE; } //if operator taps stop button roller stops continually else if(operatorPad->GetNumberedButton(ROLLER_OFF_BUTTON)) { intakeCont = INTAKE_ROLLER_STOP_STATE; } //if operator holds spit button roller spins backwards no matter what if(operatorPad->GetNumberedButton(ROLLER_OUT_BUTTON)) { intake->spinRoller(INTAKE_ROLLER_SPIT_POWER); } //not holding spit button else { //roller either spins or is stopped depending on previous code if(intakeCont) { intake->spinRoller(INTAKE_ROLLER_IN_POWER); } else { intake->spinRoller(INTAKE_ROLLER_STOP_POWER); } } if(operatorPad->GetNumberedButton(RIGHT_TRIGGER_BUTTON)) // low shot = Right Trigger { shooter->initShot(SHOT_LOW_ANGLE, SHOT_LOW_SPEED); printf("test button 1 is pressed"); } else if(operatorPad->GetNumberedButton(LEFT_TRIGGER_BUTTON)) // pass = Left Trigger { shooter->initShot(SHOT_PASS_ANGLE, SHOT_PASS_SPEED); printf("test button 2 is pressed"); } else if(operatorPad->GetNumberedButton(RIGHT_BUMPER_BUTTON)) // high shot = Right Bumper { shooter->initShot(SHOT_HIGH_ANGLE, SHOT_HIGH_SPEED); printf("test button 3 is pressed"); } else if(operatorPad->GetNumberedButton(LEFT_BUMPER_BUTTON)) // truss = Left Bumper { shooter->initShot(SHOT_TRUSS_ANGLE, SHOT_TRUSS_SPEED); printf("test button 4 is pressed"); } //printf("right Joystick distance: %f left Joystick distance: %f \n", driverPad->GetRightY(), driverPad->GetLeftY()); }
int main(int argc, char* argv[]){ Drive d; d.show(); return 0; }
void Test() { SmartDashboard::init(); Joystick* driverStick = driverInput->GetDriverJoystick(); //Relay* redLights = new Relay(1, Relay::kForward); //Relay* blueLights = new Relay(2, Relay::kForward); //Relay* cameraLights = new Relay(CAMERA_LED); //Relay* redLights = new Relay(1, Relay::kForward); //Solenoid *rightSolenoid; //Solenoid *leftSolenoid; bool gear = false; bool prevShiftButttonPressed = false; Victor* motors [] = {feeder->feederWheel, feeder->feederArm, drive->leftDrive, drive->rightDrive, catapult->ChooChooMotor}; Relay* compressor = new Relay(COMPRESSOR_RELAY); DigitalInput* compressorLimitSwitch = new DigitalInput(COMPRESSOR_SWITCH); //Compressor* compressor = new Compressor(1,1);//COMPRESSOR_SWITCH, COMPRESSOR_RELAY); //compressor->Start(); Relay* cameraLights = new Relay(CAMERA_LED); //Solenoid* right = new Solenoid(6); //Solenoid* left = new Solenoid(5); //right->Set(true); //left->Set(true); //Relay* redlights = catapult->redled; //Relay* whitelights = catapult->whiteled; //redlights->Set(redlights->kForward); //whitelights->Set(whitelights->kForward); //cameraLights->Set(cameraLights->kForward); bool compressorOn = false; while (IsTest() && IsEnabled()) { bool curShiftButtonPressed = driverStick->GetRawButton(10); //sensors->GetInputs(); //SmartDashboard::PutNumber("Rangefinder", sensors->GetDistance()); /*FEEDER WHEEL*/ if (driverStick->GetRawButton(2)) { motors[0]->Set(0.4); } else { motors[0]->Set(0); } /*FEEDER ARM*/ if (driverStick->GetRawButton(3)) { motors[1]->Set(-0.6); } else { motors[1]->Set(0); } /*LEFT DRIVE*/ if (driverStick->GetRawButton(4)) { motors[2]->Set(0.8); } else { motors[2]->Set(0); } /*RIGHT DRIVE*/ if (driverStick->GetRawButton(5)) { motors[3]->Set(0.8); } else { motors[3]->Set(0); } /* if (driverStick->GetRawButton(6)) { redlights->Set(redlights->kForward); } else { redlights->Set(redlights->kOff); } */ /*CHOOCHOO MOTOR*/ if (driverStick->GetRawButton(7) && driverStick->GetRawButton(1)) { motors[4]->Set(0.6); } else { motors[4]->Set(0); } /*CAMERA LED*/ if (driverStick->GetRawButton(8)) { cameraLights->Set(cameraLights->kForward); } else { cameraLights->Set(cameraLights->kOff); } /* if (curShiftButtonPressed && !prevShiftButttonPressed) { drive->SetHighGear(!gear); } */ if (driverStick->GetRawButton(10)) { drive->SetHighGear(true);//!gear gear = !gear; } else{ drive->SetHighGear(false); } /*COMPRESSOR*/ if(driverStick->GetRawButton(11)) { compressorOn = true; //compressor->Set(compressor->kOn); //compressor->Start(); } else { //compressor->Stop(); } if(driverStick->GetRawButton(9)) { compressorOn = false; //compressor->Set(compressor->kOff); //compressor->Stop(); } if(compressorOn){ if(!compressorLimitSwitch->Get()){ compressor->Set(compressor->kOn); } else{ compressor->Set(compressor->kOff); compressorOn = false; } } else compressor->Set(compressor->kOff); prevShiftButttonPressed = curShiftButtonPressed; } }
/** Tests that the motors are wired up correctly. When running, verify that the motor and direction printed over serial is what is observed on the robot. Also prints the change in encoder values after each movement - expect the readings to positive, and correspond with the motor that moved */ void tests::motor_wiring() { Drive md; md.resetEncoders(); if(md.faulted()) { Serial.println("Motor driver has a fault"); md.clearFault(); if(md.faulted()) { Serial.println("Motor driver still has a fault"); return; } } else Serial.println("Motors are ok"); { Serial.print("Motor R, forward: "); uint32_t encr = md.getMREncoder(); uint32_t encl = md.getMLEncoder(); Serial.flush(); md.setMRSpeed(100); delay(2e3); md.setMRSpeed(0); int32_t diffr = md.getMREncoder() - encr; int32_t diffl = md.getMLEncoder() - encl; Serial.print("dr ="); Serial.print(diffr); Serial.print(", dl ="); Serial.println(diffl); } if(md.faulted()) Serial.println("Motor driver has a fault"); { Serial.print("Motor L, forward: "); uint32_t encr = md.getMREncoder(); uint32_t encl = md.getMLEncoder(); Serial.flush(); md.setMLSpeed(100); delay(2e3); md.setMLSpeed(0); int32_t diffr = md.getMREncoder() - encr; int32_t diffl = md.getMLEncoder() - encl; Serial.print("dr ="); Serial.print(diffr); Serial.print(", dl ="); Serial.println(diffl); } if(md.faulted()) Serial.println("Motor driver has a fault"); }
void TeleopInit() { init(); drive->setShiftMode(Drive::Manual); shooterMotor->Set(0.0); loaderMotor->Set(Relay::kOff); }
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(); } }