undirbúningur falla fyrir can samskipti og prófun með lyklaborði

src/motor_controller.cpp

@@ -4,7 +4,8 @@
 MotorController::MotorController(
     RotaryEncoderDriver &encoder,
     MotorDriver &motor,
-    P_controller &controller,
+    P_controller &p_controller,
+    P_controller &pi_controller,
     State &initialization,
     State &pre_operational,
     State &operational,
@@ -17,9 +18,14 @@ MotorController::MotorController(
       stopped_state(stopped),
       encoder(encoder),
       motor(motor),
-      controller(&controller),
+      p_controller(p_controller),
+      pi_controller(pi_controller),
+      controller(&pi_controller),   // PI er sjálfgefin stýring
       led(LED_PIN),
-      reference_rpm(DEFAULT_REFERENCE_RPM),
+      target_rpm(DEFAULT_TARGET_RPM),
+      reference_rpm(DEFAULT_TARGET_RPM),
+      profile_acceleration(0),
+      profile_deceleration(0),
       last_actual_rpm(0.0),
       last_pwm(0.0)
 {
@@ -92,6 +98,8 @@ bool MotorController::run_speed_controller()
         return false;
     }
 
+    reference_rpm = target_rpm;
+
     const double actual = encoder.rpm();
 
     const double pwm_percent =
@@ -135,28 +143,71 @@ void MotorController::reset_controller()
     controller->reset();
 }
 
+void MotorController::reset_parameters()
+{
+    p_controller.restore_defaults();
+    pi_controller.restore_defaults();
+
+    select_controller(ControllerType::PI);
+    set_target_rpm(DEFAULT_TARGET_RPM);
+
+    set_profile_acceleration(0);
+    set_profile_deceleration(0);
+
+}
+
 void MotorController::set_Kp(double value)
 {
     controller->set_Kp(value);
 }
 
-void MotorController::set_Ti(double value)
+bool MotorController::set_Ti(double value)
 {
-    controller->set_Ti(value);
+    return controller->set_Ti(value);
 }
 
-void MotorController::set_controller(P_controller& new_controller)
+void MotorController::select_controller(ControllerType type)
 {
-    controller = &new_controller;
+    if (type == ControllerType::PI) 
+    {
+        controller = &pi_controller;
+    }
+    else
+    {
+        controller = &p_controller;
+    }
 
     controller->reset();
 }
 
+ControllerType MotorController::get_controller_type() const
+{
+    if (controller == &pi_controller)
+    {
+        return ControllerType::PI; 
+    }
+    else
+    {
+        return ControllerType::P;
+    }
+}
+
 void MotorController::to_stopped()
 {
     transition_to(stopped_state);
 }
 
+NodeState MotorController::get_state() const
+{
+    if (current_state == nullptr)
+    {
+        return NodeState::INITIALIZATION;
+    }
+    else{
+        return current_state->id();
+    }
+}
+
 bool MotorController::fault_active()
 {
     return motor.fault_active();