1
1
import pigpio
2
2
import threading
3
- from time import sleep
3
+ from time import sleep , time
4
4
import logging
5
5
6
6
from rotary_encoder .motorencoder import MotorEncoder
@@ -90,7 +90,8 @@ def control(self, power_left=100, power_right=100, time_elapse=None, target_dist
90
90
""" Motor time control allows the motors
91
91
to run for a certain amount of time """
92
92
def control_time (self , power_left = 100 , power_right = 100 , time_elapse = - 1 ):
93
- #self._wheelsAxle_lock.acquire() # wheelsAxle lock acquire
93
+ if time_elapse > 0 :
94
+ return self .control_time_encoder (power_left , power_right , time_elapse )
94
95
95
96
# applying tension to motors
96
97
self ._left_motor .control (power_left , - 1 )
@@ -103,6 +104,92 @@ def control_time(self, power_left=100, power_right=100, time_elapse=-1):
103
104
sleep (time_elapse )
104
105
self .stop ()
105
106
107
+ """ Motor time control allows the motors
108
+ to run for a certain amount of time """
109
+ def control_time_encoder (self , power_left = 100 , power_right = 100 , time_elapse = - 1 ):
110
+ #self._wheelsAxle_lock.acquire() # wheelsAxle lock acquire
111
+ self ._is_moving = True
112
+
113
+ # get desired direction from power, then normalize on power > 0
114
+ left_direction = power_left / abs (power_left )
115
+ right_direction = power_right / abs (power_right )
116
+ power_left = abs (power_left )
117
+ power_right = abs (power_right )
118
+
119
+ self ._left_motor .reset_state ()
120
+ self ._right_motor .reset_state ()
121
+
122
+ # applying tension to motors
123
+ self ._left_motor .control (power_left * left_direction )
124
+ self ._right_motor .control (power_right * right_direction )
125
+
126
+ #PID parameters
127
+ # assuming that power_right is equal to power_left and that coderbot
128
+ # moves at 11.5mm/s at full PWM duty cycle
129
+
130
+ target_speed_left = (self .pid_max_speed / 100 ) * power_left #velocity [mm/s]
131
+ target_speed_right = (self .pid_max_speed / 100 ) * power_right # velocity [mm/s]
132
+
133
+ left_derivative_error = 0
134
+ right_derivative_error = 0
135
+ left_integral_error = 0
136
+ right_integral_error = 0
137
+ left_prev_error = 0
138
+ right_prev_error = 0
139
+ time_init = time ()
140
+
141
+ # moving for certaing amount of distance
142
+ logging .info ("moving? " + str (self ._is_moving ) + " distance: " + str (self .distance ()) + " target: " + str (time_elapse ))
143
+ while (time () - time_init < time_elapse and self ._is_moving == True ):
144
+ # PI controller
145
+ logging .debug ("speed.left: " + str (self ._left_motor .speed ()) + " speed.right: " + str (self ._right_motor .speed ()))
146
+ if (abs (self ._left_motor .speed ()) > 10 and abs (self ._right_motor .speed ()) > 10 ):
147
+ # relative error
148
+ left_error = float (target_speed_left - self ._left_motor .speed ()) / target_speed_left
149
+ right_error = float (target_speed_right - self ._right_motor .speed ()) / target_speed_right
150
+
151
+ left_correction = (left_error * self .pid_kp ) + (left_derivative_error * self .pid_kd ) + (left_integral_error * self .pid_ki )
152
+ right_correction = (right_error * self .pid_kp ) + (right_derivative_error * self .pid_kd ) + (right_integral_error * self .pid_ki )
153
+
154
+ corrected_power_left = power_left + (left_correction * power_left )
155
+ corrected_power_right = power_right + (right_correction * power_right )
156
+
157
+ #print("LEFT correction: %f" % (left_error * KP + left_derivative_error * KD + left_integral_error * KI))
158
+ #print("RIGHT correction: %f" % (right_error * KP + right_derivative_error * KD + right_integral_error * KI))
159
+
160
+ # conrispondent new power
161
+ power_left_norm = max (min (corrected_power_left , power_left ), 0 )
162
+ power_right_norm = max (min (corrected_power_right , power_right ), 0 )
163
+
164
+ logging .debug ("ls:" + str (int (self ._left_motor .speed ())) + " rs: " + str (int (self ._right_motor .speed ())) +
165
+ " le:" + str (left_error ) + " re: " + str (right_error ) +
166
+ " ld:" + str (left_derivative_error ) + " rd: " + str (right_derivative_error ) +
167
+ " li:" + str (left_integral_error ) + " ri: " + str (right_integral_error ) +
168
+ " lc: " + str (int (left_correction )) + " rc: " + str (int (right_correction )) +
169
+ " lp: " + str (int (power_left_norm )) + " rp: " + str (int (power_right_norm )))
170
+
171
+ # adjusting power on each motors
172
+ self ._left_motor .adjust_power (power_left_norm * left_direction )
173
+ self ._right_motor .adjust_power (power_right_norm * right_direction )
174
+
175
+ left_derivative_error = (left_error - left_prev_error ) / self .pid_sample_time
176
+ right_derivative_error = (right_error - right_prev_error ) / self .pid_sample_time
177
+ left_integral_error += (left_error * self .pid_sample_time )
178
+ right_integral_error += (right_error * self .pid_sample_time )
179
+
180
+ left_prev_error = left_error
181
+ right_prev_error = right_error
182
+
183
+ # checking each SAMPLETIME seconds
184
+ sleep (self .pid_sample_time )
185
+
186
+ logging .info ("control_distance.stop, target elapse: " + str (time_elapse ) +
187
+ " actual distance: " + str (self .distance ()) +
188
+ " l ticks: " + str (self ._left_motor .ticks ()) +
189
+ " r ticks: " + str (self ._right_motor .ticks ()))
190
+ # robot arrived
191
+ self .stop ()
192
+
106
193
""" Motor distance control allows the motors
107
194
to run for a certain amount of distance (mm) """
108
195
def control_distance (self , power_left = 100 , power_right = 100 , target_distance = 0 ):
0 commit comments