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