1+ #pragma once
2+
3+ #include < vector>
4+ #include < queue>
5+ #include < map>
6+ #include < thread>
7+ #include < atomic>
8+ #include < mutex>
9+ #include < ostream>
10+
11+ #include " BoardIO.h"
12+ #include " BasePort.h"
13+
14+ class SimulationPort : public BasePort
15+ {
16+ public:
17+ SimulationPort (int portNum, std::ostream &ostr = std::cerr);
18+ ~SimulationPort ();
19+
20+ PortType GetPortType (void ) const { return PORT_SIMULATION; }
21+ int NumberOfUsers (void );
22+ bool IsOK (void ) { return true ; }
23+ unsigned int GetBusGeneration (void ) const ;
24+ void UpdateBusGeneration (unsigned int gen) {}
25+
26+ unsigned int GetPrefixOffset (MsgType msg) const { return 0 ; }
27+ unsigned int GetWritePostfixSize (void ) const { return 0 ; }
28+ unsigned int GetReadPostfixSize (void ) const { return 0 ; }
29+ unsigned int GetWriteQuadAlign (void ) const { return 0 ; }
30+ unsigned int GetReadQuadAlign (void ) const { return 0 ; }
31+ unsigned int GetMaxReadDataSize (void ) const { return 2048 ; }
32+ unsigned int GetMaxWriteDataSize (void ) const { return 2048 ; }
33+
34+ bool WriteBroadcastOutput (quadlet_t *buffer, unsigned int size);
35+ bool WriteBroadcastReadRequest (unsigned int seq);
36+ void WaitBroadcastRead (void );
37+
38+ void PromDelay (void ) const {}
39+
40+ // Adds board(s)
41+ bool AddBoard (BoardIO *board);
42+ // Removes board
43+ bool RemoveBoard (unsigned char boardId);
44+
45+ protected:
46+ bool Init (void );
47+ void Cleanup (void );
48+ nodeid_t InitNodes (void );
49+
50+ bool ReadQuadletNode (nodeid_t node, nodeaddr_t addr, quadlet_t &data, unsigned char flags = 0 );
51+ bool WriteQuadletNode (nodeid_t node, nodeaddr_t addr, quadlet_t data, unsigned char flags = 0 );
52+ bool WriteBlockNode (nodeid_t node, nodeaddr_t addr, quadlet_t *wdata, unsigned int nbytes, unsigned char flags = 0 );
53+ bool ReadBlockNode (nodeid_t node, nodeaddr_t addr, quadlet_t *rdata, unsigned int nbytes, unsigned char flags = 0 );
54+
55+ private:
56+ struct DynamicsParams {
57+ // 2.0e-5 kg m^2 (approx Maxon RE40 rotor + gearhead)
58+ double motor_inertia = 0.001 ;
59+ // 1.0e-4 Nm/(rad/s)
60+ double viscous_damping = 0.1 ;
61+ // this must coincide with the NmToAmps scale in the XML config
62+ // where you will find the scale computed as 1/torque_constant
63+ double torque_constant = 0.1 ;
64+ // this must coincide with the BitsToPosSI scale in the XML config
65+ // where you will find the scale computed as 360/counts_per_turn
66+ double counts_per_turn = 100000.0 ;
67+ // this is the simulation time step in seconds for joint dynamics
68+ // it should be equal to the period of the control loop or at least
69+ // comparable with the bandwidth of the simulated joint
70+ double dynamics_dt_sec = 0.001 ;
71+ // let's use multi-step integration for better stability
72+ unsigned int integration_steps = 5 ;
73+ // Thermal model parameters [C]
74+ double ambient_temp_c = 25.0 ;
75+ // Reduce heating effect (less aggressive temperature rise) [C/s per A^2]
76+ double thermal_heating_coeff = 0.05 ;
77+ // Increase cooling (stronger dissipation) [1/s]
78+ double thermal_cooling_coeff = 0.02 ;
79+ };
80+
81+ struct AxisState
82+ {
83+ // ENC_MIDRANGE
84+ int32_t EncoderPos = 0x800000 ;
85+
86+ double EncoderVel = 0.0 ;
87+ double SimPosition = 0.0 ;
88+ double SimVelocity = 0.0 ;
89+
90+ int32_t EncoderQtr1 = 0 ;
91+ int32_t EncoderQtr5 = 0 ;
92+ int32_t EncoderRun = 0 ;
93+
94+ // ENC_MIDRANGE
95+ int32_t EncoderPreload = 0x800000 ;
96+ int32_t EncoderOffset = 0 ;
97+
98+ // zero current
99+ uint32_t MotorCurrent = 32768 ;
100+
101+ // Default to OFF
102+ uint32_t MotorStatus = 0x00000000 ;
103+
104+ // Simple temperature state [C]
105+ double TemperatureC = 25.0 ;
106+
107+ // Per-axis dynamics parameters
108+ DynamicsParams params;
109+ };
110+
111+ struct BoardState
112+ {
113+ uint32_t Timestamp = 0 ;
114+ uint32_t Status = 0 ;
115+ uint32_t DigitalIO = 0 ;
116+ uint32_t Temperature = 0 ;
117+
118+ AxisState Axes[4 ];
119+
120+ // Simulation behavior: require a power-off after startup before granting amp enable
121+ // When true, motors won't transition to STATUS until a power-off event is seen once.
122+ bool RequirePowerCycleLatch = true ;
123+ bool HasSeenPowerOff = false ;
124+ };
125+
126+ class WriteRequest
127+ {
128+ public:
129+ nodeid_t node;
130+ nodeaddr_t addr;
131+ quadlet_t data;
132+ unsigned char flags;
133+
134+ WriteRequest (nodeid_t n, nodeaddr_t a, quadlet_t d, unsigned char f)
135+ : node(n), addr(a), data(d), flags(f) {}
136+ };
137+
138+ const uint32_t PERIOD_MASK = 0x03FFFFFFu ;
139+ const uint32_t DIR_BIT = 0x40000000u ;
140+ const uint32_t OVF_BIT = 0x80000000u ;
141+
142+ const char kSimQLASN [12 ] = " QLA 1234-56"
143+ const std::string SimFPGASerialString = " FPGA 1234-56"
144+
145+ // i want a queue to store request for every board id or node id separately
146+ std::map<nodeid_t , std::queue<WriteRequest>> WriteRequestQueues;
147+ std::map<nodeid_t , BoardState> mBoardStates ;
148+
149+ // Simulated FPGA PROM state
150+ uint32_t SimPromCurrentAddr;
151+
152+ // Dynamics simulation thread state
153+ std::thread dynamicsThread;
154+ std::mutex stateMutex;
155+ std::atomic<bool > dynamicsRun{false };
156+
157+ DynamicsParams joint_params{};
158+
159+ // Helper to fetch a simulated PROM byte at absolute 24-bit address
160+ uint8_t GetSimPromByte (uint32_t abs_addr) const ;
161+ quadlet_t ProcessPROM (nodeid_t node);
162+
163+ // Advance simple closed-loop joint dynamics for all axes
164+ void UpdateAxisDynamics (nodeid_t node, BoardState &state, double dt);
165+ void DynamicsThreadFunc ();
166+ };
0 commit comments