void CustomVehicleControllerComponentEngine::Update (dFloat timestep)
{
CustomVehicleControllerBodyStateChassis& chassis = m_controller->m_chassisState;
CustomVehicleControllerComponentEngine::dGearBox* const gearBox = GetGearBox();
gearBox->Update (timestep);
int gear = gearBox->GetGear();
#ifdef __TEST_VEHICLE_XXX__
if (gear > (CustomVehicleControllerComponentEngine::dGearBox::m_firstGear))
{
gearBox->SetGear (CustomVehicleControllerComponentEngine::dGearBox::m_firstGear);
// gearBox->SetGear (CustomVehicleControllerComponentEngine::dGearBox::m_reverseGear);
gear = gearBox->GetGear();
}
#endif
if (m_engineSwitch) {
if (gear == CustomVehicleControllerComponentEngine::dGearBox::m_newtralGear) {
dFloat param = dMax (GetParam(), 0.05f);
m_engineToque = GetTorque (m_engineRPS) * param - m_engineIdleResistance1 * m_engineRPS - m_engineIdleResistance2 * m_engineRPS * m_engineRPS;
dFloat alpha = m_engineIdleInvInertia * m_engineToque;
m_engineRPS = dMin (dMax (m_engineRPS + alpha * timestep, 0.0f), m_radiansPerSecundsAtRedLine);
} else {
dFloat gearRatio = gearBox->GetGearRatio(gear);
dFloat shaftOmega = m_differencial->GetShaftOmega();
m_engineRPS = shaftOmega * gearRatio;
dFloat torqueResistance = - m_engineInternalFriction * m_engineRPS;
dFloat param = GetParam();
dFloat nominalTorque = -GetTorque (dMax (-m_engineRPS, 0.0f)) * param;
dFloat engineTorque = nominalTorque + torqueResistance;
m_engineToque = m_engineToque + (engineTorque - m_engineToque) * timestep / m_clutchTorqueCouplingTime;
dFloat shaftTorque = m_engineToque * gearRatio;
m_differencial->ApplyTireTorque(shaftTorque, shaftOmega);
}
} else {
m_engineRPS = m_engineRPS * 0.95f;
if (m_engineRPS < 0.001f) {
m_engineRPS = 0.0f;
}
m_engineToque = 0.0f;
}
dVector front (chassis.m_matrix.RotateVector(chassis.m_localFrame[0]));
m_speedMPS = chassis.m_veloc % front;
}
/*****************************************************************************
* Function - RunPeriodFlowCalculationTasks
* DESCRIPTION:
*
*****************************************************************************/
void AdvancedFlowCtrl::RunPeriodFlowCalculationTasks()
{
float pressure_in_pit = GetPressureInPit();
float pressure_in_outlet = GetPressureInOutlet();
// run pit flow calculation
mpPitFlow->RunPitFlow(pressure_in_pit);
AFC_RUN_PAR_UPDATE_STRUCT run_par_update_arguments;
run_par_update_arguments.pressureIn = pressure_in_pit;
run_par_update_arguments.pressureOut = pressure_in_outlet;
for (int i = FIRST_PUMP; i <= LAST_PUMP; i++)
{
run_par_update_arguments.speed = GetSpeed(i);
run_par_update_arguments.torque = GetTorque(i);
run_par_update_arguments.pumpState = mPumpState[i];
run_par_update_arguments.pumpRampUpPassed = (mAwaitPumpRampUpTime[i] <= 0);
run_par_update_arguments.trainingState = mTrainingEnabledState[i];
// run learn and parameter update
UpdateFlowLearnControl((PUMP_TYPE) i);
UpdateTrainingFrequency((PUMP_TYPE) i);
mpParameterIdentifiers[i]->RunParUpdate(&run_par_update_arguments);
}
AFC_RUN_SYSTEM_FLOW_STRUCT run_system_flow_arguments;
run_system_flow_arguments.pitArea = GetPitArea();
run_system_flow_arguments.pressureIn = pressure_in_pit;
run_system_flow_arguments.pressureOut = pressure_in_outlet;
for (int i = FIRST_PUMP; i <= LAST_PUMP; i++)
{
run_system_flow_arguments.pump[i].speed = GetSpeed(i);
run_system_flow_arguments.pump[i].torque = GetTorque(i);
run_system_flow_arguments.pump[i].pumpState = mPumpState[i];
run_system_flow_arguments.pump[i].pumpRampUpPassed = (mAwaitPumpRampUpTime[i] <= 0);
t_calcvar* theta_Q = mpParameterIdentifiers[i]->GetPumpFlowParameters();
for (int j = 0; j < FLOW_DIM; j++)
{
run_system_flow_arguments.pump[i].theta[j] = theta_Q[j];
}
}
// run system flow calculation
mpSystemFlow->RunSystemFlow(&run_system_flow_arguments);
}
void M3LoadX1::StepStatus()
{
if (IsStateError())
return;
M3LoadX1EcStatus * ecs = (M3LoadX1EcStatus * )(ecc->GetStatus());
tq_sense.Step(ecs->adc_torque());
status.set_torque(tq_sense.GetTorque_mNm());
status.set_torquedot(torquedot_df.Step(GetTorque()));
}
void Ship::GetClientUpdateMessage(ClientShipUpdate& message) const
{
message.fired = false; //TODO: implement
strcpy(message.name, GetName().c_str());
Vector3f vec = GetForce();
message.force[0] = vec[0];
message.force[1] = vec[1];
message.force[2] = vec[2];
message.torque = GetTorque();
}
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";
}
void CARENGINEINFO::SetTorqueCurve(
const btScalar redline,
const std::vector<std::pair<btScalar, btScalar> > & torque)
{
torque_curve.Clear();
//this value accounts for the fact that the torque curves are usually measured
// on a dyno, but we're interested in the actual crankshaft power
const btScalar dyno_correction_factor = 1.0;//1.14;
assert(torque.size() > 1);
//ensure we have a smooth curve down to 0 RPM
if (torque[0].first != 0) torque_curve.AddPoint(0,0);
for (std::vector<std::pair<btScalar, btScalar> >::const_iterator i = torque.begin(); i != torque.end(); ++i)
{
torque_curve.AddPoint(i->first, i->second * dyno_correction_factor);
}
//ensure we have a smooth curve for over-revs
torque_curve.AddPoint(torque[torque.size()-1].first + 10000, 0);
//write out a debug torque curve file
/*std::ofstream f("out.dat");
for (btScalar i = 0; i < curve[curve.size()-1].first+1000; i+= 20) f << i << " " << torque_curve.Interpolate(i) << std::endl;*/
//for (unsigned int i = 0; i < curve.size(); i++) f << curve[i].first << " " << curve[i].second << std::endl;
//calculate engine friction
btScalar max_power_angvel = redline*3.14153/30.0;
btScalar max_power = torque_curve.Interpolate(redline) * max_power_angvel;
friction = max_power / (max_power_angvel*max_power_angvel*max_power_angvel);
//calculate idle throttle position
for (idle = 0; idle < 1.0; idle += 0.01)
{
if (GetTorque(idle, start_rpm) > -GetFrictionTorque(start_rpm*3.141593/30.0, 1.0, idle))
{
//std::cout << "Found idle throttle: " << idle << ", " << GetTorqueCurve(idle, start_rpm) << ", " << friction_torque << std::endl;
break;
}
}
}
void M3Joint::StepCommand()
{
if (!act || IsStateSafeOp())
return;
if (IsVersion(IQ) && !ctrl_simple)
return;
if(IsStateError())
{
if (IsVersion(IQ))
ctrl_simple->SetDesiredControlMode(CTRL_MODE_OFF);
else
act->SetDesiredControlMode(ACTUATOR_MODE_OFF);
return;
}
/*tmp_cnt++;
if (tmp_cnt == 100)
{
M3_DEBUG("th_in: %f\n", command.q_desired());
}*/
if (command.ctrl_mode() == JOINT_MODE_THETA && command.smoothing_mode() == SMOOTHING_MODE_MIN_JERK)
{
command.set_ctrl_mode(JOINT_MODE_THETA_MJ);
}
if (command.ctrl_mode() == JOINT_MODE_THETA_GC && command.smoothing_mode() == SMOOTHING_MODE_MIN_JERK)
{
command.set_ctrl_mode(JOINT_MODE_THETA_GC_MJ);
}
//Avoid pops
if (command.ctrl_mode()!=mode_last)
{
q_on_slew.Reset(0.0);
tq_on_slew.Reset(0.0);
pwm_on_slew.Reset(0.0);
q_slew.Reset(GetThetaDeg());
jerk_joint.Startup(GetTimestamp(), GetThetaDeg());
if (IsVersion(IQ))
tq_switch=GetTorque()*1000; // TODO: Make GetTorque nNm again
else
tq_switch=GetTorque();
q_switch=GetThetaDeg();
pwm_switch=GetPwmCmd();
if (IsVersion(IQ)) {
ctrl_simple->ResetIntegrators();
}
}
if (IsVersion(IQ)) {
if (!act->IsMotorPowerSlewedOn())
ctrl_simple->ResetIntegrators();
}
if (IsVersion(IQ)) {
ctrl_simple->SetDesiredControlMode(CTRL_MODE_OFF);
switch(command.ctrl_mode())
{
case JOINT_MODE_THETA:
case JOINT_MODE_THETA_MJ:
{
if (!IsEncoderCalibrated())
{
// M3_INFO("Not calib %s\n",GetName().c_str());
break;
}
CalcThetaDesiredSmooth();
mReal des=trans->GetThetaDesActuatorDeg();
StepBrake(des,trans->GetThetaActuatorDeg());
//Do PID in actuator space so result is direct PWM
//ctrl_simple->SetDesiredControlMode(CTRL_MODE_THETA);
ctrl_simple->SetDesiredControlMode(CTRL_MODE_THETA);
ctrl_simple->SetDesiredTheta(DEG2RAD(des));
break;
}
case JOINT_MODE_THETA_GC:
case JOINT_MODE_THETA_GC_MJ:
case JOINT_MODE_POSE:
{
if (!IsEncoderCalibrated())
break;
mReal stiffness,gravity,tq_des;
CalcThetaDesiredSmooth();
mReal des=trans->GetThetaDesJointDeg();
StepBrake(des,trans->GetThetaJointDeg());
//Do PID in joint space
if (command.ctrl_mode() == JOINT_MODE_POSE)
{
tq_des =pid_theta_gc.Step(trans->GetThetaJointDeg(),
trans->GetThetaDotJointDeg(),
des,
0.0,
0.0,
param.kq_d_pose(),
0.0,
0.0);
} else {
tq_des =pid_theta_gc.Step(trans->GetThetaJointDeg(),
trans->GetThetaDotJointDeg(),
des,
param.kq_p(),
param.kq_i(),
param.kq_d(),
param.kq_i_limit(),
param.kq_i_range());
}
/*if (pnt_cnt%200==0) {
M3_DEBUG("actuator: %s\n", GetName().c_str());
M3_DEBUG("tq_des: %f\n",tq_des);
M3_DEBUG("th: %f -> %f thdot: %f\n",trans->GetThetaJointDeg(),des,trans->GetThetaDotJointDeg());
M3_DEBUG("des: %f\n\n",des);
}*/
stiffness=CLAMP(command.q_stiffness(),0.0,1.0);
gravity=GetTorqueGravity()*param.kq_g();
//Ramp in from torque at switch-over point
mReal tq_on, tq_out;
tq_on=tq_on_slew.Step(1.0,1.0/MODE_TQ_ON_SLEW_TIME);
//tq_on = 1.0;
tq_out=tq_on*(stiffness*tq_des-gravity)+(1.0-tq_on)*tq_switch;
//tq_out = tq_switch;
//tq_out = stiffness*tq_des-gravity;
//Send out
trans->SetTorqueDesJoint(tq_out/1000.0); //TODO: convert back to mNm
ctrl_simple->SetDesiredControlMode(CTRL_MODE_TORQUE);
ctrl_simple->SetDesiredTorque(trans->GetTorqueDesActuator());
break;
}
case JOINT_MODE_TORQUE_GC:
{
if (!IsEncoderCalibrated())
break;
mReal tq_on, tq_out,gravity;
mReal tq_des=command.tq_desired();
StepBrake(tq_des,trans->GetTorqueJoint());
gravity=GetTorqueGravity()*param.kq_g();
//Ramp in from torque at switch-over point
tq_on=tq_on_slew.Step(1.0,1.0/MODE_TQ_ON_SLEW_TIME);
tq_out=tq_on*(tq_des-gravity)+(1.0-tq_on)*tq_switch;
//Send out
/*if (pnt_cnt%200==0) {
M3_DEBUG("actuator: %s tq_gravity: %f\n", GetName().c_str(),gravity);
M3_DEBUG("tq_des: %f tqout_des: %f\n",tq_des,tq_out);
}*/
trans->SetTorqueDesJoint(tq_out/1000.0);
ctrl_simple->SetDesiredControlMode(CTRL_MODE_TORQUE);
ctrl_simple->SetDesiredTorque(trans->GetTorqueDesActuator());
break;
}
case JOINT_MODE_THETADOT_GC:
{
if (!IsEncoderCalibrated())
break;
mReal stiffness,gravity,tq_des;
//CalcThetaDesiredSmooth();
trans->SetThetaDotDesJointDeg(command.qdot_desired());
mReal des=trans->GetThetaDotDesJointDeg();
//StepBrake(des,trans->GetThetaDotJointDeg());
tq_des =pid_thetadot_gc.Step(trans->GetThetaDotJointDeg(),
trans->GetThetaDotDotJointDeg(),
des,
param.kqdot_p(),
param.kqdot_i(),
param.kqdot_d(),
param.kqdot_i_limit(),
param.kqdot_i_range());
StepBrake(tq_des,trans->GetTorqueJoint());
stiffness=CLAMP(command.q_stiffness(),0.0,1.0);
gravity=GetTorqueGravity()*param.kq_g();
//Ramp in from torque at switch-over point
mReal tq_on, tq_out;
tq_on=tq_on_slew.Step(1.0,1.0/MODE_TQ_ON_SLEW_TIME);
tq_out=tq_on*(stiffness*tq_des-gravity)+(1.0-tq_on)*tq_switch;
//tq_out = tq_switch;
//tq_out = stiffness*tq_des-gravity;
//Send out
/*if (pnt_cnt%200==0) {
M3_DEBUG("actuator: %s\n", GetName().c_str());
M3_DEBUG("tq_des: %f tqout_des: %f v: %f\n",tq_des,tq_out,trans->GetThetaDotJointDeg());
M3_DEBUG("des: %f Tjoint: %f desac: %f\n\n",des,trans->GetTorqueJoint(),trans->GetTorqueDesActuator());
}*/
trans->SetTorqueDesJoint(tq_out/1000.0); //TODO: convert back to mNm
ctrl_simple->SetDesiredControlMode(CTRL_MODE_TORQUE);
ctrl_simple->SetDesiredTorque(trans->GetTorqueDesActuator());
break;
}
case JOINT_MODE_THETADOT:
{
if (!IsEncoderCalibrated())
break;
mReal stiffness,gravity,tq_des;
//CalcThetaDesiredSmooth();
trans->SetThetaDotDesJointDeg(command.qdot_desired());
mReal des=trans->GetThetaDotDesJointDeg();
//StepBrake(des,trans->GetThetaDotJointDeg());
tq_des =pid_thetadot_gc.Step(trans->GetThetaDotJointDeg(),
trans->GetThetaDotDotJointDeg(),
des,
param.kqdot_p(),
param.kqdot_i(),
param.kqdot_d(),
param.kqdot_i_limit(),
param.kqdot_i_range());
StepBrake(tq_des,trans->GetTorqueJoint());
stiffness=CLAMP(command.q_stiffness(),0.0,1.0);
//Ramp in from torque at switch-over point
mReal tq_on, tq_out;
tq_on=tq_on_slew.Step(1.0,1.0/MODE_TQ_ON_SLEW_TIME);
tq_out=tq_on*(stiffness*tq_des)+(1.0-tq_on)*tq_switch;
//Send out
/*if (pnt_cnt%200==0) {
M3_DEBUG("actuator: %s\n", GetName().c_str());
M3_DEBUG("tq_des: %f tqout_des: %f v: %f\n",tq_des,tq_out,trans->GetThetaDotJointDeg());
M3_DEBUG("des: %f Tjoint: %f desac: %f\n\n",des,trans->GetTorqueJoint(),trans->GetTorqueDesActuator());
}*/
trans->SetTorqueDesJoint(tq_out/1000.0); //TODO: convert back to mNm
ctrl_simple->SetDesiredControlMode(CTRL_MODE_TORQUE);
ctrl_simple->SetDesiredTorque(trans->GetTorqueDesActuator());
break;
}
case JOINT_MODE_TORQUE:
{
mReal tq_on,tq_out;
//Ramp in from torque at switch-over point
mReal tq_des=command.tq_desired();
StepBrake(tq_des,trans->GetTorqueJoint());
tq_on=tq_on_slew.Step(1.0,1.0/MODE_TQ_ON_SLEW_TIME);
tq_out=tq_on*tq_des+(1.0-tq_on)*tq_switch;
//Send out
trans->SetTorqueDesJoint(tq_out/1000.0);
ctrl_simple->SetDesiredControlMode(CTRL_MODE_TORQUE);
ctrl_simple->SetDesiredTorque(trans->GetTorqueDesActuator());
break;
}
case JOINT_MODE_TORQUE_GRAV_MODEL:
{
if (!IsEncoderCalibrated())
break;
mReal tq_des;
//Do PID in torque grav model space
tq_des =pid_tq_grav_model.Step(GetTorqueGravity(),
trans->GetThetaDotJointDeg(),
-command.tq_desired(),
param.kq_p_tq_gm(),
param.kq_i_tq_gm(),
param.kq_d_tq_gm(),
param.kq_i_limit_tq_gm(),
param.kq_i_range_tq_gm());
//Ramp in from torque at switch-over point
mReal tq_on, tq_out;
tq_on=tq_on_slew.Step(1.0,1.0/MODE_TQ_ON_SLEW_TIME);
//tq_on = 1.0;
tq_out=tq_on*(tq_des)+(1.0-tq_on)*tq_switch;
//Send out
trans->SetTorqueDesJoint(tq_out/1000.0); //TODO: convert back to mNm
ctrl_simple->SetDesiredControlMode(CTRL_MODE_CURRENT);
ctrl_simple->SetDesiredTorque(trans->GetTorqueDesActuator());
/*if (tmp_cnt++ == 1000)
{
M3_DEBUG("------------------------\n");
M3_DEBUG("%s\n", GetName().c_str());
M3_DEBUG("tq_gc: %f\n", GetTorqueGravity());
//M3_DEBUG("tq_dot: %f\n", -trans->GetTorqueDotJoint());
M3_DEBUG("tq_des: %f\n", tq_des);
M3_DEBUG("tq_on: %f\n", tq_on);
//M3_DEBUG("tq_switch: %f\n", tq_switch);
M3_DEBUG("tq_out: %f\n", tq_out);
M3_DEBUG("tq_act: %f\n", trans->GetTorqueDesActuator());
tmp_cnt = 0;
}*/
break;
}
case JOINT_MODE_OFF:
case JOINT_MODE_PWM:
default:
ctrl_simple->SetDesiredControlMode(CTRL_MODE_OFF);
//act->SetDesiredControlMode(ACTUATOR_MODE_OFF);
mReal des; //dummy
StepBrake(des,0);
break;
};
} else { // Legacy, no supported on BMW,MAX2 versions 2.0
// act->SetDesiredControlMode(ACTUATOR_MODE_OFF);//default
switch(command.ctrl_mode())
{
case JOINT_MODE_PWM:
{
//Ramp in from pwm at switch-over point
mReal pwm_des=command.pwm_desired();
StepBrake(pwm_des,0);
//StepBrake(pwm_des,command.pwm_cmd());
mReal pwm_on, pwm_out;
pwm_on=pwm_on_slew.Step(1.0,1.0/MODE_PWM_ON_SLEW_TIME);
pwm_out=pwm_on*pwm_des+(1.0-pwm_on)*pwm_switch;
//Send out
act->SetDesiredControlMode(ACTUATOR_MODE_PWM);
if (disable_pwm_ramp)
act->SetDesiredPwm(command.pwm_desired());
else
act->SetDesiredPwm((int)pwm_out);;
break;
}
case JOINT_MODE_THETA:
case JOINT_MODE_THETA_MJ:
{
if (!IsEncoderCalibrated())
{
// M3_INFO("Not calib %s\n",GetName().c_str());
break;
}
CalcThetaDesiredSmooth();
mReal des=trans->GetThetaDesActuatorDeg();
StepBrake(des,trans->GetThetaActuatorDeg());
//Do PID in actuator space so result is direct PWM
mReal pwm =pid_theta.Step(trans->GetThetaActuatorDeg(),
trans->GetThetaDotActuatorDeg(),
des,
param.kt_p(),
param.kt_i(),
param.kt_d(),
param.kt_i_limit(),
param.kt_i_range());
act->SetDesiredControlMode(ACTUATOR_MODE_PWM);
//Ramp in from pwm at switch-over point
mReal pwm_on, pwm_out;
pwm_on=pwm_on_slew.Step(1.0,1.0/MODE_PWM_ON_SLEW_TIME);
pwm_out=pwm_on*pwm+(1.0-pwm_on)*pwm_switch;
act->SetDesiredPwm((int)pwm_out);
/*if (tmp_cnt++%100==0)
{
M3_INFO("des %3.2f sen: %3.2f pid: %3.2f out: %3.2f\n",des,trans->GetThetaActuatorDeg(),
pwm,pwm_out);
}*/
break;
}
case JOINT_MODE_THETA_GC:
case JOINT_MODE_THETA_GC_MJ:
{
if (!IsEncoderCalibrated())
break;
mReal stiffness,gravity,tq_des;
CalcThetaDesiredSmooth();
mReal des=trans->GetThetaDesJointDeg();
StepBrake(des,trans->GetThetaJointDeg());
//Do PID in joint space
tq_des =pid_theta_gc.Step(trans->GetThetaJointDeg(),
trans->GetThetaDotJointDeg(),
des,
param.kq_p(),
param.kq_i(),
param.kq_d(),
param.kq_i_limit(),
param.kq_i_range());
stiffness=CLAMP(command.q_stiffness(),0.0,1.0);
gravity=GetTorqueGravity()*param.kq_g();
//Ramp in from torque at switch-over point
mReal tq_on, tq_out;
tq_on=tq_on_slew.Step(1.0,1.0/MODE_TQ_ON_SLEW_TIME);
tq_out=tq_on*(stiffness*tq_des-gravity)+(1.0-tq_on)*tq_switch;
//Send out
trans->SetTorqueDesJoint(tq_out);
act->SetDesiredControlMode(ACTUATOR_MODE_TORQUE);
act->SetDesiredTorque(trans->GetTorqueDesActuator());
break;
}
case JOINT_MODE_TORQUE_GC:
{
if (!IsEncoderCalibrated())
break;
mReal tq_on, tq_out,gravity;
mReal tq_des=command.tq_desired();
StepBrake(tq_des,trans->GetTorqueJoint());
gravity=GetTorqueGravity()*param.kq_g();
//Ramp in from torque at switch-over point
tq_on=tq_on_slew.Step(1.0,1.0/MODE_TQ_ON_SLEW_TIME);
tq_out=tq_on*(tq_des-gravity)+(1.0-tq_on)*tq_switch;
//Send out
trans->SetTorqueDesJoint(tq_out);
act->SetDesiredControlMode(ACTUATOR_MODE_TORQUE);
act->SetDesiredTorque(trans->GetTorqueDesActuator());
break;
}
case JOINT_MODE_TORQUE:
{
mReal tq_on,tq_out;
//Ramp in from torque at switch-over point
mReal tq_des=command.tq_desired();
StepBrake(tq_des,trans->GetTorqueJoint());
tq_on=tq_on_slew.Step(1.0,1.0/MODE_TQ_ON_SLEW_TIME);
tq_out=tq_on*tq_des+(1.0-tq_on)*tq_switch;
//Send out
trans->SetTorqueDesJoint(tq_out);
act->SetDesiredControlMode(ACTUATOR_MODE_TORQUE);
act->SetDesiredTorque(trans->GetTorqueDesActuator());
/*if (tmp_cnt++ == 100)
{
M3_DEBUG("tq_des: %f\n", tq_des);
M3_DEBUG("tq_on: %f\n", tq_on);
M3_DEBUG("tq_switch: %f\n", tq_switch);
M3_DEBUG("tq_out: %f\n", tq_out);
M3_DEBUG("tq_act: %f\n", trans->GetTorqueDesActuator());
tmp_cnt = 0;
}*/
break;
}
case JOINT_MODE_OFF:
default:
act->SetDesiredControlMode(ACTUATOR_MODE_OFF);
mReal des; //dummy
StepBrake(des,0);
break;
};
}
mode_last=(int)command.ctrl_mode();
/*if (tmp_cnt == 100)
{
M3_DEBUG("tq_out: %f\n", ((M3ActuatorCommand*)act->GetCommand())->tq_desired() );
tmp_cnt = 0;
}*/
}
void CustomVehicleControllerComponentEngine::InitEngineTorqueCurve (dFloat vehicleSpeed,
dFloat idleTorque, dFloat rpsAtIdleTorque,
dFloat peakTorque, dFloat rpsAtPeakTorque,
dFloat peakHorsePower, dFloat rpsAtPeakHorsePower,
dFloat torqueAtRedLine, dFloat rpsAtRedLineTorque)
{
int oldGear = m_gearBox->GetGear();
m_gearBox->SetGear(m_gearBox->GetGearCount() - 1);
ConvertToMetricSystem (vehicleSpeed, idleTorque, rpsAtIdleTorque, peakTorque, rpsAtPeakTorque, peakHorsePower, rpsAtPeakHorsePower, torqueAtRedLine, rpsAtRedLineTorque);
SetTopSpeed (vehicleSpeed, rpsAtPeakHorsePower);
dFloat torqueAtPeakPower = peakHorsePower / rpsAtPeakHorsePower;
dAssert (rpsAtIdleTorque > 0.0f);
dAssert (rpsAtIdleTorque < rpsAtPeakTorque);
dAssert (rpsAtPeakTorque < rpsAtPeakHorsePower);
dAssert (rpsAtPeakHorsePower < rpsAtRedLineTorque);
dAssert (idleTorque > 0.0f);
//dAssert (idleTorque < peakTorque);
dAssert (peakTorque > torqueAtPeakPower);
dAssert (torqueAtPeakPower > torqueAtRedLine);
dAssert (torqueAtRedLine > 0.0f);
dAssert (peakTorque * rpsAtPeakTorque < peakHorsePower);
dFloat rpsTable[5];
dFloat torqueTable[5];
rpsTable[0] = 0.0f;
rpsTable[1] = rpsAtIdleTorque;
rpsTable[2] = rpsAtPeakTorque;
rpsTable[3] = rpsAtPeakHorsePower;
rpsTable[4] = rpsAtRedLineTorque;
torqueTable[0] = idleTorque;
torqueTable[1] = idleTorque;
torqueTable[2] = peakTorque;
torqueTable[3] = torqueAtPeakPower;
torqueTable[4] = torqueAtRedLine;
const int count = sizeof (rpsTable) / sizeof (rpsTable[0]);
for (int i = 0; i < count; i ++) {
rpsTable[i] /= m_crownGearRatio;
torqueTable[i] *= m_crownGearRatio * 1.0f;
}
m_torqueCurve.InitalizeCurve (sizeof (rpsTable)/sizeof (rpsTable[0]), rpsTable, torqueTable);
m_radiansPerSecundsAtIdleTorque = rpsTable[1];
m_radiansPerSecundsAtPeakPower = rpsTable[3];
m_radiansPerSecundsAtRedLine = rpsTable[4];
m_engineInternalFriction = torqueTable[2] / (m_radiansPerSecundsAtRedLine * 4.0f);
m_engineIdleResistance1 = torqueTable[2] / (m_radiansPerSecundsAtRedLine * 2.0f);
dFloat W = 0.5f * (rpsTable[3] + rpsTable[4]);
dFloat T = GetTorque (W);
m_engineIdleResistance2 = (T - W * m_engineIdleResistance1) / (W * W);
if (m_engineIdleResistance2 < 1.0e-4f) {
m_engineIdleResistance2 = 1.0e-4f;
}
m_gearBox->SetOptimalShiftLimits (rpsTable[2] /rpsTable[4], rpsTable[3] / rpsTable[4]);
m_gearBox->SetGear(oldGear);
}
bool CarEngineInfo::Load(const PTree & cfg, std::ostream & error_output)
{
std::vector<btScalar> pos(3, 0.0f);
if (!cfg.get("displacement", displacement, error_output)) return false;
if (!cfg.get("max-power", maxpower, error_output)) return false;
if (!cfg.get("peak-engine-rpm", redline, error_output)) return false;
if (!cfg.get("rpm-limit", rpm_limit, error_output)) return false;
if (!cfg.get("inertia", inertia, error_output)) return false;
if (!cfg.get("start-rpm", start_rpm, error_output)) return false;
if (!cfg.get("stall-rpm", stall_rpm, error_output)) return false;
if (!cfg.get("position", pos, error_output)) return false;
if (!cfg.get("mass", mass, error_output)) return false;
position.setValue(pos[0], pos[1], pos[2]);
// fuel consumption
btScalar fuel_heating_value = 4.5E7; // Ws/kg
btScalar engine_efficiency = 0.35;
cfg.get("fuel-heating-value", fuel_heating_value);
cfg.get("efficiency", engine_efficiency);
fuel_rate = 1 / (engine_efficiency * fuel_heating_value);
// nos parameters
cfg.get("nos-mass", nos_mass);
cfg.get("nos-boost", nos_boost);
cfg.get("nos-ratio", nos_fuel_ratio);
// friction (Heywood 1988 tfmep)
friction[0] = btScalar(97000 / (4 * M_PI)) * displacement;
friction[1] = btScalar(15000 / (4 * M_PI)) * displacement;
friction[2] = btScalar(5000 / (4 * M_PI)) * displacement;
std::vector<btScalar> f(3, 0);
if (cfg.get("torque-friction", f))
{
friction[0] = f[0];
friction[1] = f[1];
friction[2] = f[2];
}
// torque
int curve_num = 0;
std::vector<btScalar> torque_point(2);
std::string torque_str("torque-curve-00");
std::vector<std::pair<btScalar, btScalar> > torque;
while (cfg.get(torque_str, torque_point))
{
torque.push_back(std::pair<btScalar, btScalar>(torque_point[0], torque_point[1]));
curve_num++;
std::ostringstream s;
s << "torque-curve-";
s.width(2);
s.fill('0');
s << curve_num;
torque_str = s.str();
}
if (torque.size() <= 1)
{
error_output << "You must define at least 2 torque curve points." << std::endl;
return false;
}
// set torque curve
torque_curve.Clear();
if (torque[0].first > stall_rpm)
{
btScalar dx = torque[1].first - torque[0].first;
btScalar dy = torque[1].second - torque[0].second;
btScalar stall_torque = dy / dx * (stall_rpm - torque[0].first) + torque[0].second;
torque_curve.AddPoint(stall_rpm, stall_torque);
error_output << "Torque curve begins above stall rpm.\n"
<< "Extrapolating to " << stall_rpm << ", " << stall_torque << std::endl;
}
for (const auto & tp : torque)
{
torque_curve.AddPoint(tp.first, tp.second);
}
if (torque[torque.size() - 1].first < rpm_limit)
{
btScalar r = torque[torque.size() - 1].first + 10000.0f;
btScalar t = 0.0f;
torque_curve.AddPoint(r , t);
error_output << "Torque curve ends below rpm limit.\n"
<< "Extrapolating to " << r << ", " << t << std::endl;
}
// calculate idle throttle position
for (idle_throttle = 0.0f; idle_throttle < 1.0f; idle_throttle += 0.01f)
{
if (GetTorque(idle_throttle, start_rpm) > -GetFrictionTorque(idle_throttle, start_rpm))
break;
}
// calculate idle throttle slope
btScalar stall_throttle;
for (stall_throttle = idle_throttle; stall_throttle < 1.0f; stall_throttle += 0.01f)
{
if (GetTorque(stall_throttle, stall_rpm) > -GetFrictionTorque(stall_throttle, stall_rpm))
break;
}
idle_throttle_slope = 1.5f * (idle_throttle - stall_throttle) / (start_rpm - stall_rpm);
return true;
}