// Called repeatedly when this Command is scheduled to run void RPMHoldingCommand::Execute() { double countsPerRevolution = 360; double currentTime = timer->Get(); double timeDifference = currentTime - lastTime; lastTime = currentTime; Encoder* motorEncoder = prototypingsubsystem->GetMotorEncoder(); int currentCount = motorEncoder->Get(); int countDifference = currentCount - lastCount; double rawRPM = countDifference * (60.0 / countsPerRevolution) / timeDifference; double rpm = GetRPM(rawRPM); printf("rawRPM %f rpm %f\n", rawRPM, rpm); rpmSource->inputRPM(rpm); //rpmSource->inputRPM(rpm); printf("setpoint %f RPM %f result %f error %f \n", controller->GetSetpoint(), rpm, controller->Get(), controller->GetError()); SmartDashboard *sd = oi->getSmartDashboard(); oi->DisplayEncoder(motorEncoder); oi->DisplayPIDController(controller); sd->PutDouble("Count Difference", countDifference); sd->PutDouble("Current count", currentCount); sd->PutDouble("Time difference", timeDifference); sd->PutDouble("Calculated RPM", rpm); sd->PutDouble("Motor speed", prototypingsubsystem->GetMotor()->Get()); sd->PutDouble("Counts per revolution", countsPerRevolution); lastCount = currentCount; }
bool Shooter::ShooterUpToSpeed() { if(abs(GetRPM() - desiredRPM) < 5){ return true; } else { return false; } }
btScalar CARENGINE::Integrate(btScalar clutch_drag, btScalar clutch_angvel, btScalar dt) { clutch_torque = clutch_drag; btScalar torque_limit = shaft.getMomentum(clutch_angvel) / dt; if ((clutch_torque > 0 && clutch_torque > torque_limit) || (clutch_torque < 0 && clutch_torque < torque_limit)) { clutch_torque = torque_limit; } stalled = (GetRPM() < info.stall_rpm); //make sure the throttle is at least idling if (throttle_position < info.idle) throttle_position = info.idle; //engine drive torque btScalar friction_factor = 1.0; //used to make sure we allow friction to work if we're out of gas or above the rev limit btScalar rev_limit = info.rpm_limit; if (rev_limit_exceeded) rev_limit -= 100.0; //tweakable if (GetRPM() < rev_limit) rev_limit_exceeded = false; else rev_limit_exceeded = true; combustion_torque = info.GetTorque(throttle_position, GetRPM()); if (out_of_gas || rev_limit_exceeded || stalled) { friction_factor = 0.0; combustion_torque = 0.0; } friction_torque = info.GetFrictionTorque(shaft.ang_velocity, friction_factor, throttle_position); if (stalled) { //try to model the static friction of the engine friction_torque *= 100.0; } btScalar total_torque = combustion_torque + friction_torque + clutch_torque; shaft.applyMomentum(total_torque * dt); return clutch_torque; }
/************ * Integrate * ************/ void pEngine::Integrate() // Step the engine. // Also looks at the stall state (you can kickstart the engine when driving). { float rpm=GetRPM(); //qdbg("pEngine:Integrate()\n"); //qdbg(" rotV=%.2f, rotA=%.2f\n",rotV,rotA); // This is normally done by RDriveLineComp, but avoid the function call rotV+=rotA*RR_TIMESTEP; // Deduce state of engine (stalling) if(IsStalled()) { // Turning back on? if(rpm>=startRPM) { //qdbg("UNStalling engine!\n"); DisableStall(); } } else { // Stalling? if(rpm<stallRPM) { EnableStall(); // Trigger sample? } } int autoClutch = 1; // Auto-clutch assist if(autoClutch) { if(rpm<autoClutchRPM) { // Engage clutch (is picked up by the driveline) float autoClutch=(rpm-idleRPM)/(autoClutchRPM-idleRPM); //qdbg("Autoclutch %f\n",autoClutch); car->GetDriveLine()->EnableAutoClutch(); //flags|=AUTOCLUTCH_ACTIVE; if(autoClutch<0)autoClutch=0; else if(autoClutch>1)autoClutch=1; car->GetDriveLine()->SetClutchApplication(autoClutch); } else { // Turn off auto-clutch car->GetDriveLine()->DisableAutoClutch(); } } //qdbg(" post rotV=%.2f\n",rotV); }
void CARENGINE::DebugPrint(std::ostream & out) const { out << "---Engine---" << "\n"; out << "Throttle position: " << throttle_position << "\n"; out << "Combustion torque: " << combustion_torque << "\n"; out << "Clutch torque: " << -clutch_torque << "\n"; out << "Friction torque: " << friction_torque << "\n"; out << "Total torque: " << GetTorque() << "\n"; out << "RPM: " << GetRPM() << "\n"; out << "Rev limit exceeded: " << rev_limit_exceeded << "\n"; out << "Running: " << !stalled << "\n"; }
btScalar CarEngine::Integrate(btScalar clutch_drag, btScalar clutch_angvel, btScalar dt) { btScalar rpm = GetRPM(); clutch_torque = clutch_drag; btScalar torque_limit = shaft.getMomentum(clutch_angvel) / dt; if ((clutch_torque > 0 && clutch_torque > torque_limit) || (clutch_torque < 0 && clutch_torque < torque_limit)) { clutch_torque = torque_limit; } stalled = (rpm < info.stall_rpm); //make sure the throttle is at least idling btScalar idle_position = info.idle_throttle + info.idle_throttle_slope * (rpm - info.start_rpm); if (throttle_position < idle_position) throttle_position = idle_position; //engine drive torque btScalar rev_limit = info.rpm_limit; if (rev_limit_exceeded) rev_limit -= 100.0; //tweakable rev_limit_exceeded = rpm > rev_limit; combustion_torque = info.GetTorque(throttle_position, rpm); //nitrous injection if (nos_mass > 0 && nos_boost_factor > 0) { btScalar boost = nos_boost_factor * info.nos_boost; combustion_torque += boost / shaft.ang_velocity; btScalar fuel_consumed = boost * info.fuel_rate * dt; btScalar nos_consumed = info.nos_fuel_ratio * fuel_consumed; nos_mass = btMax(btScalar(0), nos_mass - nos_consumed); } if (out_of_gas || rev_limit_exceeded || stalled) combustion_torque = 0.0; friction_torque = info.GetFrictionTorque(throttle_position, rpm); //try to model the static friction of the engine if (stalled) friction_torque *= 2; btScalar total_torque = combustion_torque + friction_torque + clutch_torque; shaft.applyMomentum(total_torque * dt); return clutch_torque; }
void Shooter::Idle(){ if (desiredRPM > GetRPM()) shootJag->Set(1.0); else shootJag->Set(0.0); // SmartDashboard::PutNumber("desiredRPMInIdle", desiredRPM); // SmartDashboard::PutNumber("GetRPMInIdle", GetRPM()); // SmartDashboard::PutNumber("ShootJagSpeed", shootJag->Get()); double shootTime = shootTimer->Get(); if(shootTime > 0.5){ //The timing for the servo feeding frisbees into the shooter shootServo->Set(servoPull); shootTimer->Reset(); shootTimer->Stop(); doneShooting = true; } }
void testInit(void) { TEST_ASSERT_EQUAL(DEFAULT_FREQ, GetFrequency()); TEST_ASSERT_EQUAL(DEFAULT_RPM, GetRPM()); TEST_ASSERT_EQUAL(50, GetDuty()); }
/************** * Calc Forces * **************/ void pEngine::CalcForces() { float minTorque,maxTorque; float rpm=GetRPM(); static int starterDelay; // Temp crap to avoid multiple starter smps #ifdef LTRACE qdbg("pEngine::CalcForces()\n"); qdbg(" rpm=%f, throttle=%.2f\n",rpm,throttle); #endif if(starterDelay>0) starterDelay--; // Check starter; this section assumes CalcForces() is called // only once per simulation step. Although this may not be // the case in the future, it should not harm that much. if(IsStalled()) { // There's only some engine braking // Note this skips calculating min/maxTorque tEngine=GetMinTorque(rpm); //qdbg("Stalled; check starter=%d\n",RMGR->controls->control[RControls::T_STARTER]->value); bool starter = true; if(starter) { // Use the starter tEngine+=starterTorque; // Raise the starter sample volume if(starterDelay==0) { starterDelay=1000/car->_lastDT; } #ifdef LTRACE qdbg(" starting; T_starter=%f, tEngine=%f\n",starterTorque,tEngine); #endif } #ifdef LTRACE qdbg("Stalled engine torque: %.3f\n",tEngine); #endif } else { // Engine is running // Calculate minimal torque (if 0% throttle) minTorque=GetMinTorque(rpm); // Calculate maximum torque (100% throttle situation) maxTorque=GetMaxTorque(rpm); // The throttle accounts for how much of the torque is actually // produced. // NOTE: The output power then is 2*PI*rps*outputTorque // (rps=rpm/60; rotations per second). Nothing but a statistic now. tEngine=(maxTorque-minTorque)*throttle+minTorque; #ifdef LTRACE qdbg("minTorque=%f, maxTorque=%.f, throttle=%f => torque=%f\n",minTorque,maxTorque,throttle,tEngine); #endif } }