diff --git a/src/esc.py b/src/esc.py index 530d6a3..2d29e36 100644 --- a/src/esc.py +++ b/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 \ No newline at end of file diff --git a/src/main.py b/src/main.py index 2d7f3e3..27d9721 100644 --- a/src/main.py +++ b/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() diff --git a/src/motor.py b/src/motor.py index a7192c1..2b9c75e 100644 --- a/src/motor.py +++ b/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) \ No newline at end of file + self.set_speed(speed) diff --git a/src/wifi.py b/src/wifi.py index e3a9c1f..0879718 100644 --- a/src/wifi.py +++ b/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())) \ No newline at end of file + print("network config {}".format(self.if_sta.ifconfig())) \ No newline at end of file