86 lines
No EOL
2.5 KiB
Python
86 lines
No EOL
2.5 KiB
Python
from motor import Motor
|
|
from time import sleep
|
|
|
|
DELTA = 10
|
|
DELTA_TURN = 5
|
|
DELTA_TURN_MAX = 60
|
|
|
|
class Esc:
|
|
def __init__(self, pin_m1_ena, pin_m1_in1, pin_m1_in2,
|
|
pin_m2_ena, pin_m2_in1, pin_m2_in2):
|
|
self.m1 = Motor("m1", pin_m1_ena, pin_m1_in1, pin_m1_in2)
|
|
self.m2 = Motor("m2", pin_m2_ena, pin_m2_in1, pin_m2_in2)
|
|
|
|
## regular movement
|
|
def accel(self):
|
|
speed = min((self.m1.speed + self.m2.speed)/2 + DELTA, 100)
|
|
self.m1.set_speed(speed)
|
|
self.m2.set_speed(speed)
|
|
|
|
def decel(self):
|
|
speed = max((self.m1.speed + self.m2.speed)/2 - DELTA, -100)
|
|
self.m1.set_speed(speed)
|
|
self.m2.set_speed(speed)
|
|
|
|
# m1: left, m2: right
|
|
def left(self):
|
|
s1 = self.m1.speed
|
|
s2 = self.m2.speed
|
|
avg = (s1 + s2)/2
|
|
delta = s2 - s1
|
|
|
|
if delta <= (DELTA_TURN_MAX - 2*DELTA_TURN):
|
|
self.m1.dec(DELTA_TURN)
|
|
self.m2.inc(DELTA_TURN)
|
|
elif delta > (DELTA_TURN_MAX):
|
|
# print("delta exceeded, resetting")
|
|
self.m1.set_speed(avg - DELTA_TURN)
|
|
self.m2.set_speed(avg + DELTA_TURN)
|
|
# elif delta == DELTA_TURN_MAX:
|
|
# print("delta reached, not changing")
|
|
|
|
def right(self):
|
|
s1 = self.m1.speed
|
|
s2 = self.m2.speed
|
|
avg = (s1 + s2)/2
|
|
delta = s1 - s2
|
|
if delta <= (DELTA_TURN_MAX - 2*DELTA_TURN):
|
|
self.m1.inc(DELTA_TURN)
|
|
self.m2.dec(DELTA_TURN)
|
|
elif delta > (DELTA_TURN_MAX):
|
|
# print("delta exceeded, resetting")
|
|
self.m1.set_speed(avg + DELTA_TURN)
|
|
self.m2.set_speed(avg - DELTA_TURN)
|
|
# elif delta == DELTA_TURN_MAX:
|
|
# print("delta reached, not changing")
|
|
|
|
## extra
|
|
def neutral(self):
|
|
speed = (self.m1.speed + self.m2.speed)/2
|
|
self.m1.set_speed(speed)
|
|
self.m2.set_speed(speed)
|
|
|
|
def stop(self):
|
|
self.m1.set_speed(0)
|
|
self.m2.set_speed(0)
|
|
|
|
def rotate(self):
|
|
if self.m1.speed >= 0:
|
|
self.m1.set_speed(0)
|
|
self.m2.set_speed(0)
|
|
sleep(0.25)
|
|
self.m1.set_speed(-50)
|
|
self.m2.set_speed(50)
|
|
sleep(0.75)
|
|
else:
|
|
self.m1.set_speed(0)
|
|
self.m2.set_speed(0)
|
|
sleep(0.25)
|
|
self.m1.set_speed(50)
|
|
self.m2.set_speed(-50)
|
|
sleep(0.75)
|
|
|
|
|
|
def get_status(self):
|
|
s = "{}, {}".format(self.m1.get_status(), self.m2.get_status())
|
|
return s |