-
Notifications
You must be signed in to change notification settings - Fork 5
/
dcmotor_sim.py
147 lines (121 loc) · 4.64 KB
/
dcmotor_sim.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
# Libraries
import numpy as np
import matplotlib.pyplot as plt
from pid_controller import PIDController
from lpsb import trajectory_planner
# DC Motor class
class Dc_motor():
def __init__(self):
self.V_max = 24.
self.V = 24.
self.R = 10.
self.L = 0.24
self.b = 0.000001
self.kb = 0.02
self.kt = self.kb
self.I_motor = 10.0
self.I_load = 0.0
self.delta_t = 0.01
self.load_torque = 0.0
self.N = 1.0
self.n = 1.0
self.i_phase = [0.]
self.ang_speed = [0.]
self.theta = [0.]
self.theta_output = [0.]
self.control = False
self.theta_desired = 0.
def set_motor_param(self,R,L,b,kb,kt,I_motor):
self.R, self.L, self.b, self.kb, self.kt, self.I_motor = R,L,b,kb,kt, I_motor
def set_load_inertia(self,load):
self.I_load = load
def set_sample_time(self,delta_t):
self.delta_t = delta_t
def set_load_torque(self,load_torque):
self.load_torque = load_torque
def set_gearbox_param(self,n, effi):
self.N = n
self.n = effi
def set_voltage(self,V):
self.V = V
def check_params(self):
print('No Load Response: {}'.format(self.V/self.kb))
print('Stall Torque: {}'.format(self.kt*(self.V/self.R)))
print('Time Constant: {:2f}'.format(self.L/self.R))
def set_duration(self,duration):
self.duration = duration
def set_controller(self):
self.control = True
def motor_path(self,omega_desired):
self.omega_desired = omega_desired
def update(self,pid):
time_steps = int(self.duration/self.delta_t)
print(time_steps)
for i in range(1,time_steps):
# Current
pid.setTarget(self.omega_desired[i])
self.i_phase.append((self.delta_t/self.L)*(self.V - self.i_phase[i-1]*self.R - self.kb*self.ang_speed[i-1]) + self.i_phase[i-1])
if self.i_phase[i]>= self.V_max/self.R:
self.i_phase[i] = self.V_max/self.R
elif self.i_phase[i]<= -self.V_max/self.R:
self.i_phase[i] = -self.V_max/self.R
# Angular Speed
self.ang_speed.append((self.delta_t/(self.I_motor + self.I_load/(self.N**2))*(self.kt*self.i_phase[i-1]) - self.load_torque/(self.n*self.N) - self.b*self.ang_speed[i-1]) + self.ang_speed[i-1])
if self.ang_speed[i] <= -self.V_max/self.kb:
self.ang_speed[i] = -self.V_max/self.kb
elif self.ang_speed[i] >= self.V_max/self.kb:
self.ang_speed[i] = self.V_max/self.kb
self.theta.append((self.delta_t)*self.ang_speed[i-1] +self.theta[i-1])
self.theta_output.append(self.theta[i]*self.N)
time = delta_t*i
if self.control:
self.V = pid.update(self.ang_speed[i],time)
def plot_graph(self):
fig,ax = plt.subplots(2,2)
time = delta_t* np.array(range(len(self.i_phase)))
ax[0,0].plot(time, self.i_phase)
ax[0,0].set_xlabel('Time (seconds)')
ax[0,0].set_ylabel('Current (A)')
ax[1,0].plot(time, self.ang_speed)
ax[1,0].plot(time, self.omega_desired[:len(self.ang_speed)], linestyle = '--', color = 'r')
ax[1,0].set_xlabel('Time (seconds)')
ax[1,0].set_ylabel('Angular Velocity (rad/sec)')
ax[0,1].plot(time, self.theta_output)
ax[0,1].set_xlabel('Time (seconds)')
ax[0,1].set_ylabel('Angular Position (rads)')
ax[1,1].plot(self.ang_speed, self.kt*np.array(self.i_phase))
ax[1,1].set_xlabel('Angular Velocity (rad/sec)')
ax[1,1].set_ylabel('Torque (Nm)')
plt.show()
if __name__ == '__main__':
# Parameters
V = 24
R = 10
L = 0.24
b = 0.000001
kb = 0.02
kt = 0.02
I_motor = 9e-6
I_load = 0.
N = 1
n = 1
delta_t = 0.001
T_load = 0
motor_1: Dc_motor = Dc_motor()
motor_1.set_motor_param(R,L,b,kb,kt,I_motor)
motor_1.set_load_torque(T_load)
motor_1.set_voltage(V)
motor_1.set_duration(40.0)
motor_1.set_controller()
motor_1.set_sample_time(delta_t)
pid = PIDController(kp = 0.12, ki = 3, kd = 0.0001, max_windup = 20,
start_time = 0, alpha = 0.75, u_bounds = [-V, V])
Q_matrix = np.matrix(np.array([10,1000,3000,4000,3000]))
time = np.array([10,10,10,10])
acceleration = np.array([40,35,60,50,50])
theta_desired,omega_desired = trajectory_planner(Q_matrix,time,acceleration,delta_t)
motor_1.motor_path(omega_desired[0])
#print(len(omega_desired[0]))
pid.setTarget(800)
motor_1.update(pid)
motor_1.plot_graph()