konni/uc-wifi-rx-udp
1
0
Fork 0
Code Issues Pull requests Projects Releases Wiki Activity

rewrite to mimic the behavior of regular rc remotes

Browse source
This commit is contained in:
Konstantin Koslowski 2021-07-17 15:23:01 +02:00
parent 002c4bde18
commit fce5bab794
3 changed files with 113 additions and 134 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

122
src/esc.py
View file

@ -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
View file

@ -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__":

24
src/motor.py
View file

@ -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)
@ -43,11 +43,3 @@ class Motor:
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)