GarandPLG
6c0a1fc15b
Replace the compile‑time CAR_MASS constant with a runtime‑settable _member _carMass. Add Engine::setCarMass, default it to 1200 kg, and use it in the acceleration calculation. Prompt the user for the car mass on startup via Serial and update the enable/disable messages.
67 lines
2.1 KiB
C++
67 lines
2.1 KiB
C++
#include "Engine.h"
|
|
#include "Config.h"
|
|
using namespace EngineConsts;
|
|
|
|
Engine::Engine(uint8_t throttlePin,
|
|
uint8_t clutchPin,
|
|
uint8_t brakePin)
|
|
: _throttlePin(throttlePin), _clutchPin(clutchPin), _brakePin(brakePin) {}
|
|
|
|
void Engine::begin() {}
|
|
|
|
float Engine::readNormalized(uint8_t pin) const {
|
|
return analogRead(pin) / 1023.0f;
|
|
}
|
|
|
|
void Engine::update(float dt, uint8_t gearIdx) {
|
|
const float throttle = readNormalized(_throttlePin);
|
|
const float clutch = readNormalized(_clutchPin);
|
|
const float brake = readNormalized(_brakePin);
|
|
|
|
const float targetRPM = RPM_IDLE + throttle * (RPM_MAX - RPM_IDLE);
|
|
|
|
const float wheelRPM = (_speedMs / (2.0f * PI * WHEEL_RADIUS)) * 60.0f;
|
|
|
|
const float gearRatio = GEAR_RATIO[gearIdx];
|
|
const float drivenRPM = wheelRPM * fabs(gearRatio) * FINAL_DRIVE;
|
|
|
|
_engineRPM = clutch * drivenRPM + (1.0f - clutch) * targetRPM;
|
|
_engineRPM = constrain(_engineRPM, RPM_IDLE, 7500.0f);
|
|
|
|
const float engineForce = (_engineRPM / RPM_MAX) * (1.0f - clutch) * ENGINE_FORCE_FACTOR;
|
|
const float brakeForce = brake * BRAKE_FORCE_MAX;
|
|
const float dragForce = 0.5f * DRAG_COEFF * FRONTAL_AREA * (_speedMs * _speedMs);
|
|
const int direction = (gearIdx == -1) ? -1 : 1;
|
|
const float netForce = direction * engineForce - brakeForce - dragForce;
|
|
const float acceleration = netForce / _carMass;
|
|
|
|
_speedMs += acceleration * dt;
|
|
if (_speedMs < 0.0f) _speedMs = 0.0f;
|
|
|
|
setMotor(IN_3, IN_4, ENA_2, targetRPM, false);
|
|
|
|
bool reverseWheel = (gearIdx == -1);
|
|
setMotor(IN_1, IN_2, ENA_1, wheelRPM * 10.0f, reverseWheel);
|
|
}
|
|
|
|
int Engine::getRPM() const {
|
|
return static_cast<int>(_engineRPM + 0.5f);
|
|
}
|
|
|
|
int Engine::getSpeedKmh() const {
|
|
return static_cast<int>((_speedMs * 3.6f) + 0.5f);
|
|
}
|
|
|
|
uint8_t Engine::rpmToPwm(float rpm) {
|
|
rpm = constrain(rpm, 0.0f, RPM_MAX);
|
|
return static_cast<uint8_t>(roundf(rpm / RPM_MAX * 255.0f));
|
|
}
|
|
|
|
void Engine::setMotor(uint8_t inA, uint8_t inB, uint8_t ena,
|
|
float rpm, bool reverse) {
|
|
digitalWrite(inA, reverse ? LOW : HIGH);
|
|
digitalWrite(inB, reverse ? HIGH : LOW);
|
|
|
|
analogWrite(ena, rpmToPwm(rpm));
|
|
}
|