Add Engine class and integrate it into Projekt

Add clutch, brake, and throttle pin constants to Config.h.
Implement Engine class in Engine.h/Engine.cpp to read analog inputs,
compute engine RPM, vehicle speed, and apply forces.
Create an Engine instance in Projekt.ino, initialize it in setup,
and update it every 20ms, displaying RPM and speed.

Config.h

@@ -24,3 +24,8 @@ const int GEAR_THRESH_HIGH = 682;
 // LCD
 const int LCD_COLS = 16;
 const int LCD_ROWS = 2;
+
+// Silnik
+const uint8_t CLUTCH_PIN = A7;
+const uint8_t BRAKE_PIN = A2;
+const uint8_t THROTTLE_PIN = A3;

Engine.cpp

@@ -0,0 +1,93 @@
+#include "Engine.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) {
+  Serial.println("----------------");
+
+  const float throttle = readNormalized(_throttlePin);
+  Serial.print("Throttle: ");
+  Serial.println(throttle);
+
+  const float clutch = readNormalized(_clutchPin);
+  Serial.print("Clutch: ");
+  Serial.println(clutch);
+
+  const float brake = readNormalized(_brakePin);
+  Serial.print("Brake: ");
+  Serial.println(brake);
+
+  const float targetRPM = RPM_IDLE + throttle * (RPM_MAX - RPM_IDLE);
+  Serial.print("targetRPM: ");
+  Serial.println(targetRPM);
+
+  const float wheelRPM = (_speedMs / (2.0f * PI * WHEEL_RADIUS)) * 60.0f;
+  Serial.print("wheelRPM: ");
+  Serial.println(wheelRPM);
+
+  const float gearRatio = GEAR_RATIO[gearIdx];
+  Serial.print("gearRatio: ");
+  Serial.println(gearRatio);
+
+  const float drivenRPM = wheelRPM * fabs(gearRatio) * FINAL_DRIVE;
+  Serial.print("drivenRPM: ");
+  Serial.println(drivenRPM);
+
+  _engineRPM = clutch * drivenRPM + (1.0f - clutch) * targetRPM;
+  Serial.print("_engineRPM: ");
+  Serial.println(_engineRPM);
+
+  _engineRPM = constrain(_engineRPM, RPM_IDLE, 7500.0f);
+  Serial.print("_engineRPM (constrain): ");
+  Serial.println(_engineRPM);
+
+  const float engineForce = (_engineRPM / RPM_MAX) * (1.0f - clutch) * ENGINE_FORCE_FACTOR;
+  Serial.print("engineForce: ");
+  Serial.println(engineForce);
+
+  const float brakeForce = brake * BRAKE_FORCE_MAX;
+  Serial.print("brakeForce: ");
+  Serial.println(brakeForce);
+
+  const float dragForce = 0.5f * DRAG_COEFF * FRONTAL_AREA * (_speedMs * _speedMs);
+  Serial.print("dragForce: ");
+  Serial.println(dragForce);
+
+  const int direction = (gearIdx == 5) ? -1 : 1;
+  Serial.print("direction: ");
+  Serial.println(direction);
+
+  const float netForce = direction * engineForce - brakeForce - dragForce;
+  Serial.print("netForce: ");
+  Serial.println(netForce);
+
+  const float acceleration = netForce / CAR_MASS;
+  Serial.print("acceleration: ");
+  Serial.println(acceleration);
+
+  _speedMs += acceleration * dt;
+  if (_speedMs < 0.0f) _speedMs = 0.0f;
+  Serial.print("_speedMs: ");
+  Serial.println(_speedMs);
+
+  delay(1000);
+}
+
+int Engine::getRPM() const {
+  return static_cast<int>(_engineRPM + 0.5f);
+}
+
+int Engine::getSpeedKmh() const {
+  return static_cast<int>((_speedMs * 3.6f) + 0.5f);
+}

Engine.h

@@ -0,0 +1,48 @@
+#pragma once
+#include <Arduino.h>
+
+namespace EngineConsts {
+constexpr float GEAR_RATIO[6] = {
+  3.31f,
+  1.95f,
+  1.28f,
+  0.97f,
+  0.75f,
+  -3.66f
+};
+
+constexpr float FINAL_DRIVE = 3.74f;   // przekładnia różnicowa
+constexpr float WHEEL_RADIUS = 0.30f;  // m
+constexpr float CAR_MASS = 1200.0f;    // kg
+constexpr float RPM_IDLE = 800.0f;     // obroty jałowe
+constexpr float RPM_MAX = 7000.0f;     // maksymalne obroty
+
+constexpr float ENGINE_FORCE_FACTOR = 3000.0f;  // N przy maks. RPM i wciśniętym sprzęgle
+constexpr float BRAKE_FORCE_MAX = 5000.0f;      // N przy pełnym pedale hamulca
+constexpr float DRAG_COEFF = 0.3f;              // Cd
+constexpr float FRONTAL_AREA = 2.2f;            // m^2
+}
+
+class Engine {
+public:
+  Engine(uint8_t throttlePin,
+         uint8_t clutchPin,
+         uint8_t brakePin);
+
+  void begin();
+
+  void update(float dt, uint8_t gearIdx);
+
+  int getRPM() const;
+  int getSpeedKmh() const;
+
+private:
+  const uint8_t _throttlePin;
+  const uint8_t _clutchPin;
+  const uint8_t _brakePin;
+
+  float _engineRPM = EngineConsts::RPM_IDLE;
+  float _speedMs = 0.0f;
+
+  float readNormalized(uint8_t pin) const;
+};

Projekt.ino

@@ -5,15 +5,19 @@
 #include "Gear.h"
 #include "Sonar.h"
 #include "RFID.h"
+#include "Engine.h"
 
 Display display;
 GearSelector gear(GEAR_X_PIN, GEAR_Y_PIN, GEAR_THRESH_LOW, GEAR_THRESH_HIGH);
 Sonar sonar(TRIG_PIN, ECHO_PIN, SONAR_MAX_DIST_CM);
 RFIDReader rfid(SS_PIN, RST_PIN);
+Engine engine(THROTTLE_PIN, CLUTCH_PIN, BRAKE_PIN);
 
 bool leftBlink = false;
 bool rightBlink = false;
 
+unsigned long prevUpdate = 0;
+
 void setup() {
   Serial.begin(9600);
 
@@ -22,6 +26,7 @@ void setup() {
 
   rfid.begin();
   display.begin(LCD_COLS, LCD_ROWS);
+  engine.begin();
 }
 
 void loop() {
@@ -33,6 +38,15 @@ void loop() {
   rightBlink = digitalRead(TS_RIGHT);
   display.updateDirection(leftBlink, rightBlink);
 
+  unsigned long now = millis();
+  float dt = (now - prevUpdate) / 1000.0f;
+  if (dt >= 0.02f) {
+    prevUpdate = now;
+    engine.update(dt, gear.getGear());
+    display.updateRPM(engine.getRPM());
+    display.updateSpeed(engine.getSpeedKmh());
+  }
+
   long dist = sonar.measure();
   // Serial.println(dist);