├── __pycache__ ├── PID.cpython-37.pyc ├── Graphics.cpython-37.pyc ├── Physics.cpython-37.pyc └── rocketConfig.cpython-37.pyc ├── rocketConfig.py ├── README.md ├── PID.py ├── Graphics.py ├── Simulator.py └── Physics.py /__pycache__/PID.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jmack66/TVC-Sim/HEAD/__pycache__/PID.cpython-37.pyc -------------------------------------------------------------------------------- /__pycache__/Graphics.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jmack66/TVC-Sim/HEAD/__pycache__/Graphics.cpython-37.pyc -------------------------------------------------------------------------------- /__pycache__/Physics.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jmack66/TVC-Sim/HEAD/__pycache__/Physics.cpython-37.pyc -------------------------------------------------------------------------------- /__pycache__/rocketConfig.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jmack66/TVC-Sim/HEAD/__pycache__/rocketConfig.cpython-37.pyc -------------------------------------------------------------------------------- /rocketConfig.py: -------------------------------------------------------------------------------- 1 | from dataclasses import dataclass 2 | 3 | @dataclass 4 | class vehicleProperties: 5 | mass: float 6 | mmoi: float 7 | com2TVC: float 8 | servo_lim: float 9 | servo_rate_lim: float 10 | 11 | 12 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # TVC-Sim 2 | An improved version of PIDTest- moved things around so that the main file was simpler and easier to follow along with 3 | 4 | Things have been moved/abstracted to make the simulator file much more plug and play. It doesnt have all the features yet, will maybe get to adding these soon. But its a big improvement 5 | to the old code 6 | 7 | 8 | The original video for the PID sim 9 | https://youtu.be/ZMI_kpNUgJM 10 | -------------------------------------------------------------------------------- /PID.py: -------------------------------------------------------------------------------- 1 | 2 | 3 | class PID(): 4 | def __init__(self,KP,KI,KD,target = 0): 5 | self.kp = KP 6 | self.ki = KI 7 | self.kd = KD 8 | self.sp = target 9 | self.error_last = 0 10 | self.integral_error = 0 11 | self.saturation_max = None 12 | self.saturation_min = None 13 | def compute(self,pos,dt): 14 | error = self.sp - pos #compute the error 15 | derivative_error = (error - self.error_last) / dt #find the derivative of the error (how the error changes with time) 16 | self.integral_error += error * dt #error build up over time 17 | output = self.kp*error + self.ki*self.integral_error + self.kd*derivative_error 18 | self.error_last = error 19 | if output > self.saturation_max and self.saturation_max is not None: 20 | output = self.saturation_max 21 | elif output < self.saturation_min and self.saturation_min is not None: 22 | output = self.saturation_min 23 | return output 24 | def setLims(self,min,max): 25 | self.saturation_max = max 26 | self.saturation_min = min 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /Graphics.py: -------------------------------------------------------------------------------- 1 | import turtle 2 | import matplotlib.pyplot as plt 3 | import numpy as np 4 | 5 | COLORS = ['g','g','r','c','m','y','k'] 6 | 7 | class GraphicHandler(): 8 | def __init__(self): 9 | self.window = turtle.Screen() 10 | self.window.setup(800,600) 11 | self.fig = None 12 | self.axs = None 13 | self.agents = [] 14 | self.graph_rate = 0.01 15 | def graphsHandler(self,num_graphs,graphs_dict): 16 | self.fig, self.axs = plt.subplots(num_graphs, sharex=True) 17 | i = 0 18 | for a in self.axs: 19 | a.set(xlabel= graphs_dict[i]['xlab'],ylabel= graphs_dict[i]['ylab']) 20 | i += 1 21 | def setGraphRate(self,dt): 22 | self.graph_rate = dt 23 | def setGraphColors(self): 24 | pass 25 | def updateGraphs(self,graph_values): 26 | i = 0 27 | for a in self.axs: 28 | a.plot(graph_values[i][0],graph_values[i][1],'tab:red') 29 | i += 1 30 | plt.pause(self.graph_rate) 31 | def showGraphs(self,graph_values,batch_num = 0): 32 | i = 0 33 | for a in self.axs: 34 | a.plot(graph_values[i][0],graph_values[i][1],'tab:red') 35 | i += 1 36 | plt.show() 37 | def createAgent(self,color): 38 | agent = turtle.Turtle() 39 | agent.shape('square') 40 | agent.color(color) 41 | agent.penup() 42 | agent.goto(0,0) 43 | agent.speed(0) 44 | self.agents.append(agent) 45 | return len(self.agents) - 1 # returns the agent adress(ID) so it can be accessed for moving 46 | def moveAgent(self,ID,x,y): 47 | agent = self.agents[ID] 48 | agent.setx(x) 49 | agent.sety(y) 50 | 51 | def rotateAgent(self,ID,angle): # angle in radians 52 | agent = self.agents[ID] 53 | agent.setheading(np.rad2deg(angle)) 54 | 55 | if __name__ == "__main__": 56 | graph_labels = {"xlab" : "xlab", "ylab" : "ylab", "title" : "Title"} 57 | graph_labels2 = {"xlab" : "xlab2", "ylab" : "ylab2", "title" : "Title2"} 58 | graphs_dict = [graph_labels,graph_labels2] 59 | g = GraphicHandler() 60 | g.graphsHandler(2,graphs_dict) 61 | x1 = [] 62 | y1 = [] 63 | x2 = [] 64 | y2 = [] 65 | for i in range(10): 66 | x1.append(i) 67 | y1.append(i) 68 | y2.append(i) 69 | x2.append(i) 70 | graph_values = [(x1,y1),(x2,y2)] 71 | g.updateGraphs(graph_values) 72 | 73 | -------------------------------------------------------------------------------- /Simulator.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import time 3 | import PID as pid 4 | import Graphics as gh 5 | import Physics as phys 6 | import rocketConfig 7 | #Units are SI units... m/s/radians/kg 8 | 9 | time_ret = [] 10 | angles_ret = [] 11 | vert_pos_ret = [] 12 | hori_pos_ret = [] 13 | 14 | ori_labels = {"xlab" : "time(s)", "ylab" : "angle(theta)", "title" : "Theta"} 15 | translate_labels = {"xlab" : "time(s)", "ylab" : "Pos Z", "title" : "Position Z"} 16 | vertical_labels = {"xlab" : "time(s)", "ylab" : "Pos X", "title" : "Position X"} 17 | 18 | graphs_dict = [ori_labels,translate_labels,vertical_labels] 19 | 20 | graphics = gh.GraphicHandler() 21 | graphics.graphsHandler(3,graphs_dict) #number of graphs and their labels 22 | 23 | rocket = graphics.createAgent('black') 24 | targe = graphics.createAgent('red') 25 | 26 | vehicle = rocketConfig.vehicleProperties(1,0.1,0.5,np.deg2rad(15),np.deg2rad(150)) 27 | #mass(kg),mmoi(kg m^2),com2TVC(meters),servo_lim(rad),servo_rate_lim(rad/s) 28 | 29 | THRUST = 12 # Newtons 30 | 31 | 32 | #initial state vector 33 | state_vector = {"ax" : 0 ,"vx" : 0,"px" : 0,"az" : 0 ,"vz" : 1,"pz" : 0 ,"alpha" : 0.0,"omega" : 0.5,"theta" : 0.0} 34 | 35 | rocket_phys = phys.threeDofPhysics(state_vector,vehicle.mass,vehicle.mmoi) 36 | 37 | #Controller setup 38 | controller = pid.PID(0.07,0.01,0.01,0) #KP,KI,KD,setpoint 39 | controller.setLims(-10,10)#output limits 40 | #our TVC is also limited by SERVO_LIMIT but we might want to change the the two independently 41 | 42 | sim_time = 0.0 43 | time_lim = 10.0 44 | delta_t = 0.1 45 | tvc_input = 0 46 | 47 | 48 | if __name__ == "__main__": 49 | while(sim_time < time_lim): 50 | 51 | #Physics 52 | forces = rocket_phys.tvcPhysics(tvc_input,THRUST,vehicle,delta_t) # Compute Forces from TVC 53 | rocket_phys.inputForces(forces,delta_t) # Apply Forces to body 54 | graphics.moveAgent(rocket,rocket_phys.state_vector["pz"],rocket_phys.state_vector["px"])# Update Graphics and Plots 55 | graphics.rotateAgent(rocket,rocket_phys.state_vector["theta"]) 56 | 57 | #Saving data we want to plot 58 | angles_ret.append(np.rad2deg(rocket_phys.state_vector['theta'])) 59 | hori_pos_ret.append(rocket_phys.state_vector["pz"]) 60 | vert_pos_ret.append(rocket_phys.state_vector["px"]) 61 | graphs = [(time_ret,angles_ret),(time_ret,hori_pos_ret),(time_ret,vert_pos_ret)] 62 | 63 | #Real-time plotting is slow right now -> need to look into blit for matplotlib 64 | #graphics.updateGraphs(graphs) 65 | 66 | # Compute Control Logic 67 | tvc_input = controller.compute(rocket_phys.state_vector['theta'],delta_t) 68 | 69 | 70 | time_ret.append(sim_time) 71 | sim_time += delta_t 72 | 73 | graphics.showGraphs(graphs) -------------------------------------------------------------------------------- /Physics.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | # The following is a simplified physics simulation for a 3-DOF (Degrees of Freedom) system. 4 | # The system is subject to forces and moments which influence its state over time. 5 | 6 | class ThreeDofPhysics: 7 | def __init__(self, initial_state, mass, moment_of_inertia): 8 | """ 9 | Initialize the physics simulation with initial conditions. 10 | 11 | Parameters: 12 | - initial_state (dict): Initial values of acceleration (ax, az), velocity (vx, vz), position (px, pz), 13 | angular acceleration (alpha), angular velocity (omega), and angle (theta). 14 | - mass (float): Mass of the object. 15 | - moment_of_inertia (float): Moment of inertia around the rotation axis. 16 | """ 17 | self.state = initial_state 18 | self.mass = mass 19 | self.moment_of_inertia = moment_of_inertia 20 | self.last_input_angle = None 21 | self.actuator_state = [] 22 | 23 | def apply_forces(self, force_vector, time_step, use_gravity=True, constrain_x_negative=False): 24 | """ 25 | Update the state of the system based on the applied forces. 26 | 27 | Parameters: 28 | - force_vector (list): A list containing [Fx, Fz, Tau] where Fx is the force in x-direction, 29 | Fz is the force in z-direction, and Tau is the torque. 30 | - time_step (float): Time increment for the simulation. 31 | - use_gravity (bool): Whether to include gravitational acceleration. 32 | - constrain_x_negative (bool): Whether to prevent negative position in the x-direction. 33 | 34 | Returns: 35 | - dict: Updated state vector. 36 | """ 37 | gravity_acceleration = -9.81 if use_gravity else 0 38 | self.state["ax"] = (force_vector[0] / self.mass) + gravity_acceleration 39 | self.state["az"] = force_vector[1] / self.mass 40 | self.state["alpha"] = force_vector[2] / self.moment_of_inertia 41 | 42 | # Update velocities and positions 43 | self.state["vx"] += self.state["ax"] * time_step 44 | self.state["vz"] += self.state["az"] * time_step 45 | self.state["omega"] += self.state["alpha"] * time_step 46 | self.state["px"] += self.state["vx"] * time_step 47 | if constrain_x_negative and self.state["px"] < 0: 48 | self.state["px"] = 0 49 | self.state["pz"] += self.state["vz"] * time_step 50 | self.state["theta"] += self.state["omega"] * time_step 51 | 52 | return self.state 53 | 54 | def calculate_actuator_forces(self, input_angle, thrust, vehicle, time_step, print_warnings=False): 55 | """ 56 | Calculate the forces and torque generated by the actuator based on input angle and thrust. 57 | 58 | Parameters: 59 | - input_angle (float): The angle of the actuator in radians. 60 | - thrust (float): The thrust force generated by the actuator. 61 | - vehicle (object): Vehicle object containing servo limits and rate limits. 62 | - time_step (float): Time increment for the simulation. 63 | - print_warnings (bool): Whether to print warnings when limits are reached. 64 | 65 | Returns: 66 | - list: Calculated forces [Fx, Fz] and torque Tau. 67 | """ 68 | # Constrain input_angle within servo limits 69 | if input_angle > vehicle.servo_limit: 70 | input_angle = vehicle.servo_limit 71 | if print_warnings: 72 | print("WARNING: Actuator Limit Reached") 73 | if input_angle < -vehicle.servo_limit: 74 | input_angle = -vehicle.servo_limit 75 | if print_warnings: 76 | print("WARNING: Actuator Limit Reached") 77 | 78 | # Rate of change of input angle 79 | if self.last_input_angle is None: 80 | self.last_input_angle = input_angle 81 | angle_change_rate = (self.last_input_angle - input_angle) / time_step 82 | 83 | # Constrain angle change rate within servo rate limits 84 | if angle_change_rate > vehicle.servo_rate_limit: 85 | input_angle = self.last_input_angle - vehicle.servo_rate_limit * time_step 86 | if print_warnings: 87 | print("WARNING: Actuator Rate Limit Reached") 88 | if angle_change_rate < -vehicle.servo_rate_limit: 89 | input_angle = self.last_input_angle + vehicle.servo_rate_limit * time_step 90 | if print_warnings: 91 | print("WARNING: Actuator Rate Limit Reached") 92 | 93 | # Calculate forces and torque 94 | Fz = np.sin(self.state["theta"]) * np.sin(input_angle) * thrust 95 | Fx = np.cos(self.state["theta"]) * np.cos(input_angle) * thrust 96 | torque = np.sin(input_angle) * thrust * vehicle.com_to_tvc 97 | 98 | # Update actuator state and last input angle 99 | self.actuator_state = input_angle 100 | self.last_input_angle = input_angle 101 | 102 | return [Fx, Fz, torque] 103 | 104 | if __name__ == "__main__": 105 | # Define initial state vector with zeros 106 | initial_state = { 107 | "ax": 0, "vx": 0, "px": 0, 108 | "az": 0, "vz": 0, "pz": 0, 109 | "alpha": 0, "omega": 0, "theta": 0 110 | } 111 | 112 | # Create a ThreeDofPhysics instance with mass=1 and moment of inertia=0.1 113 | physics_simulation = ThreeDofPhysics(initial_state, 1, 0.1) 114 | 115 | # Apply forces and print the updated state for 10 time steps 116 | for _ in range(10): 117 | updated_state = physics_simulation.apply_forces([10, 1, 1], 1) 118 | print(updated_state) 119 | --------------------------------------------------------------------------------