This commit is contained in:
Konstantin Koslowski 2021-07-17 14:43:04 +02:00
parent 0899deb958
commit 002c4bde18
4 changed files with 141 additions and 84 deletions

View file

@ -3,7 +3,7 @@ from time import sleep
DELTA = 10 DELTA = 10
DELTA_TURN = 5 DELTA_TURN = 5
DELTA_MAX = 30 DELTA_TURN_MAX = 60
class Esc: class Esc:
def __init__(self, pin_m1_ena, pin_m1_in1, pin_m1_in2, def __init__(self, pin_m1_ena, pin_m1_in1, pin_m1_in2,
@ -11,66 +11,76 @@ class Esc:
self.m1 = Motor("m1", pin_m1_ena, pin_m1_in1, pin_m1_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.m2 = Motor("m2", pin_m2_ena, pin_m2_in1, pin_m2_in2)
def fwd(self): ## regular movement
def accel(self):
speed = min((self.m1.speed + self.m2.speed)/2 + DELTA, 100) speed = min((self.m1.speed + self.m2.speed)/2 + DELTA, 100)
self.m1.set_speed(speed) self.m1.set_speed(speed)
self.m2.set_speed(speed) self.m2.set_speed(speed)
def rev(self): def decel(self):
speed = max((self.m1.speed + self.m2.speed)/2 - DELTA, -100) speed = max((self.m1.speed + self.m2.speed)/2 - DELTA, -100)
self.m1.set_speed(speed) self.m1.set_speed(speed)
self.m2.set_speed(speed) self.m2.set_speed(speed)
# m1: left, m2: right
def left(self): def left(self):
s1 = self.m1.speed s1 = self.m1.speed
s2 = self.m2.speed s2 = self.m2.speed
avg = (s1 + s2)/2 avg = (s1 + s2)/2
delta = s2 - s1 delta = s2 - s1
if delta <= (DELTA_MAX - 2*DELTA_TURN): if delta <= (DELTA_TURN_MAX - 2*DELTA_TURN):
self.m1.dec(DELTA_TURN) self.m1.dec(DELTA_TURN)
self.m2.inc(DELTA_TURN) self.m2.inc(DELTA_TURN)
elif delta == DELTA_MAX: elif delta > (DELTA_TURN_MAX):
print("delta reached, not changing") # print("delta exceeded, resetting")
elif delta > (DELTA_MAX):
print("delta exceeded, resetting")
self.m1.set_speed(avg - DELTA_TURN) self.m1.set_speed(avg - DELTA_TURN)
self.m2.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): def right(self):
s1 = self.m1.speed s1 = self.m1.speed
s2 = self.m2.speed s2 = self.m2.speed
avg = (s1 + s2)/2 avg = (s1 + s2)/2
delta = s1 - s2 delta = s1 - s2
if delta <= (DELTA_MAX - 2*DELTA_TURN): if delta <= (DELTA_TURN_MAX - 2*DELTA_TURN):
self.m1.inc(DELTA_TURN) self.m1.inc(DELTA_TURN)
self.m2.dec(DELTA_TURN) self.m2.dec(DELTA_TURN)
elif delta == DELTA_MAX: elif delta > (DELTA_TURN_MAX):
print("delta reached, not changing") # print("delta exceeded, resetting")
elif delta > (DELTA_MAX):
print("delta exceeded, resetting")
self.m1.set_speed(avg + DELTA_TURN) self.m1.set_speed(avg + DELTA_TURN)
self.m2.set_speed(avg - DELTA_TURN) self.m2.set_speed(avg - DELTA_TURN)
# elif delta == DELTA_TURN_MAX:
# print("delta reached, not changing")
# stop ## extra
def b1(self): 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.m1.set_speed(0)
self.m2.set_speed(0) self.m2.set_speed(0)
def b2(self): def rotate(self):
if self.m1.speed >= 0: if self.m1.speed >= 0:
self.m1.set_speed(0) self.m1.set_speed(0)
self.m2.set_speed(0) self.m2.set_speed(0)
sleep(1) sleep(0.25)
self.m1.set_speed(-50) self.m1.set_speed(-50)
self.m2.set_speed(50) self.m2.set_speed(50)
sleep(0.75)
else: else:
self.m1.set_speed(0) self.m1.set_speed(0)
self.m2.set_speed(0) self.m2.set_speed(0)
sleep(1) sleep(0.25)
self.m1.set_speed(50) self.m1.set_speed(50)
self.m2.set_speed(-50) self.m2.set_speed(-50)
sleep(0.75)
def get_status(self): def get_status(self):
s = (self.m1.get_status(), self.m2.get_status()) s = "{}, {}".format(self.m1.get_status(), self.m2.get_status())
return s return s

View file

@ -1,69 +1,99 @@
# main.py # main.py
import machine import machine
import usocket as socket import usocket as socket
from time import sleep import time
from esc import Esc from esc import Esc
from wifi import WifiAP # local
from wifi import WifiSTA from credentials import *
import wifi
# import web # import web
PIN_M1_ENA = 5 # D1 ## Pins
PIN_M1_IN1 = 4 # D2 ## M1: left, M2: right
PIN_M1_IN2 = 0 # D3 # PIN_M1_ENA = 5 # D1
PIN_M2_IN1 = 12 # D6 # PIN_M1_IN1 = 4 # D2
PIN_M2_IN2 = 13 # D7 # PIN_M1_IN2 = 0 # D3
PIN_M2_ENA = 15 # D8 # 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
## Variables
esc = None esc = None
def main(): def main():
print("main") print("main")
global esc global esc
wifi_sta = WifiSTA() # wifiSTA = wifi.WifiSTA(SSID, PASS)
# wifi_ap = WifiAP() wifiAP = wifi.WifiAP(SSID, PASS)
wifiAP.connect()
esc = Esc(PIN_M1_ENA, PIN_M1_IN1, PIN_M1_IN2, esc = Esc(PIN_M1_ENA, PIN_M1_IN1, PIN_M1_IN2,
PIN_M2_ENA, PIN_M2_IN1, PIN_M2_IN2) PIN_M2_ENA, PIN_M2_IN1, PIN_M2_IN2)
s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
s.setsockopt(socket.SOL_SOCKET,socket.SO_REUSEADDR, 1) s.setsockopt(socket.SOL_SOCKET,socket.SO_REUSEADDR, 1)
s.settimeout(0.1)
s.bind(("", 80)) s.bind(("", 80))
ts_cmd = 0
ts_status = 0
while True: while True:
# wifiSTA.check()
request = "" request = ""
try: try:
request, addr = s.recvfrom(256) request, addr = s.recvfrom(256)
req = request.decode() except OSError as ex:
print("data: {}, from: {}".format(req, addr))
if len(request) == 0:
print("no data")
continue
except:
print("error: no data")
pass pass
except Exception as ex:
print("Exception Type:{}, args: {}".format(type(ex).__name__, ex.args))
if "fwd" in req: if len(request) > 0:
print("--> fwd") print('len > 0')
esc.fwd() ts_cmd = time.time()
req = request.decode()
print("data: {}, from: {}".format(req, addr))
## buttons
if "accel" in req:
esc.accel()
print(esc.get_status())
elif "left" in req: elif "left" in req:
print("--> left")
esc.left() esc.left()
print(esc.get_status())
elif "rev" in req: elif "decel" in req:
print("--> rev") esc.decel()
esc.rev() print(esc.get_status())
elif "right" in req: elif "right" in req:
print("--> right")
esc.right() esc.right()
print(esc.get_status())
elif "stop" in req:
esc.stop()
print(esc.get_status())
elif "rotate" in req:
esc.rotate()
else:
print("unknown: {}".format(req))
elif "b1" in req:
print("--> b1")
esc.b1()
elif "b2" in req: if time.time() > (ts_cmd + T_NEUTRAL):
print("--> b2") ts_cmd = time.time()
esc.b2() # print(" -: neutral")
esc.neutral()
# print(esc.get_status())
if time.time() > (ts_status + T_STATUS):
ts_status = time.time()
print(esc.get_status())
if __name__ == "__main__": if __name__ == "__main__":
main() main()

View file

@ -1,5 +1,5 @@
import machine import machine
from time import sleep import time
class Motor: class Motor:
@ -16,27 +16,27 @@ class Motor:
# state # state
self.dir = -1 self.dir = -1
self.speed = -1 self.speed = -1
sleep(1) time.sleep(1)
# initialize motors # initialize motors
self.set_speed(0) self.set_speed(0)
print(self.get_status())
def get_status(self): def get_status(self):
s = "[{}:status] pins {},{} duty {}".format(self.name, self.pin1.value(), self.pin2.value(), self.pwm.duty()) s = "[{}:status] pins {},{} duty {}".format(
self.name, self.pin1.value(), self.pin2.value(), self.pwm.duty())
return s return s
def set_dir(self, dir): def set_dir(self, dir):
if self.dir == dir: if self.dir == dir:
return return
self.dir = dir self.dir = dir
v1 = 1 if dir == self.DIR_FWD else 0 v1 = 1 if self.dir == self.DIR_FWD else 0
v2 = 0 if dir == self.DIR_FWD else 1 v2 = 0 if self.dir == self.DIR_FWD else 1
self.pin1.value(v1) self.pin1.value(v1)
self.pin2.value(v2) self.pin2.value(v2)
def set_speed(self, speed=100): def set_speed(self, speed):
print("[{}:set_speed] {} [-100:100]".format(self.name, speed)) # print("{}:set_speed] {} -100:100".format(self.name, speed))
if speed < 0: if (speed < 0):
self.set_dir(self.DIR_REV) self.set_dir(self.DIR_REV)
else: else:
self.set_dir(self.DIR_FWD) self.set_dir(self.DIR_FWD)

View file

@ -1,4 +1,3 @@
from credentials import *
import network import network
import time import time
@ -7,25 +6,43 @@ netmask = "255.255.255.0"
gateway = "192.168.0.1" gateway = "192.168.0.1"
dns = "192.168.0.1" dns = "192.168.0.1"
if_ap = network.WLAN(network.AP_IF)
if_sta = network.WLAN(network.STA_IF)
class WifiAP: class WifiAP:
def __init__(self): def __init__(self, ssid, password):
global if_ap, if_sta self.ssid = ssid
if_sta.active(False) self.password = password
if_ap.active(True) # STA: disable
if_ap.ifconfig([address, netmask, gateway, dns]) self.if_sta = network.WLAN(network.STA_IF)
if_ap.config(essid=SSID, password=PASS) self.if_sta.active(False)
print("network config {}".format(if_ap.ifconfig())) # AP: enable
self.if_ap = network.WLAN(network.AP_IF)
def connect(self):
self.if_ap.active(True)
self.if_ap.ifconfig([address, netmask, gateway, dns])
self.if_ap.config(essid=self.ssid, password=self.password)
print("network config {}".format(self.if_ap.ifconfig()))
class WifiSTA: class WifiSTA:
def __init__(self): def __init__(self, ssid, password):
global if_ap, if_sta self.ssid = ssid
if_ap.active(False) self.password = password
if_sta.active(True) # AP: disable
if_sta.connect(SSID, PASS) self.if_ap = network.WLAN(network.AP_IF)
while not if_sta.isconnected(): self.if_ap.active(False)
pass # STA: enable
self.if_sta = network.WLAN(network.STA_IF)
self.if_sta.active(True)
def check(self):
if not self.if_sta.isconnected():
print("wifi down, reconnecting...")
self.connect()
def connect(self):
print("connecting wifi")
self.if_sta.active(True)
self.if_sta.connect(self.ssid, self.password)
while not self.if_sta.isconnected():
time.sleep(0.1) time.sleep(0.1)
print("network config {}".format(if_sta.ifconfig())) print("network config {}".format(self.if_sta.ifconfig()))