Skip to content

Navigation Menu

Sign in
Appearance settings

Search code, repositories, users, issues, pull requests...

Provide feedback

We read every piece of feedback, and take your input very seriously.

Saved searches

Use saved searches to filter your results more quickly
Sign up
Appearance settings

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)

AltStyle によって変換されたページ (->オリジナル) /