-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathRobot.py
124 lines (111 loc) · 5.29 KB
/
Robot.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
# Simple two DC motor robot class. Exposes a simple LOGO turtle-like API for
# moving a robot forward, backward, and turning. See RobotTest.py for an
# example of using this class.
# Author: Tony DiCola
# License: MIT License https://opensource.org/licenses/MIT
import time
import atexit
from Raspi_MotorHAT import Raspi_MotorHAT, Raspi_DCMotor
# from Adafruit_MotorHAT import Adafruit_MotorHAT
class Robot(object):
def __init__(self, addr=0x6f, left_id=1, right_id=2, left_trim=0, right_trim=0,
stop_at_exit=True):
"""Create an instance of the robot. Can specify the following optional
parameters:
- addr: The I2C address of the motor HAT, default is 0x60.
- left_id: The ID of the left motor, default is 1.
- right_id: The ID of the right motor, default is 2.
- left_trim: Amount to offset the speed of the left motor, can be positive
or negative and use useful for matching the speed of both
motors. Default is 0.
- right_trim: Amount to offset the speed of the right motor (see above).
- stop_at_exit: Boolean to indicate if the motors should stop on program
exit. Default is True (highly recommended to keep this
value to prevent damage to the bot on program crash!).
"""
# Initialize motor HAT and left, right motor.
self._mh = Raspi_MotorHAT(addr)
self._left = self._mh.getMotor(left_id)
self._right = self._mh.getMotor(right_id)
self._left_trim = left_trim
self._right_trim = right_trim
# Start with motors turned off.
self._left.run(Raspi_MotorHAT.RELEASE)
self._right.run(Raspi_MotorHAT.RELEASE)
# Configure all motors to stop at program exit if desired.
if stop_at_exit:
atexit.register(self.stop)
def _left_speed(self, speed):
"""Set the speed of the left motor, taking into account its trim offset.
"""
assert 0 <= speed <= 255, 'Speed must be a value between 0 to 255 inclusive!'
speed += self._left_trim
speed = max(0, min(255, speed)) # Constrain speed to 0-255 after trimming.
self._left.setSpeed(speed)
def _right_speed(self, speed):
"""Set the speed of the right motor, taking into account its trim offset.
"""
assert 0 <= speed <= 255, 'Speed must be a value between 0 to 255 inclusive!'
speed += self._right_trim
speed = max(0, min(255, speed)) # Constrain speed to 0-255 after trimming.
self._right.setSpeed(speed)
def stop(self):
"""Stop all movement."""
self._left.run(Raspi_MotorHAT.RELEASE)
self._right.run(Raspi_MotorHAT.RELEASE)
def forward(self, speed, seconds=None):
"""Move forward at the specified speed (0-255). Will start moving
forward and return unless a seconds value is specified, in which
case the robot will move forward for that amount of time and then stop.
"""
# Set motor speed and move both forward.
self._left_speed(speed)
self._right_speed(speed)
self._left.run(Raspi_MotorHAT.FORWARD)
self._right.run(Raspi_MotorHAT.FORWARD)
# If an amount of time is specified, move for that time and then stop.
if seconds is not None:
time.sleep(seconds)
self.stop()
def backward(self, speed, seconds=None):
"""Move backward at the specified speed (0-255). Will start moving
backward and return unless a seconds value is specified, in which
case the robot will move backward for that amount of time and then stop.
"""
# Set motor speed and move both backward.
self._left_speed(speed)
self._right_speed(speed)
self._left.run(Raspi_MotorHAT.BACKWARD)
self._right.run(Raspi_MotorHAT.BACKWARD)
# If an amount of time is specified, move for that time and then stop.
if seconds is not None:
time.sleep(seconds)
self.stop()
def right(self, speed, seconds=None):
"""Spin to the right at the specified speed. Will start spinning and
return unless a seconds value is specified, in which case the robot will
spin for that amount of time and then stop.
"""
# Set motor speed and move both forward.
self._left_speed(speed)
self._right_speed(speed)
self._left.run(Raspi_MotorHAT.FORWARD)
self._right.run(Raspi_MotorHAT.BACKWARD)
# If an amount of time is specified, move for that time and then stop.
if seconds is not None:
time.sleep(seconds)
self.stop()
def left(self, speed, seconds=None):
"""Spin to the left at the specified speed. Will start spinning and
return unless a seconds value is specified, in which case the robot will
spin for that amount of time and then stop.
"""
# Set motor speed and move both forward.
self._left_speed(speed)
self._right_speed(speed)
self._left.run(Raspi_MotorHAT.BACKWARD)
self._right.run(Raspi_MotorHAT.FORWARD)
# If an amount of time is specified, move for that time and then stop.
if seconds is not None:
time.sleep(seconds)
self.stop()