GarandPLG
1f849e3455
Removed the empty Engine::begin() method and its declaration. Moved getRPM, getSpeedKmh, rpmToPwm, and setMotor implementations to the top of Engine.cpp and deleted their duplicate definitions at the bottom. Fixed wheel motor PWM calculation by using the raw wheelRPM instead of wheelRPM * 10.0f. Deleted the call to engine.begin() in the Arduino setup.
65 lines
2.1 KiB
C++
65 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) {}
|
|
|
|
float Engine::readNormalized(uint8_t pin) const {
|
|
return analogRead(pin) / 1023.0f;
|
|
}
|
|
|
|
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));
|
|
}
|
|
|
|
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, reverseWheel);
|
|
}
|