diff --git a/src/esc.py b/src/esc.py index 2d29e36..9c6789f 100644 --- a/src/esc.py +++ b/src/esc.py @@ -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 \ No newline at end of file + return("{}, {}".format(self.m1.get_status(), self.m2.get_status())) diff --git a/src/main.py b/src/main.py index 27d9721..807dce6 100644 --- a/src/main.py +++ b/src/main.py @@ -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__": diff --git a/src/motor.py b/src/motor.py index 2b9c75e..565f6d4 100644 --- a/src/motor.py +++ b/src/motor.py @@ -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) @@ -42,12 +42,4 @@ class Motor: self.set_dir(self.DIR_FWD) 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) + self.pwm.duty(s) \ No newline at end of file