Пример #1
0
// 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;

}
Пример #2
0
bool Shooter::ShooterUpToSpeed() {
	if(abs(GetRPM() - desiredRPM) < 5){
		return true;
	}
	else {
		return false;
	}
}
Пример #3
0
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;
}
Пример #4
0
/************
* 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);
}
Пример #5
0
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";
}
Пример #6
0
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;
}
Пример #7
0
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;
	}
}
Пример #8
0
void testInit(void)
{
	TEST_ASSERT_EQUAL(DEFAULT_FREQ, GetFrequency());
	TEST_ASSERT_EQUAL(DEFAULT_RPM, GetRPM());
	TEST_ASSERT_EQUAL(50, GetDuty());
}
Пример #9
0
/**************
* 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
	

  }
}