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_TURN = 5
|
||||
DELTA_MAX = 30
|
||||
DELTA_TURN_MAX = 60
|
||||
|
||||
class Esc:
|
||||
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.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)
|
||||
self.m1.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)
|
||||
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_MAX - 2*DELTA_TURN):
|
||||
if delta <= (DELTA_TURN_MAX - 2*DELTA_TURN):
|
||||
self.m1.dec(DELTA_TURN)
|
||||
self.m2.inc(DELTA_TURN)
|
||||
elif delta == DELTA_MAX:
|
||||
print("delta reached, not changing")
|
||||
elif delta > (DELTA_MAX):
|
||||
print("delta exceeded, resetting")
|
||||
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_MAX - 2*DELTA_TURN):
|
||||
if delta <= (DELTA_TURN_MAX - 2*DELTA_TURN):
|
||||
self.m1.inc(DELTA_TURN)
|
||||
self.m2.dec(DELTA_TURN)
|
||||
elif delta == DELTA_MAX:
|
||||
print("delta reached, not changing")
|
||||
elif delta > (DELTA_MAX):
|
||||
print("delta exceeded, resetting")
|
||||
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")
|
||||
|
||||
# stop
|
||||
def b1(self):
|
||||
## 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 b2(self):
|
||||
def rotate(self):
|
||||
if self.m1.speed >= 0:
|
||||
self.m1.set_speed(0)
|
||||
self.m2.set_speed(0)
|
||||
sleep(1)
|
||||
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(1)
|
||||
sleep(0.25)
|
||||
self.m1.set_speed(50)
|
||||
self.m2.set_speed(-50)
|
||||
sleep(0.75)
|
||||
|
||||
|
||||
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
|
||||
104
src/main.py
104
src/main.py
|
|
@ -1,69 +1,99 @@
|
|||
# main.py
|
||||
import machine
|
||||
import usocket as socket
|
||||
from time import sleep
|
||||
import time
|
||||
from esc import Esc
|
||||
from wifi import WifiAP
|
||||
from wifi import WifiSTA
|
||||
# local
|
||||
from credentials import *
|
||||
import wifi
|
||||
|
||||
# import web
|
||||
|
||||
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
|
||||
## 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
|
||||
|
||||
## Variables
|
||||
esc = None
|
||||
|
||||
def main():
|
||||
print("main")
|
||||
global esc
|
||||
wifi_sta = WifiSTA()
|
||||
# wifi_ap = WifiAP()
|
||||
# wifiSTA = wifi.WifiSTA(SSID, PASS)
|
||||
wifiAP = wifi.WifiAP(SSID, PASS)
|
||||
wifiAP.connect()
|
||||
esc = Esc(PIN_M1_ENA, PIN_M1_IN1, PIN_M1_IN2,
|
||||
PIN_M2_ENA, PIN_M2_IN1, PIN_M2_IN2)
|
||||
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))
|
||||
|
||||
ts_cmd = 0
|
||||
ts_status = 0
|
||||
while True:
|
||||
# wifiSTA.check()
|
||||
request = ""
|
||||
try:
|
||||
request, addr = s.recvfrom(256)
|
||||
req = request.decode()
|
||||
|
||||
print("data: {}, from: {}".format(req, addr))
|
||||
if len(request) == 0:
|
||||
print("no data")
|
||||
continue
|
||||
except:
|
||||
print("error: no data")
|
||||
except OSError as ex:
|
||||
pass
|
||||
except Exception as ex:
|
||||
print("Exception Type:{}, args: {}".format(type(ex).__name__, ex.args))
|
||||
|
||||
if "fwd" in req:
|
||||
print("--> fwd")
|
||||
esc.fwd()
|
||||
if len(request) > 0:
|
||||
print('len > 0')
|
||||
ts_cmd = time.time()
|
||||
req = request.decode()
|
||||
print("data: {}, from: {}".format(req, addr))
|
||||
|
||||
elif "left" in req:
|
||||
print("--> left")
|
||||
esc.left()
|
||||
## 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:
|
||||
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()
|
||||
else:
|
||||
print("unknown: {}".format(req))
|
||||
|
||||
elif "rev" in req:
|
||||
print("--> rev")
|
||||
esc.rev()
|
||||
|
||||
elif "right" in req:
|
||||
print("--> right")
|
||||
esc.right()
|
||||
if time.time() > (ts_cmd + T_NEUTRAL):
|
||||
ts_cmd = time.time()
|
||||
# print(" -: neutral")
|
||||
esc.neutral()
|
||||
# print(esc.get_status())
|
||||
|
||||
elif "b1" in req:
|
||||
print("--> b1")
|
||||
esc.b1()
|
||||
if time.time() > (ts_status + T_STATUS):
|
||||
ts_status = time.time()
|
||||
print(esc.get_status())
|
||||
|
||||
elif "b2" in req:
|
||||
print("--> b2")
|
||||
esc.b2()
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
|
|
|
|||
20
src/motor.py
20
src/motor.py
|
|
@ -1,5 +1,5 @@
|
|||
import machine
|
||||
from time import sleep
|
||||
import time
|
||||
|
||||
|
||||
class Motor:
|
||||
|
|
@ -16,27 +16,27 @@ class Motor:
|
|||
# state
|
||||
self.dir = -1
|
||||
self.speed = -1
|
||||
sleep(1)
|
||||
time.sleep(1)
|
||||
# initialize motors
|
||||
self.set_speed(0)
|
||||
print(self.get_status())
|
||||
|
||||
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
|
||||
|
||||
def set_dir(self, dir):
|
||||
if self.dir == dir:
|
||||
return
|
||||
self.dir = dir
|
||||
v1 = 1 if dir == self.DIR_FWD else 0
|
||||
v2 = 0 if dir == self.DIR_FWD else 1
|
||||
v1 = 1 if self.dir == self.DIR_FWD else 0
|
||||
v2 = 0 if self.dir == self.DIR_FWD else 1
|
||||
self.pin1.value(v1)
|
||||
self.pin2.value(v2)
|
||||
|
||||
def set_speed(self, speed=100):
|
||||
print("[{}:set_speed] {} [-100:100]".format(self.name, speed))
|
||||
if speed < 0:
|
||||
def set_speed(self, speed):
|
||||
# print("{}:set_speed] {} -100:100".format(self.name, speed))
|
||||
if (speed < 0):
|
||||
self.set_dir(self.DIR_REV)
|
||||
else:
|
||||
self.set_dir(self.DIR_FWD)
|
||||
|
|
@ -50,4 +50,4 @@ class Motor:
|
|||
|
||||
def dec(self, delta):
|
||||
speed = max(self.speed - delta, -100)
|
||||
self.set_speed(speed)
|
||||
self.set_speed(speed)
|
||||
|
|
|
|||
53
src/wifi.py
53
src/wifi.py
|
|
@ -1,4 +1,3 @@
|
|||
from credentials import *
|
||||
import network
|
||||
import time
|
||||
|
||||
|
|
@ -7,25 +6,43 @@ netmask = "255.255.255.0"
|
|||
gateway = "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:
|
||||
def __init__(self):
|
||||
global if_ap, if_sta
|
||||
if_sta.active(False)
|
||||
if_ap.active(True)
|
||||
if_ap.ifconfig([address, netmask, gateway, dns])
|
||||
if_ap.config(essid=SSID, password=PASS)
|
||||
print("network config {}".format(if_ap.ifconfig()))
|
||||
def __init__(self, ssid, password):
|
||||
self.ssid = ssid
|
||||
self.password = password
|
||||
# STA: disable
|
||||
self.if_sta = network.WLAN(network.STA_IF)
|
||||
self.if_sta.active(False)
|
||||
# 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:
|
||||
def __init__(self):
|
||||
global if_ap, if_sta
|
||||
if_ap.active(False)
|
||||
if_sta.active(True)
|
||||
if_sta.connect(SSID, PASS)
|
||||
while not if_sta.isconnected():
|
||||
pass
|
||||
def __init__(self, ssid, password):
|
||||
self.ssid = ssid
|
||||
self.password = password
|
||||
# AP: disable
|
||||
self.if_ap = network.WLAN(network.AP_IF)
|
||||
self.if_ap.active(False)
|
||||
# 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)
|
||||
print("network config {}".format(if_sta.ifconfig()))
|
||||
print("network config {}".format(self.if_sta.ifconfig()))
|
||||
Loading…
Reference in a new issue