Skip to content

Commit 6ab6d2b

Browse files
committed
add encoder to control_time (with t > 0)
1 parent b8023a0 commit 6ab6d2b

File tree

4 files changed

+117
-22
lines changed

4 files changed

+117
-22
lines changed

coderbot/main.py

Lines changed: 1 addition & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -68,15 +68,7 @@ def run_server():
6868
try:
6969
app.bot_config = Config.read()
7070

71-
bot = CoderBot.get_instance(motor_trim_factor=float(app.bot_config.get('move_motor_trim', 1.0)),
72-
motor_max_power=int(app.bot_config.get('motor_max_power', 100)),
73-
motor_min_power=int(app.bot_config.get('motor_min_power', 0)),
74-
hw_version=app.bot_config.get('hardware_version'),
75-
pid_params=(float(app.bot_config.get('pid_kp', 1.0)),
76-
float(app.bot_config.get('pid_kd', 0.1)),
77-
float(app.bot_config.get('pid_ki', 0.01)),
78-
float(app.bot_config.get('pid_max_speed', 200)),
79-
float(app.bot_config.get('pid_sample_time', 0.01))))
71+
bot = CoderBot.get_instance()
8072

8173
try:
8274
audio_device = Audio.get_instance()

coderbot/rotary_encoder/wheelsaxel.py

Lines changed: 89 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,6 @@
11
import pigpio
22
import threading
3-
from time import sleep
3+
from time import sleep, time
44
import logging
55

66
from rotary_encoder.motorencoder import MotorEncoder
@@ -90,7 +90,8 @@ def control(self, power_left=100, power_right=100, time_elapse=None, target_dist
9090
""" Motor time control allows the motors
9191
to run for a certain amount of time """
9292
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)
9495

9596
# applying tension to motors
9697
self._left_motor.control(power_left, -1)
@@ -103,6 +104,92 @@ def control_time(self, power_left=100, power_right=100, time_elapse=-1):
103104
sleep(time_elapse)
104105
self.stop()
105106

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+
106193
""" Motor distance control allows the motors
107194
to run for a certain amount of distance (mm) """
108195
def control_distance(self, power_left=100, power_right=100, target_distance=0):

coderbot/v1.yml

Lines changed: 19 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -446,7 +446,7 @@ paths:
446446
tags:
447447
- Direct control
448448
requestBody:
449-
description: Movement speed and duration
449+
description: Movement speed and duration or distance
450450
required: true
451451
content:
452452
application/json:
@@ -464,12 +464,14 @@ paths:
464464
tags:
465465
- Direct control
466466
requestBody:
467-
description: Movement Speed and duration
467+
description: Movement Speed and duration or distance
468468
required: true
469469
content:
470470
application/json:
471471
schema:
472-
$ref: '#/components/schemas/TurnParams'
472+
oneOf:
473+
- $ref: '#/components/schemas/TurnParamsElapse'
474+
- $ref: '#/components/schemas/TurnParamsDistance'
473475
responses:
474476
200:
475477
description: Sent command to the bot GPIO.
@@ -629,7 +631,7 @@ components:
629631
required:
630632
- speed
631633
- distance
632-
TurnParams:
634+
TurnParamsElapse:
633635
type: object
634636
properties:
635637
speed:
@@ -642,14 +644,25 @@ components:
642644
minimum: -1.0
643645
maximum: 1.0
644646
description: Duration of the movement. 0 doesn't move the coderbot.
647+
required:
648+
- speed
649+
- elapse
650+
TurnParamsDistance:
651+
type: object
652+
properties:
653+
speed:
654+
type: number
655+
minimum: -100.0
656+
maximum: 100.0
657+
description: 0 to 100 represent a forward movement (100 being the maximum speed), while 0 to -100 is a backward movement (-100 being the maximu speed)
645658
distance:
646659
type: number
647660
minimum: 0.0
648-
maximum: 1000.0
661+
maximum: 100000.0
649662
description: Target distqnce in mm.
650663
required:
651664
- speed
652-
- elapse
665+
- distance
653666
Settings:
654667
type: object
655668
Photo:

defaults/config.json

Lines changed: 8 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -36,17 +36,20 @@
3636
"ctrl_hud_image":"",
3737
"load_at_start":"",
3838
"sound_start":"$startup",
39-
"hw_version":"5",
39+
"hardware_version":"5",
4040
"audio_volume_level":"100",
4141
"wifi_mode":"ap",
4242
"wifi_ssid":"coderbot",
4343
"wifi_psk":"coderbot",
4444
"packages_installed":"",
4545
"admin_password":"",
4646
"hardware_version":"5",
47-
"pid_kp":"0.9",
48-
"pid_kd":"0.1",
49-
"pid_ki":"0.01",
47+
"pid_kp":"8.0",
48+
"pid_kd":"0.0",
49+
"pid_ki":"0.0",
5050
"pid_max_speed":"200",
51-
"pid_sample_time":"0.01"
51+
"pid_sample_time":"0.05",
52+
"movement_use_mpu": "false",
53+
"movement_use_motion": "false",
54+
"movement_use_encoder": "true"
5255
}

0 commit comments

Comments
 (0)