update
This commit is contained in:
parent
0899deb958
commit
002c4bde18
4 changed files with 141 additions and 84 deletions
48
src/esc.py
48
src/esc.py
|
@ -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
|
100
src/main.py
100
src/main.py
|
@ -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()
|
||||||
|
|
18
src/motor.py
18
src/motor.py
|
@ -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)
|
||||||
|
|
53
src/wifi.py
53
src/wifi.py
|
@ -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()))
|
Loading…
Reference in a new issue