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 b8023a0

Browse files
committed
add motor min max power
modify PID
1 parent bd5450f commit b8023a0

File tree

9 files changed

+119
-74
lines changed

9 files changed

+119
-74
lines changed

‎coderbot/api.py

Lines changed: 22 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -3,36 +3,40 @@
33
This file contains every method called by the API defined in v2.yml
44
"""
55

6+
import logging
67
import os
78
import subprocess
8-
import logging
9-
import connexion
10-
from werkzeug.datastructures import Headers
11-
from flask import (request,
12-
send_file,
13-
Response)
14-
import picamera
159
import urllib
10+
1611
import connexion
12+
import picamera
13+
from flask import Response, request, send_file
14+
from werkzeug.datastructures import Headers
1715

18-
from program import ProgramEngine, Program
1916
from config import Config
2017
from activity import Activities
18+
from audio import Audio
2119
from camera import Camera
2220
from cnn.cnn_manager import CNNManager
23-
from musicPackages import MusicPackageManager
24-
from audio import Audio
2521
from coderbotTestUnit import run_test as runCoderbotTestUnit
26-
from coderbot import CoderBot
22+
from musicPackages import MusicPackageManager
23+
from program import Program, ProgramEngine
24+
2725
from balena import Balena
26+
from coderbot import CoderBot
2827

2928
BUTTON_PIN = 16
3029

3130
config = Config.read()
32-
bot = CoderBot.get_instance(
33-
motor_trim_factor=float(config.get("move_motor_trim", 1.0)),
34-
hw_version=config.get("hw_version")
35-
)
31+
bot = CoderBot.get_instance(motor_trim_factor=float(config.get('move_motor_trim', 1.0)),
32+
motor_max_power=int(config.get('motor_max_power', 100)),
33+
motor_min_power=int(config.get('motor_min_power', 0)),
34+
hw_version=config.get('hardware_version'),
35+
pid_params=(float(config.get('pid_kp', 1.0)),
36+
float(config.get('pid_kd', 0.1)),
37+
float(config.get('pid_ki', 0.01)),
38+
float(config.get('pid_max_speed', 200)),
39+
float(config.get('pid_sample_time', 0.01))))
3640
audio_device = Audio.get_instance()
3741
cam = Camera.get_instance()
3842

@@ -125,9 +129,10 @@ def move(body):
125129
def turn(body):
126130
speed=body.get("speed")
127131
elapse=body.get("elapse")
128-
if speed is None or speed == 0:
132+
distance=body.get("distance")
133+
if (speed is None or speed == 0) or (elapse is not None and distance is not None):
129134
return 400
130-
bot.turn(speed=speed, elapse=elapse)
135+
bot.turn(speed=speed, elapse=elapse, distance=distance)
131136
return 200
132137

133138
def takePhoto():

‎coderbot/coderbot.py

Lines changed: 19 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -20,6 +20,7 @@
2020
import os
2121
import sys
2222
import time
23+
from math import copysign
2324
import logging
2425
import pigpio
2526
import sonar
@@ -100,7 +101,7 @@ class CoderBot(object):
100101

101102
# pylint: disable=too-many-instance-attributes
102103

103-
def __init__(self, motor_trim_factor=1.0, hw_version="5"):
104+
def __init__(self, motor_trim_factor=1.0, motor_min_power=0, motor_max_power=100, hw_version="5", pid_params=(0.8, 0.1, 0.01, 200, 0.01)):
104105
try:
105106
self._mpu = mpu.AccelGyroMag()
106107
logging.info("MPU available")
@@ -116,6 +117,8 @@ def __init__(self, motor_trim_factor=1.0, hw_version="5"):
116117
self._cb_elapse = dict()
117118
self._encoder = self.GPIOS.HAS_ENCODER
118119
self._motor_trim_factor = motor_trim_factor
120+
self._motor_min_power = motor_min_power
121+
self._motor_max_power = motor_max_power
119122
self._twin_motors_enc = WheelsAxel(
120123
self.pi,
121124
enable_pin=self.GPIOS.PIN_MOTOR_ENABLE,
@@ -126,7 +129,8 @@ def __init__(self, motor_trim_factor=1.0, hw_version="5"):
126129
right_forward_pin=self.GPIOS.PIN_RIGHT_FORWARD,
127130
right_backward_pin=self.GPIOS.PIN_RIGHT_BACKWARD,
128131
right_encoder_feedback_pin_A=self.GPIOS.PIN_ENCODER_RIGHT_A,
129-
right_encoder_feedback_pin_B=self.GPIOS.PIN_ENCODER_RIGHT_B)
132+
right_encoder_feedback_pin_B=self.GPIOS.PIN_ENCODER_RIGHT_B,
133+
pid_params=pid_params)
130134
self.motor_control = self._dc_enc_motor
131135

132136
self._cb1 = self.pi.callback(self.GPIOS.PIN_PUSHBUTTON, pigpio.EITHER_EDGE, self._cb_button)
@@ -153,21 +157,23 @@ def exit(self):
153157
s.cancel()
154158

155159
@classmethod
156-
def get_instance(cls, motor_trim_factor=1.0, hw_version="5", servo=False):
160+
def get_instance(cls, motor_trim_factor=1.0, motor_max_power=100, motor_min_power=0, hw_version="5", pid_params=(0.8, 0.1, 0.01, 200, 0.01)):
157161
if not cls.the_bot:
158-
cls.the_bot = CoderBot(motor_trim_factor=motor_trim_factor, hw_version=hw_version)
162+
cls.the_bot = CoderBot(motor_trim_factor=motor_trim_factor, motor_max_power=motor_max_power, motor_min_power=motor_min_power, hw_version=hw_version, pid_params=pid_params)
159163
return cls.the_bot
160164

165+
def get_motor_power(self, speed):
166+
return int(copysign(min(max(((self._motor_max_power - self._motor_min_power) * abs(speed) / 100) + self._motor_min_power, self._motor_min_power), self._motor_max_power), speed))
167+
161168
def move(self, speed=100, elapse=None, distance=None):
162-
self._motor_trim_factor = 1.0
163-
speed_left = min(100, max(-100, speed * self._motor_trim_factor))
164-
speed_right = min(100, max(-100, speed / self._motor_trim_factor))
169+
speed_left = speed * self._motor_trim_factor
170+
speed_right = speed / self._motor_trim_factor
165171
self.motor_control(speed_left=speed_left, speed_right=speed_right, time_elapse=elapse, target_distance=distance)
166172

167-
def turn(self, speed=100, elapse=-1):
168-
speed_left = min(100, max(-100, speed * self._motor_trim_factor))
169-
speed_right = -min(100, max(-100, speed / self._motor_trim_factor))
170-
self.motor_control(speed_left=speed_left, speed_right=speed_right, time_elapse=elapse)
173+
def turn(self, speed=100, elapse=None, distance=None):
174+
speed_left = speed * self._motor_trim_factor
175+
speed_right = -speed / self._motor_trim_factor
176+
self.motor_control(speed_left=speed_left, speed_right=speed_right, time_elapse=elapse, target_distance=distance)
171177

172178
def turn_angle(self, speed=100, angle=0):
173179
z = self._mpu.get_gyro()[2]
@@ -225,8 +231,8 @@ def _servo_control(self, pin, angle):
225231
self.pi.set_PWM_dutycycle(pin, duty)
226232

227233
def _dc_enc_motor(self, speed_left=100, speed_right=100, time_elapse=None, target_distance=None):
228-
self._twin_motors_enc.control(power_left=speed_left,
229-
power_right=speed_right,
234+
self._twin_motors_enc.control(power_left=self.get_motor_power(speed_left),
235+
power_right=self.get_motor_power(speed_right),
230236
time_elapse=time_elapse,
231237
target_distance=target_distance)
232238

‎coderbot/coderbotTestUnit.py

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -12,13 +12,13 @@
1212
If no test was executed on that component, 0 is preserved.
1313
"""
1414
from coderbot import CoderBot
15-
c = CoderBot.get_instance()
1615

1716
# Single components tests
1817

1918
# encoder motors
2019
def __test_encoder():
2120
try:
21+
c = CoderBot.get_instance()
2222
# moving both wheels at speed 100 clockwise
2323
print("moving both wheels at speed 100 clockwise")
2424
assert(c.speed() == 0)

‎coderbot/main.py

Lines changed: 12 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -22,10 +22,10 @@
2222
# Logging configuration
2323
logger = logging.getLogger()
2424
logger.setLevel(os.environ.get("LOGLEVEL", "INFO"))
25-
sh = logging.StreamHandler()
26-
formatter = logging.Formatter('%(message)s')
27-
sh.setFormatter(formatter)
28-
logger.addHandler(sh)
25+
# sh = logging.StreamHandler()
26+
# formatter = logging.Formatter('%(message)s')
27+
# sh.setFormatter(formatter)
28+
# logger.addHandler(sh)
2929

3030
## (Connexion) Flask app configuration
3131

@@ -69,7 +69,14 @@ def run_server():
6969
app.bot_config = Config.read()
7070

7171
bot = CoderBot.get_instance(motor_trim_factor=float(app.bot_config.get('move_motor_trim', 1.0)),
72-
hw_version=app.bot_config.get('hardware_version'))
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))))
7380

7481
try:
7582
audio_device = Audio.get_instance()

‎coderbot/rotary_encoder/motorencoder.py

Lines changed: 5 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -156,18 +156,19 @@ def rotary_callback(self, tick):
156156

157157
# taking groups of n ticks each
158158
if (self._ticks_counter == 0):
159-
self._start_timer = time() # clock started
159+
self._start_timer = tick # clock started
160160
elif (abs(self._ticks_counter) == self._ticks_threshold):
161-
self._current_timer = time()
162-
elapse = self._current_timer - self._start_timer # calculating time elapse
161+
self._current_timer = tick
162+
elapse = (self._current_timer - self._start_timer) /1000000.0 # calculating time elapse
163163
# calculating current speed
164164
self._encoder_speed = self._ticks_threshold * self._distance_per_tick / elapse # (mm/s)
165165

166-
self._ticks += tick # updating ticks
166+
self._ticks += 1 # updating ticks
167167

168168
if(abs(self._ticks_counter) < self._ticks_threshold):
169169
self._ticks_counter += 1
170170
else:
171+
self._start_timer = tick # clock started
171172
self._ticks_counter = 0
172173

173174
# updating ticks counter using module

‎coderbot/rotary_encoder/rotarydecoder.py

Lines changed: 5 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -27,8 +27,8 @@ def __init__(self, pi, feedback_pin_A, feedback_pin_B, callback):
2727
self._pi.set_pull_up_down(feedback_pin_B, pigpio.PUD_UP)
2828

2929
# callback function on EITHER_EDGE for each pin
30-
self._callback_triggerA = self._pi.callback(feedback_pin_A, pigpio.EITHER_EDGE, self._pulse)
31-
self._callback_triggerB = self._pi.callback(feedback_pin_B, pigpio.EITHER_EDGE, self._pulse)
30+
self._callback_triggerA = self._pi.callback(feedback_pin_A, pigpio.EITHER_EDGE, self._pulseA)
31+
self._callback_triggerB = self._pi.callback(feedback_pin_B, pigpio.EITHER_EDGE, self._pulseA)
3232

3333
self._lastGpio = None
3434

@@ -91,6 +91,9 @@ def _pulse(self, gpio, level, tick):
9191
self._lock.release()
9292
self._callback(direction)
9393

94+
def _pulseA(self, gpio, level, tick):
95+
self._callback(tick)
96+
9497
def cancel(self):
9598

9699
"""

‎coderbot/rotary_encoder/wheelsaxel.py

Lines changed: 42 additions & 30 deletions
Original file line numberDiff line numberDiff line change
@@ -16,7 +16,8 @@ class WheelsAxel:
1616

1717
def __init__(self, pi, enable_pin,
1818
left_forward_pin, left_backward_pin, left_encoder_feedback_pin_A, left_encoder_feedback_pin_B,
19-
right_forward_pin, right_backward_pin, right_encoder_feedback_pin_A, right_encoder_feedback_pin_B):
19+
right_forward_pin, right_backward_pin, right_encoder_feedback_pin_A, right_encoder_feedback_pin_B,
20+
pid_params):
2021

2122
# state variables
2223
self._is_moving = False
@@ -36,6 +37,12 @@ def __init__(self, pi, enable_pin,
3637
right_encoder_feedback_pin_A,
3738
right_encoder_feedback_pin_B)
3839

40+
self.pid_kp = pid_params[0]
41+
self.pid_kd = pid_params[1]
42+
self.pid_ki = pid_params[2]
43+
self.pid_max_speed = pid_params[3]
44+
self.pid_sample_time = pid_params[4]
45+
3946
# other
4047
#self._wheelsAxle_lock = threading.RLock() # race condition lock
4148

@@ -118,70 +125,75 @@ def control_distance(self, power_left=100, power_right=100, target_distance=0):
118125
#PID parameters
119126
# assuming that power_right is equal to power_left and that coderbot
120127
# moves at 11.5mm/s at full PWM duty cycle
121-
MAX_SPEED=180
122-
target_speed_left = (MAX_SPEED / 100) * power_left #velocity [mm/s]
123-
target_speed_right = (MAX_SPEED / 100) * power_right # velocity [mm/s]
128+
129+
target_speed_left = (self.pid_max_speed / 100) * power_left #velocity [mm/s]
130+
target_speed_right = (self.pid_max_speed / 100) * power_right # velocity [mm/s]
124131

125132
# SOFT RESPONSE
126-
#KP = 0.04 #proportional coefficient
127-
#KD = 0.02 # derivative coefficient
128-
#KI = 0.005 # integral coefficient
133+
#KP = 0.04 #proportional coefficient
134+
#KD = 0.02 # derivative coefficient
135+
#KI = 0.005 # integral coefficient
129136

130137
# MEDIUM RESPONSE
131-
KP = 0.4 #proportional coefficient
132-
KD = 0.1 # derivative coefficient
133-
KI = 0.02 # integral coefficient
138+
# KP = 0.9 #proportional coefficient
139+
# KD = 0.1 # derivative coefficient
140+
# KI = 0.05 # integral coefficient
134141

135142
# STRONG RESPONSE
136-
#KP = 0.9 # proportional coefficient
137-
#KD = 0.05 # derivative coefficient
138-
#KI = 0.03 # integral coefficient
139-
140-
SAMPLETIME = 0.01
143+
# KP = 0.9 # proportional coefficient
144+
# KD = 0.05 # derivative coefficient
145+
# KI = 0.03 # integral coefficient
141146

142147
left_derivative_error = 0
143148
right_derivative_error = 0
144149
left_integral_error = 0
145150
right_integral_error = 0
151+
left_prev_error = 0
152+
right_prev_error = 0
146153
# moving for certaing amount of distance
147154
logging.info("moving? " + str(self._is_moving) + " distance: " + str(self.distance()) + " target: " + str(target_distance))
148155
while(abs(self.distance()) < abs(target_distance) and self._is_moving == True):
149156
# PI controller
150157
logging.debug("speed.left: " + str(self._left_motor.speed()) + " speed.right: " + str(self._right_motor.speed()))
151158
if(abs(self._left_motor.speed()) > 10 and abs(self._right_motor.speed()) > 10):
152159
# relative error
153-
left_error = (target_speed_left - self._left_motor.speed()) / target_speed_left*100.0
154-
right_error = (target_speed_right - self._right_motor.speed()) / target_speed_right*100.0
160+
left_error = float(target_speed_left - self._left_motor.speed()) / target_speed_left
161+
right_error = float(target_speed_right - self._right_motor.speed()) / target_speed_right
155162

156-
left_correction = (left_error * KP) + (left_derivative_error * KD) + (left_integral_error * KI)
157-
right_correction = (right_error * KP) + (right_derivative_error * KD) + (right_integral_error * KI)
163+
left_correction = (left_error * self.pid_kp) + (left_derivative_error * self.pid_kd) + (left_integral_error * self.pid_ki)
164+
right_correction = (right_error * self.pid_kp) + (right_derivative_error * self.pid_kd) + (right_integral_error * self.pid_ki)
158165

159-
corrected_power_left = power_left + left_correction -right_correction
160-
corrected_power_right = power_right + right_correction -left_correction
166+
corrected_power_left = power_left + (left_correction *power_left)
167+
corrected_power_right = power_right + (right_correction *power_right)
161168

162169
#print("LEFT correction: %f" % (left_error * KP + left_derivative_error * KD + left_integral_error * KI))
163170
#print("RIGHT correction: %f" % (right_error * KP + right_derivative_error * KD + right_integral_error * KI))
164171

165172
# conrispondent new power
166-
power_left_norm = max(min(corrected_power_left, 100), 0)
167-
power_right_norm = max(min(corrected_power_right, 100), 0)
173+
power_left_norm = max(min(corrected_power_left, power_left), 0)
174+
power_right_norm = max(min(corrected_power_right, power_right), 0)
168175

169-
logging.info("ls:" + str(int(self._left_motor.speed())) + " rs: " + str(int(self._right_motor.speed())) +
170-
" le:" + str(int(left_error)) + " re: " + str(int(right_error)) +
176+
logging.debug("ls:" + str(int(self._left_motor.speed())) + " rs: " + str(int(self._right_motor.speed())) +
177+
" le:" + str(left_error) + " re: " + str(right_error) +
178+
" ld:" + str(left_derivative_error) + " rd: " + str(right_derivative_error) +
179+
" li:" + str(left_integral_error) + " ri: " + str(right_integral_error) +
171180
" lc: " + str(int(left_correction)) + " rc: " + str(int(right_correction)) +
172181
" lp: " + str(int(power_left_norm)) + " rp: " + str(int(power_right_norm)))
173182

174183
# adjusting power on each motors
175184
self._left_motor.adjust_power(power_left_norm * left_direction )
176185
self._right_motor.adjust_power(power_right_norm * right_direction)
177186

178-
left_derivative_error = left_error
179-
right_derivative_error = right_error
180-
left_integral_error += left_error
181-
right_integral_error += right_error
187+
left_derivative_error = (left_error - left_prev_error) / self.pid_sample_time
188+
right_derivative_error = (right_error - right_prev_error) / self.pid_sample_time
189+
left_integral_error += (left_error * self.pid_sample_time)
190+
right_integral_error += (right_error * self.pid_sample_time)
191+
192+
left_prev_error = left_error
193+
right_prev_error = right_error
182194

183195
# checking each SAMPLETIME seconds
184-
sleep(SAMPLETIME)
196+
sleep(self.pid_sample_time)
185197

186198
logging.info("control_distance.stop, target dist: " + str(target_distance) +
187199
" actual distance: " + str(self.distance()) +

‎coderbot/v1.yml

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -642,6 +642,11 @@ components:
642642
minimum: -1.0
643643
maximum: 1.0
644644
description: Duration of the movement. 0 doesn't move the coderbot.
645+
distance:
646+
type: number
647+
minimum: 0.0
648+
maximum: 1000.0
649+
description: Target distqnce in mm.
645650
required:
646651
- speed
647652
- elapse

0 commit comments

Comments
(0)

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