Skip to content

Commit a769bb4

Browse files
previprevi
committed
wip mpu
1 parent 4380cd5 commit a769bb4

File tree

4 files changed

+92
-61
lines changed

4 files changed

+92
-61
lines changed

‎coderbot.py

Lines changed: 81 additions & 51 deletions
Original file line numberDiff line numberDiff line change
@@ -26,32 +26,59 @@
2626
from rotary_encoder.wheelsaxel import WheelsAxel
2727

2828
# GPIO
29-
# motors
30-
PIN_MOTOR_ENABLE = None #22
31-
PIN_LEFT_FORWARD = 17 #25
32-
PIN_LEFT_BACKWARD = 18 # 24
33-
PIN_RIGHT_FORWARD = 22 # 4
34-
PIN_RIGHT_BACKWARD = 23 #17
35-
#?
36-
PIN_PUSHBUTTON = 16 #11
37-
# servo
38-
PIN_SERVO_3 = 7 #9
39-
PIN_SERVO_4 = 1 #10
40-
# sonar
41-
PIN_SONAR_1_TRIGGER = 5 #18
42-
PIN_SONAR_1_ECHO = 27 #7
43-
PIN_SONAR_2_TRIGGER = 5 #18
44-
PIN_SONAR_2_ECHO = 6 #8
45-
PIN_SONAR_3_TRIGGER = 5 #18
46-
PIN_SONAR_3_ECHO = 12 #23
47-
PIN_SONAR_4_TRIGGER = 5 #18
48-
PIN_SONAR_4_ECHO = 13 #23
49-
50-
# encoder
51-
PIN_ENCODER_LEFT_A = 14
52-
PIN_ENCODER_LEFT_B = 15 #6
53-
PIN_ENCODER_RIGHT_A = 24 #15
54-
PIN_ENCODER_RIGHT_B = 25 #12
29+
class GPIO_CODERBOT_V_4():
30+
# motors
31+
PIN_MOTOR_ENABLE = 22
32+
PIN_LEFT_FORWARD = 25
33+
PIN_LEFT_BACKWARD = 24
34+
PIN_RIGHT_FORWARD = 4
35+
PIN_RIGHT_BACKWARD = 17
36+
37+
PIN_PUSHBUTTON = 11
38+
# servo
39+
PIN_SERVO_3 = 9
40+
PIN_SERVO_4 = 10
41+
# sonar
42+
PIN_SONAR_1_TRIGGER = 18
43+
PIN_SONAR_1_ECHO = 7
44+
PIN_SONAR_2_TRIGGER = 18
45+
PIN_SONAR_2_ECHO = 8
46+
PIN_SONAR_3_TRIGGER = 18
47+
PIN_SONAR_3_ECHO = 23
48+
49+
# encoder
50+
PIN_ENCODER_LEFT_A = 14
51+
PIN_ENCODER_LEFT_B = 6
52+
PIN_ENCODER_RIGHT_A = 15
53+
PIN_ENCODER_RIGHT_B = 12
54+
55+
class GPIO_CODERBOT_V_5():
56+
# motors
57+
PIN_MOTOR_ENABLE = None #22
58+
PIN_LEFT_FORWARD = 17 #25
59+
PIN_LEFT_BACKWARD = 18 # 24
60+
PIN_RIGHT_FORWARD = 22 # 4
61+
PIN_RIGHT_BACKWARD = 23 #17
62+
63+
PIN_PUSHBUTTON = 16 #11
64+
# servo
65+
PIN_SERVO_3 = 7 #9
66+
PIN_SERVO_4 = 1 #10
67+
# sonar
68+
PIN_SONAR_1_TRIGGER = 5 #18
69+
PIN_SONAR_1_ECHO = 27 #7
70+
PIN_SONAR_2_TRIGGER = 5 #18
71+
PIN_SONAR_2_ECHO = 6 #8
72+
PIN_SONAR_3_TRIGGER = 5 #18
73+
PIN_SONAR_3_ECHO = 12 #23
74+
PIN_SONAR_4_TRIGGER = 5 #18
75+
PIN_SONAR_4_ECHO = 13 #23
76+
77+
# encoder
78+
PIN_ENCODER_LEFT_A = 14
79+
PIN_ENCODER_LEFT_B = 15 #6
80+
PIN_ENCODER_RIGHT_A = 24 #15
81+
PIN_ENCODER_RIGHT_B = 25 #12
5582

5683
# PWM
5784
PWM_FREQUENCY = 100 #Hz
@@ -61,44 +88,47 @@ class CoderBot(object):
6188

6289
# pylint: disable=too-many-instance-attributes
6390

64-
_pin_out = [PIN_LEFT_FORWARD, PIN_RIGHT_FORWARD, PIN_LEFT_BACKWARD, PIN_RIGHT_BACKWARD, PIN_SERVO_3, PIN_SERVO_4]
65-
6691
def __init__(self, motor_trim_factor=1.0, encoder=True):
92+
try:
93+
self._mpu = mpu.AccelGyroMag()
94+
self.GPIOS = GPIO_CODERBOT_V_5()
95+
logging.info("MPU available")
96+
except:
97+
logging.info("MPU not available")
98+
self.GPIOS = GPIO_CODERBOT_V_4()
99+
100+
self._pin_out = [self.GPIOS.PIN_LEFT_FORWARD, self.GPIOS.PIN_RIGHT_FORWARD, self.GPIOS.PIN_LEFT_BACKWARD, self.GPIOS.PIN_RIGHT_BACKWARD, self.GPIOS.PIN_SERVO_3, self.GPIOS.PIN_SERVO_4]
67101
self.pi = pigpio.pi('localhost')
68-
self.pi.set_mode(PIN_PUSHBUTTON, pigpio.INPUT)
102+
self.pi.set_mode(self.GPIOS.PIN_PUSHBUTTON, pigpio.INPUT)
69103
self._cb = dict()
70104
self._cb_last_tick = dict()
71105
self._cb_elapse = dict()
72106
self._encoder = encoder
73107
self._motor_trim_factor = motor_trim_factor
74108
self._twin_motors_enc = WheelsAxel(
75109
self.pi,
76-
enable_pin=PIN_MOTOR_ENABLE,
77-
left_forward_pin=PIN_LEFT_FORWARD,
78-
left_backward_pin=PIN_LEFT_BACKWARD,
79-
left_encoder_feedback_pin_A=PIN_ENCODER_LEFT_A,
80-
left_encoder_feedback_pin_B=PIN_ENCODER_LEFT_B,
81-
right_forward_pin=PIN_RIGHT_FORWARD,
82-
right_backward_pin=PIN_RIGHT_BACKWARD,
83-
right_encoder_feedback_pin_A=PIN_ENCODER_RIGHT_A,
84-
right_encoder_feedback_pin_B=PIN_ENCODER_RIGHT_B)
110+
enable_pin=self.GPIOS.PIN_MOTOR_ENABLE,
111+
left_forward_pin=self.GPIOS.PIN_LEFT_FORWARD,
112+
left_backward_pin=self.GPIOS.PIN_LEFT_BACKWARD,
113+
left_encoder_feedback_pin_A=self.GPIOS.PIN_ENCODER_LEFT_A,
114+
left_encoder_feedback_pin_B=self.GPIOS.PIN_ENCODER_LEFT_B,
115+
right_forward_pin=self.GPIOS.PIN_RIGHT_FORWARD,
116+
right_backward_pin=self.GPIOS.PIN_RIGHT_BACKWARD,
117+
right_encoder_feedback_pin_A=self.GPIOS.PIN_ENCODER_RIGHT_A,
118+
right_encoder_feedback_pin_B=self.GPIOS.PIN_ENCODER_RIGHT_B)
85119
self.motor_control = self._dc_enc_motor
86120

87-
self._cb1 = self.pi.callback(PIN_PUSHBUTTON, pigpio.EITHER_EDGE, self._cb_button)
121+
self._cb1 = self.pi.callback(self.GPIOS.PIN_PUSHBUTTON, pigpio.EITHER_EDGE, self._cb_button)
88122

89123
for pin in self._pin_out:
90124
self.pi.set_PWM_frequency(pin, PWM_FREQUENCY)
91125
self.pi.set_PWM_range(pin, PWM_RANGE)
92126

93-
self.sonar = [sonar.Sonar(self.pi, PIN_SONAR_1_TRIGGER, PIN_SONAR_1_ECHO),
94-
sonar.Sonar(self.pi, PIN_SONAR_2_TRIGGER, PIN_SONAR_2_ECHO),
95-
sonar.Sonar(self.pi, PIN_SONAR_3_TRIGGER, PIN_SONAR_3_ECHO),
96-
sonar.Sonar(self.pi, PIN_SONAR_4_TRIGGER, PIN_SONAR_4_ECHO)]
127+
self.sonar = [sonar.Sonar(self.pi, self.GPIOS.PIN_SONAR_1_TRIGGER, self.GPIOS.PIN_SONAR_1_ECHO),
128+
sonar.Sonar(self.pi, self.GPIOS.PIN_SONAR_2_TRIGGER, self.GPIOS.PIN_SONAR_2_ECHO),
129+
sonar.Sonar(self.pi, self.GPIOS.PIN_SONAR_3_TRIGGER, self.GPIOS.PIN_SONAR_3_ECHO),
130+
sonar.Sonar(self.pi, self.GPIOS.PIN_SONAR_4_TRIGGER, self.GPIOS.PIN_SONAR_4_ECHO)]
97131

98-
try:
99-
self._mpu = mpu.AccelGyroMag()
100-
except:
101-
logging.info("MPU not available")
102132

103133
#self.stop()
104134
self._is_moving = False
@@ -163,22 +193,22 @@ def get_mpu_accel(self, axis=None):
163193
if axis is None:
164194
return acc
165195
else:
166-
return acc[axis]
196+
return int(acc[axis]*100.0)/100.0
167197

168198
def get_mpu_gyro(self, axis=None):
169199
gyro = self._mpu.get_gyro()
170200
if axis is None:
171201
return gyro
172202
else:
173-
return gyro[axis]
203+
return int(gyro[axis]*100.0)/200.0
174204

175205
def get_mpu_heading(self):
176206
hdg = self._mpu.get_hdg()
177-
return hdg
207+
return int(hdg)
178208

179209
def get_mpu_temp(self):
180210
temp = self._mpu.get_temp()
181-
return temp
211+
return int(temp*100.0)/100.0
182212

183213
def _servo_control(self, pin, angle):
184214
duty = ((angle + 90) * 100 / 180) + 25

‎main.py

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -22,7 +22,7 @@
2222
from flask_cors import CORS
2323
from werkzeug.datastructures import Headers
2424

25-
from coderbot import CoderBot, PIN_PUSHBUTTON
25+
from coderbot import CoderBot
2626
from camera import Camera
2727
from motion import Motion
2828
from audio import Audio
@@ -486,7 +486,7 @@ def run_server():
486486
app.bot_config = {}
487487
logging.error(e)
488488

489-
bot.set_callback(PIN_PUSHBUTTON, button_pushed, 100)
489+
bot.set_callback(bot.GPIOS.PIN_PUSHBUTTON, button_pushed, 100)
490490

491491
remove_doreset_file()
492492

‎mpu.py

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -46,9 +46,9 @@ def get_gyro(self):
4646

4747
def get_acc(self):
4848
temp, acc, gyro = self.driver.read_values()
49-
return (gyro[AccelGyroMag.X_IND],
50-
gyro[AccelGyroMag.Y_IND],
51-
gyro[AccelGyroMag.Z_IND])
49+
return (acc[AccelGyroMag.X_IND],
50+
acc[AccelGyroMag.Y_IND],
51+
acc[AccelGyroMag.Z_IND])
5252

5353
def get_hdg(self):
5454
hdg = self.driver.mag_heading()

‎mpu2.py

Lines changed: 6 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -28,18 +28,19 @@ def main(self):
2828
ag_data_ready = self.driver.read_ag_status().accelerometer_data_available
2929
if ag_data_ready:
3030
self.read_ag()
31-
self.read_magnetometer()
31+
print("")
32+
#self.read_magnetometer()
3233
count += 1
3334
time.sleep(0.05)
3435
finally:
3536
self.driver.close()
3637

3738
def read_ag(self):
3839
temp, acc, gyro = self.driver.read_values()
39-
print("Temp: %.1f °f" % temp, end='')
40-
print("Gyro Roll: %.4f, Pitch: %.4f, Yaw: %.4f" % (gyro[SimpleExample.ROLL_IND],
41-
gyro[SimpleExample.PITCH_IND],
42-
gyro[SimpleExample.YAW_IND]), end='')
40+
#print("Temp: %.1f °f" % temp, end='')
41+
#print("Gyro Roll: %.4f, Pitch: %.4f, Yaw: %.4f" % (gyro[SimpleExample.ROLL_IND],
42+
# gyro[SimpleExample.PITCH_IND],
43+
# gyro[SimpleExample.YAW_IND]), end='')
4344
print("X: %.4f, Y: %.4f, Z: %.4f" % (acc[SimpleExample.X_IND],
4445
acc[SimpleExample.Y_IND],
4546
acc[SimpleExample.Z_IND]), end='')

0 commit comments

Comments
 (0)