Everything's cursed still but it compiles

include/controller/feedforward.hpp

@@ -5,14 +5,15 @@
 
 namespace controllers {
 template <isQuantity In, isQuantity Out> class LinearFeedForward : public Controller<In, In, Out> {
-        using Div = Divided<In, Out>::Self;
+        using Div = Divided<Out, In>::Self;
         Div m_kF;
         Out m_b;
     public:
-        LinearFeedForward(const In initial_target, const Div kF, const Out b = 0)
+        LinearFeedForward(const In initial_target, const Div kF, const Out b = Out(0))
             : m_kF(kF), m_b(b), Controller<In, In, Out>(initial_target) {}
 
-        virtual Out update(In setting) override { return m_kF * setting + m_b; }
+        virtual Out update(const In &input) override {
+             return (m_kF * input) + m_b; }
 
         virtual void reset() override {
             // do nothing lmao
@@ -25,7 +26,7 @@ template <isQuantity In, isQuantity Out> class FunctionalFeedForward : public Co
         FunctionalFeedForward(const In initial_target, std::function<Out(In)> func)
             : m_func(func), Controller<In, In, Out>(initial_target) {}
 
-        virtual Out update(In setting) override { return m_func(setting); }
+        virtual Out update(const In &setting) override { return m_func(setting); }
 
         virtual void reset() override {
             // do nothing lmao

include/controller/fusions.hpp

@@ -7,48 +7,58 @@
 
 namespace controllers {
 
-template <typename Target, typename In, typename Out,
-          typename = std::enable_if<Isomorphic<typeof(std::declval<Out> + std::declval<Out>),
-                                               Out>>> // todo is this really necessary? Probably
-class AdditiveFusionController : public Controller<Target, In, Out> {
-        static_assert(std::declval<Out>() + std::declval<Out>());
-        Controller<Target, In, Out>&controller1, &controller2;
-        Out current;
+template <typename Target, typename In, typename Out> class AdditiveFusionController
+    : public Controller<Target, In, Out> {
+        Controller<Target, In, Out>&m_controller1, &m_controller2;
     public:
         AdditiveFusionController(Controller<Target, In, Out>& controller1, Controller<Target, In, Out>& controller2)
-            : controller1(controller1), controller2(controller2) {}
+            : m_controller1(controller1), m_controller2(controller2),
+              Controller<Target, In, Out>(controller1.getTarget()) {}
 
-        virtual Out getOut() const override { return current; }
+        virtual Out update(const In& input) override {
+            return m_controller1.update(input) + m_controller2.update(input);
+        }
 
-        virtual Out step(In setting) override {
-            current = controller1.step(setting) + controller2.step(setting);
-            return current;
+        virtual void reset() override {
+            m_controller1.reset();
+            m_controller2.reset();
         }
 };
 
-template <typename Target, typename In, typename Mid, typename Out> class ChainedFusionController
+template <typename Target, typename Target2, typename In, typename Mid, typename Out> class ChainedFusionController
     : public Controller<Target, In, Out> {
-        Controller<Target, In, Mid>& controller1;
-        Controller<Target, Mid, Out>& controller2;
+        Controller<Target, In, Mid>& m_controller1;
+        Controller<Target2, Mid, Out>& m_controller2;
     public:
-        ChainedFusionController(Controller<Target, In, Mid>& controller1, Controller<Target, Mid, Out>& controller2)
-            : controller1(controller1), controller2(controller2) {}
+        ChainedFusionController(Controller<Target, In, Mid>& controller1, Controller<Target2, Mid, Out>& controller2)
+            : m_controller1(controller1), m_controller2(controller2),
+              Controller<Target, In, Out>(controller1.getTarget()) {}
+
+        virtual Out update(const In& input) override { return m_controller2.update(m_controller1.update(input)); }
 
-        virtual Out update(In input) override { return controller2.step(controller1.step(input)); }
+        virtual void reset() override {
+            m_controller1.reset();
+            m_controller2.reset();
+        }
 };
 
 template <typename Target, typename In, typename Out, typename Target1, typename In1, typename Out1, typename Target2,
           typename In2, typename Out2>
-class CustomChainedController : public Controller<Target, In, Out> {
+class FunctionalChainedController : public Controller<Target, In, Out> {
         using Function = std::function<Out(In, Controller<Target1, In1, Out1>&, Controller<Target2, In2, Out2>&)>;
-        Controller<Target1, In1, Out1>& controller1;
-        Controller<Target2, In2, Out2>& controller2;
-        Function func;
+        Controller<Target1, In1, Out1>& m_controller1;
+        Controller<Target2, In2, Out2>& m_controller2;
+        Function m_func;
     public:
-        CustomChainedController(Controller<Target1, In1, Out1>& controller1,
-                                Controller<Target2, In2, Out2>& controller2, Function func)
-            : controller1(controller1), controller2(controller2), func(func) {}
+        FunctionalChainedController(Controller<Target1, In1, Out1>& controller1,
+                                    Controller<Target2, In2, Out2>& controller2, Function func, Target initial_target)
+            : m_controller1(controller1), m_controller2(controller2), m_func(func), Controller<Target, In, Out>(initial_target) {}
+
+        virtual Out update(const In& input) override { return m_func(input, m_controller1, m_controller2); }
 
-        virtual Out update(In input) override { return func(input, controller1, controller2); }
+        virtual void reset() override {
+            m_controller1.reset();
+            m_controller2.reset();
+        }
 };
 } // namespace controllers

include/controller/pid.hpp

@@ -83,5 +83,9 @@ template <isQuantity In, isQuantity Out> class PID : public Controller<In, In, O
             m_prev_error = NAN;
             m_current = Out(0);
         }
+
+        pidConfig<In,Out>& getConfig() {
+            return m_config;
+        }
 };
 } // namespace controllers

src/main.cpp

@@ -1,6 +1,15 @@
 #include "main.h"
+#include "controller/feedforward.hpp"
+#include "controller/fusions.hpp"
 #include "controller/pid.hpp"
 #include "controller/slew.hpp"
+#include "units/units.hpp"
+#include <functional>
+
+Force test(Length, controllers::Controller<Length, Length, Voltage>& a,
+           controllers::Controller<Length, Length, LinearVelocity>& b) {
+    return 0_N;
+}
 
 /**
  * Runs initialization code. This occurs as soon as the program is started.
@@ -12,6 +21,13 @@ void initialize() {
     controllers::pidConfig<Length, Voltage> config {1, 2, 3};
     controllers::PID pid(config, 10_in);
     controllers::Slewed testSlewed(pid, 0.1_volt);
+    controllers::AdditiveFusionController addTest(pid, testSlewed);
+    controllers::LinearFeedForward<Voltage, LinearVelocity> ff(1_volt, 3 * m / sec / volt);
+    controllers::ChainedFusionController chainedTest(addTest, ff);
+    std::function<Force(Length, controllers::Controller<Length, Length, Voltage>&,
+                        controllers::Controller<Length, Length, LinearVelocity>&)>
+        b = test;
+    controllers::FunctionalChainedController functionalTest(addTest, chainedTest, b, 5_m);
 }
 
 /**