forked from Karenw1004/Deeppicar-v3
-
Notifications
You must be signed in to change notification settings - Fork 4
/
actuator-adafruit_hat.py
58 lines (48 loc) · 1.3 KB
/
actuator-adafruit_hat.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
from Adafruit_MotorHAT import Adafruit_MotorHAT, Adafruit_DCMotor
# init
mh = Adafruit_MotorHAT(addr=0x60)
# throttle
cur_speed = 0
MAX_SPEED = 255
# Actuator API:
# init, set_throttle, set_steering
def init(default_speed=0):
global steering, throttle
steering = mh.getMotor(1)
throttle = mh.getMotor(2)
set_throttle(default_speed)
def set_throttle(throttle_pct):
global cur_speed
speed = int(MAX_SPEED * throttle_pct / 100)
cur_speed = min(MAX_SPEED, speed)
print ("speed: %d" % cur_speed)
if throttle_pct > 0:
throttle.setSpeed(cur_speed)
throttle.run(Adafruit_MotorHAT.FORWARD)
else:
throttle.setSpeed(-cur_speed)
throttle.run(Adafruit_MotorHAT.BACKWARD)
def set_steering(steering_deg):
global cur_steer
cur_steer = steering_deg
if steering_deg < -10:
left()
elif steering_deg > 10:
right()
else:
center()
# steering
def center():
steering.setSpeed(0)
def left():
steering.setSpeed(MAX_SPEED)
steering.run(Adafruit_MotorHAT.BACKWARD)
def right():
steering.setSpeed(MAX_SPEED)
steering.run(Adafruit_MotorHAT.FORWARD)
# stop
def stop():
throttle.setSpeed(0)
steering.setSpeed(0)
throttle.run(Adafruit_MotorHAT.RELEASE)
steering.run(Adafruit_MotorHAT.RELEASE)