lvl1controller concept update

source/RobotAPI/components/units/RobotUnit.h

@@ -226,14 +226,7 @@ namespace armarx
         std::unordered_map<std::string, std::size_t> jointIndices;
         std::chrono::nanoseconds controlLoopPeriod;
         std::chrono::nanoseconds controlLoopEmergencyStopPeriod;
-
-        std::vector<LVL0Controller<ePositionControl>*> lvl0PositionControlController;
-        std::vector<LVL0Controller<eVelocityControl>*> lvl0VelocityControlController;
-        std::vector<LVL0Controller<eTorqueControl>*> lvl0TorqueControlController;
-        std::vector<LVL0Controller<ePositionVelocityControl>*> lvl0PositionVelocityControlController;
-
-
-
+        std::map<std::string, std::vector<LVL0ControllerBase*>> lvl0ControllerMap;
 
     private:
         enum class ControlThreadState

source/RobotAPI/libraries/Controllers/LVL1Controller.h

@@ -46,26 +46,78 @@ namespace armarx
     *
     * Detailed description of class LVL1Controller.
     */
+    template <typename ControlDataStruct>
     class LVL1Controller:
         virtual public LVL1ControllerInterface,
         virtual public Component
     {
     public:
-        LVL1Controller(/*string instanceName, RobotUnitPtr robotUnit, ControllerConfig config*/);
-        virtual void run() = 0;
-        virtual std::map<std::string, ControlMode> getJointControlmodeMap(const Ice::Current& = GlobalIceCurrent) = 0;
-        //virtual void setJointTargetPtrs(std::vector<JointTargetBase*> ptrs) = 0;
+        LVL1Controller(std::string instanceName, RobotUnitPtr robotUnit, ControllerConfigPtr config);
+        virtual void run(const IceUtil::Time& timestamp, const IceUtil::Time& delta) = 0;
 
         virtual bool isControllerActive(const Ice::Current& = GlobalIceCurrent) const final
         {
             return isActive;
         }
+        const ControlDataStruct& readControlStruct()
+        {
+            return trippleBuffer.read();
+        }
+        void updateControlStruct()
+        {
+            trippleBuffer.write(data);
+        }
+
+        void swapBufferAndRun(const IceUtil::Time& timestamp, const IceUtil::Time& delta)
+        {
+            readControlStruct();
+            run(timestamp, delta);
+        }
+        ControlDataStruct& getWriterControlStruct()
+        {
+            return controlData;
+        }
+
     private:
+        ControlDataStruct controlData;
         friend class RobotUnit;
         std::atomic_bool isActive {false};
     };
     using LVL1ControllerPtr = IceInternal::Handle<LVL1Controller>;
 
+    class TrajectoryController : virtual public LVL1Controller<TrajectoryControlDataStruct>,
+            public TrajectoryControllerInterface
+    {
+        JointVelocityTargetPtr targetJnt1Ptr;
+        std::vector<const float*> jointValuePtrs;
+        ExampleLvl1Controller(std::string instanceName, RobotUnitPtr robotUnit, ControllerConfigPtr config)
+        {
+            jointValuePtrs = robotUnit->getKinematicUnitData()->getJointAngles({"jnt1","jnt2"});
+            targetJnt1Ptr = dynamic_cast<JointVelocityTarget*>(robotUnit->getJointTarget("jnt1", ControlModes::VelocityMode)); // for the results of this controller, goes to
+
+        }
+
+        ExampleLvl1Controller(std::string instanceName, JointVelocityTargetPtr targetPtr, ControllerConfigPtr config)
+        {
+
+        }
+
+        void run(const IceUtil::Time& timestamp, const IceUtil::Time& delta)
+        {
+            pid.update(TrajectoryControlDataStruct.pos, jointValuePtrs.at(0), delta.toMicroSeconds());
+            *targetJnt1Ptr = pid.getValue();
+        }
+
+
+        void setTrajectory(Trajectory traj, Ice::Current())
+        {
+            ScopedLock lock(mutex);
+            controlData.pos = traj.pos.at(rand());
+            updateControlStruct();
+        }
+        TrajectoryControlDataStruct getTrajectoryControlDataStruct(Ice::Current()) const;
+    }
+
 }
 
 #endif