Brushed motors drivers were implemented

src/Logic/PIDController.hpp

@@ -9,10 +9,11 @@
 class PIDController : public SavedPidRegulator {
 public:
     PIDController(float p, float i, float d, BrushedMotor *rotor,
-                  const char *name = "PID", uint16_t dt = 10)
+                  const char *name = "PID", uint16_t dt = 10, const bool isEnabled = false)
             : SavedPidRegulator(p, i, d, name, dt) {
         assert(rotor != nullptr);
         _rotor = rotor;
+        _isEnabled = isEnabled;
         this->_dt = dt;
         setLimits(0, _rotor->maxDuty());
         _pid_timer = millis();
@@ -20,27 +21,30 @@ public:
     }
 
     void enable() {
-        _rotor->enable();
+        _isEnabled = true;
+        _rotor->setDuty(0);
     }
 
     void disable() {
-        _rotor->disable();
+        _isEnabled = false;
+        _rotor->setDuty(0);
     }
 
     void tick() {
-        if (millis() - _pid_timer >= _dt) {
-            _rotor->setDuty((uint32_t) getResultTimer());
+        if (millis() - _pid_timer >= _dt and _isEnabled) {
+            _rotor->setDuty((uint16_t) getResultTimer());
             _pid_timer = millis();
         }
     }
 
-    void setRotorDuty(uint32_t duty) {
+    void setRotorDuty(int16_t duty) {
         /* Used only for test purposes */
-        _rotor->setDuty(duty);
+        _rotor->setExtendedDuty(duty);
     }
 
 private:
     uint16_t _dt;
     BrushedMotor *_rotor = nullptr;
     uint32_t _pid_timer = 0;
+    bool _isEnabled = false;
 };