-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathRobot.py
150 lines (138 loc) · 6.55 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
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
# 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 Adafruit_MotorHAT import Adafruit_MotorHAT
class Robot(object):
def __init__(self, addr=0x60, 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 = Adafruit_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(Adafruit_MotorHAT.RELEASE)
self._right.run(Adafruit_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(Adafruit_MotorHAT.RELEASE)
self._right.run(Adafruit_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(Adafruit_MotorHAT.FORWARD)
self._right.run(Adafruit_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(Adafruit_MotorHAT.BACKWARD)
self._right.run(Adafruit_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 pivotright(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(Adafruit_MotorHAT.BACKWARD)
self._right.run(Adafruit_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 pivotleft(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(Adafruit_MotorHAT.FORWARD)
self._right.run(Adafruit_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 turnright(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*3/4)
self._right_speed(speed)
self._left.run(Adafruit_MotorHAT.FORWARD)
self._right.run(Adafruit_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 turnleft(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*3/4)
self._left.run(Adafruit_MotorHAT.FORWARD)
self._right.run(Adafruit_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()