rewrite to mimic the behavior of regular rc remotes
This commit is contained in:
parent
002c4bde18
commit
fce5bab794
3 changed files with 113 additions and 134 deletions
122
src/esc.py
122
src/esc.py
|
@ -1,86 +1,76 @@
|
|||
from motor import Motor
|
||||
from time import sleep
|
||||
|
||||
DELTA = 10
|
||||
DELTA_TURN = 5
|
||||
DELTA_TURN_MAX = 60
|
||||
## Parameters
|
||||
# SPEED_MAX = 100
|
||||
# SPEED_MIN = -100
|
||||
SPEED_MAX = 70
|
||||
SPEED_MIN = -70
|
||||
DELTA_ACC = 10
|
||||
DELTA_DEC = 10
|
||||
FACTOR_TRN = 0.3
|
||||
FACTOR_ROT = 0.5
|
||||
|
||||
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)
|
||||
self.m1_speed = 0
|
||||
self.m2_speed = 0
|
||||
|
||||
## 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")
|
||||
if self.m2_speed > 0:
|
||||
self.m1_speed = self.m2_speed * FACTOR_TRN
|
||||
elif self.m2_speed < 0:
|
||||
self.m1_speed = self.m2_speed * FACTOR_TRN
|
||||
|
||||
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")
|
||||
if self.m1_speed > 0:
|
||||
self.m2_speed = self.m1_speed * FACTOR_TRN
|
||||
elif self.m1_speed < 0:
|
||||
self.m2_speed = self.m1_speed * FACTOR_TRN
|
||||
|
||||
## extra
|
||||
def neutral(self):
|
||||
speed = (self.m1.speed + self.m2.speed)/2
|
||||
self.m1.set_speed(speed)
|
||||
self.m2.set_speed(speed)
|
||||
def accel(self):
|
||||
speed = max(self.m1_speed, self.m2_speed)
|
||||
self.m1_speed = min(speed + DELTA_ACC, SPEED_MAX)
|
||||
self.m2_speed = min(speed + DELTA_ACC, SPEED_MAX)
|
||||
|
||||
def decel(self):
|
||||
speed = min(self.m1_speed, self.m2_speed)
|
||||
self.m1_speed = max(speed - DELTA_DEC, SPEED_MIN)
|
||||
self.m2_speed = max(speed - DELTA_DEC, SPEED_MIN)
|
||||
|
||||
def slow(self):
|
||||
s1 = 0
|
||||
if self.m1_speed > DELTA_DEC:
|
||||
s1 = self.m1_speed - DELTA_DEC
|
||||
elif self.m1_speed < -DELTA_DEC:
|
||||
s1 = self.m1_speed + DELTA_DEC
|
||||
self.m1_speed = s1
|
||||
|
||||
s2 = 0
|
||||
if self.m2_speed > DELTA_DEC:
|
||||
s2 = self.m2_speed - DELTA_DEC
|
||||
elif self.m2_speed < -DELTA_DEC:
|
||||
s2 = self.m2_speed + DELTA_DEC
|
||||
self.m2_speed = s2
|
||||
|
||||
def stop(self):
|
||||
self.m1.set_speed(0)
|
||||
self.m2.set_speed(0)
|
||||
self.m1_speed = 0
|
||||
self.m2_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 rotate_left(self):
|
||||
self.m1_speed = SPEED_MIN * FACTOR_ROT
|
||||
self.m2_speed = SPEED_MAX * FACTOR_ROT
|
||||
|
||||
def rotate_right(self):
|
||||
self.m1_speed = SPEED_MAX * FACTOR_ROT
|
||||
self.m2_speed = SPEED_MIN * FACTOR_ROT
|
||||
|
||||
def apply(self):
|
||||
self.m1.set_speed(self.m1_speed)
|
||||
self.m2.set_speed(self.m2_speed)
|
||||
|
||||
def get_status(self):
|
||||
s = "{}, {}".format(self.m1.get_status(), self.m2.get_status())
|
||||
return s
|
||||
return("{}, {}".format(self.m1.get_status(), self.m2.get_status()))
|
||||
|
|
99
src/main.py
99
src/main.py
|
@ -1,51 +1,49 @@
|
|||
# main.py
|
||||
# standard
|
||||
import time
|
||||
# micropython
|
||||
import ujson as json
|
||||
import machine
|
||||
import usocket as socket
|
||||
import time
|
||||
from esc import Esc
|
||||
# local
|
||||
from credentials import *
|
||||
from esc import Esc
|
||||
from credentials import SSID, PASS
|
||||
import wifi
|
||||
|
||||
# import web
|
||||
|
||||
## Pins
|
||||
## M1: left, M2: right
|
||||
# PIN_M1_ENA = 5 # D1
|
||||
# PIN_M1_IN1 = 4 # D2
|
||||
# PIN_M1_IN2 = 0 # D3
|
||||
# PIN_M2_IN1 = 12 # D6
|
||||
# PIN_M2_IN2 = 13 # D7
|
||||
# PIN_M2_ENA = 15 # D8
|
||||
## M1: right, M2: left
|
||||
PIN_M1_IN1 = 12 # D6
|
||||
PIN_M1_IN2 = 13 # D7
|
||||
PIN_M1_ENA = 15 # D8
|
||||
PIN_M2_ENA = 5 # D1
|
||||
PIN_M2_IN1 = 4 # D2
|
||||
PIN_M2_IN2 = 0 # D3
|
||||
## Parameters
|
||||
T_NEUTRAL=0.5
|
||||
T_STATUS=2
|
||||
## HOST
|
||||
RX_ADDR = ("0.0.0.0", 8000)
|
||||
|
||||
## Variables
|
||||
## Parameters
|
||||
FAIL_COUNT_MAX = 10
|
||||
|
||||
## Runtime
|
||||
esc = None
|
||||
|
||||
def main():
|
||||
print("main")
|
||||
global esc
|
||||
# wifiSTA = wifi.WifiSTA(SSID, PASS)
|
||||
# wifiSTA.connect()
|
||||
wifiAP = wifi.WifiAP(SSID, PASS)
|
||||
wifiAP.connect()
|
||||
# Controller
|
||||
esc = Esc(PIN_M1_ENA, PIN_M1_IN1, PIN_M1_IN2,
|
||||
PIN_M2_ENA, PIN_M2_IN1, PIN_M2_IN2)
|
||||
# Socket
|
||||
s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
||||
s.setsockopt(socket.SOL_SOCKET,socket.SO_REUSEADDR, 1)
|
||||
s.settimeout(0.1)
|
||||
s.bind(("", 80))
|
||||
s.bind(RX_ADDR)
|
||||
|
||||
ts_cmd = 0
|
||||
ts_status = 0
|
||||
fail_count = 0
|
||||
values = {}
|
||||
while True:
|
||||
# wifiSTA.check()
|
||||
request = ""
|
||||
|
@ -56,43 +54,42 @@ def main():
|
|||
except Exception as ex:
|
||||
print("Exception Type:{}, args: {}".format(type(ex).__name__, ex.args))
|
||||
|
||||
## read request
|
||||
if len(request) > 0:
|
||||
print('len > 0')
|
||||
ts_cmd = time.time()
|
||||
fail_count = 0
|
||||
req = request.decode()
|
||||
print("data: {}, from: {}".format(req, addr))
|
||||
values = json.loads(req)
|
||||
else:
|
||||
fail_count += 1
|
||||
if fail_count > FAIL_COUNT_MAX:
|
||||
print("resetting values")
|
||||
fail_count = 0
|
||||
values = {}
|
||||
|
||||
## buttons
|
||||
if "accel" in req:
|
||||
esc.accel()
|
||||
print(esc.get_status())
|
||||
elif "left" in req:
|
||||
esc.left()
|
||||
print(esc.get_status())
|
||||
elif "decel" in req:
|
||||
## control
|
||||
# special
|
||||
if values.get("btn_b1") and not values.get("btn_b2"):
|
||||
esc.rotate_left()
|
||||
elif values.get("btn_b2") and not values.get("btn_b1"):
|
||||
esc.rotate_right()
|
||||
elif values.get("btn_b1") and values.get("btn_b2"):
|
||||
esc.stop()
|
||||
# accel or decel
|
||||
else:
|
||||
if values.get("btn_dn"):
|
||||
esc.decel()
|
||||
print(esc.get_status())
|
||||
elif "right" in req:
|
||||
esc.right()
|
||||
print(esc.get_status())
|
||||
elif "stop" in req:
|
||||
esc.stop()
|
||||
print(esc.get_status())
|
||||
elif "rotate" in req:
|
||||
esc.rotate()
|
||||
elif values.get("btn_up"):
|
||||
esc.accel()
|
||||
else:
|
||||
print("unknown: {}".format(req))
|
||||
esc.slow()
|
||||
# left or right
|
||||
if values.get("btn_le"):
|
||||
esc.left()
|
||||
elif values.get("btn_ri"):
|
||||
esc.right()
|
||||
esc.apply()
|
||||
|
||||
|
||||
if time.time() > (ts_cmd + T_NEUTRAL):
|
||||
ts_cmd = time.time()
|
||||
# print(" -: neutral")
|
||||
esc.neutral()
|
||||
# print(esc.get_status())
|
||||
|
||||
if time.time() > (ts_status + T_STATUS):
|
||||
ts_status = time.time()
|
||||
print(esc.get_status())
|
||||
print("[{}] {}".format(fail_count, esc.get_status()))
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
|
|
26
src/motor.py
26
src/motor.py
|
@ -1,6 +1,7 @@
|
|||
import machine
|
||||
# standard
|
||||
import time
|
||||
|
||||
# micropy
|
||||
import machine
|
||||
|
||||
class Motor:
|
||||
PWM_MAX = 1023
|
||||
|
@ -21,14 +22,13 @@ class Motor:
|
|||
self.set_speed(0)
|
||||
|
||||
def get_status(self):
|
||||
s = "[{}:status] pins {},{} duty {}".format(
|
||||
self.name, self.pin1.value(), self.pin2.value(), self.pwm.duty())
|
||||
return s
|
||||
return("{}: pins {},{} duty {}".format(
|
||||
self.name, self.pin1.value(), self.pin2.value(), self.pwm.duty()))
|
||||
|
||||
def set_dir(self, dir):
|
||||
if self.dir == dir:
|
||||
def set_dir(self, d):
|
||||
if self.dir == d:
|
||||
return
|
||||
self.dir = dir
|
||||
self.dir = d
|
||||
v1 = 1 if self.dir == self.DIR_FWD else 0
|
||||
v2 = 0 if self.dir == self.DIR_FWD else 1
|
||||
self.pin1.value(v1)
|
||||
|
@ -42,12 +42,4 @@ class Motor:
|
|||
self.set_dir(self.DIR_FWD)
|
||||
self.speed = speed
|
||||
s = int(self.PWM_MAX * abs(speed)/100)
|
||||
self.pwm.duty(s)
|
||||
|
||||
def inc(self, delta):
|
||||
speed = min(self.speed + delta, 100)
|
||||
self.set_speed(speed)
|
||||
|
||||
def dec(self, delta):
|
||||
speed = max(self.speed - delta, -100)
|
||||
self.set_speed(speed)
|
||||
self.pwm.duty(s)
|
Loading…
Reference in a new issue