urumbu_corexy plotter code, button sensing in .ino

UrumbotXY/code/examples/spiral.xy

+UP
+100, 100
+DOWN
+99.98254759356271, 99.00015230484361
+97.98376593952453, 99.06995129824861
+98.14077380825336, 102.06583990251234
+102.13103000929266, 101.78681400753584
+101.69525129555436, 96.8058405170771
+95.72811992334472, 97.43301129668302
+96.58120532718075, 104.38083435817227
+104.50334987711331, 103.26744955049175
+103.09543969175124, 94.3782544851355
+93.24736216162916, 96.11473626180481
+95.34626111077115, 106.9126352797291
+107.08403231957683, 104.41769498991603
+104.15966861310662, 91.75088414770796
+90.57552844524265, 95.13779068610329
+94.45781412178044, 109.62667808043932
+109.83800125679356, 105.21648038736734
+104.86768227650703, 88.95929953599574
+87.74866498319425, 94.52160543474474
+93.93445991788025, 112.48645837113175
+112.72831233359844, 105.6460555046184
+105.20258539314707, 86.04086654817719
+84.80454059267775, 94.28221160332726
+93.79135654793097, 115.45382323273341
+115.71644753135345, 105.69214379891434
+105.15099098783602, 83.03444912299807
+81.78234578405761, 94.43209893951396
+94.04008927702536, 118.48927509259991
+118.76262187707536, 105.34407133459506
+104.70314288993158, 79.98009982755258
+78.72238077639838, 94.98009982755252
+94.68856109861011, 121.55228614931798
+121.82610017561576, 104.59486969385544
+103.85301202011998, 76.91874095165637
+75.66573455324857, 95.93129966966177
+95.74090982553489, 124.60162121977669
+124.86552162303312, 103.44135213724782
+102.59836576640738, 73.89183826549794
+72.65395712935201, 97.28697432787304
+97.19745238029543, 127.59566682469509
+127.83923010505464, 101.88416243723361
+100.94080991644404, 70.94106964809978
+69.72872724639362, 99.04455511517199
+99.05465672908068, 130.4927642847967
+130.70560794398136, 99.92779598460082
+98.88580279058684, 68.10799083120607
+66.93151774947273, 101.1976216467838
+101.3051417255739, 133.2515445697211
+133.42341083079953, 97.58059294680655
+96.44264139988374, 65.43370052627165
+64.30326091555662, 103.73592268222043
+103.93770494986236, 135.83126262576187
+135.952101666797, 94.85470343821264
+93.62441963429043, 62.95850721115414
+61.88401601049682, 106.64542490740126
+106.93737844639122, 138.192128906709
+138.25218104075338, 91.7660248436269
+90.44795866786455, 60.721599847769866
+59.71264134233871, 109.9083894248426
+110.28551208376328, 140.29563584453592
+140.28551208376354, 88.33411161746974
+86.93370994826061, 58.760724782442765
+57.82647305553557, 113.50347553969632
+113.95988407940277, 142.10487702328774
+142.01563747390384, 84.58205806014112
+83.10563131652137, 57.111871046996185
+56.261012873518055, 117.40587125140763
+117.93483805483126, 143.58485686018966
+143.40808640711327, 80.53635474964811
+78.99103697880642, 55.80896623102221
+55.049626946008296, 121.58744968603531
+122.18144581355992, 144.70278865249307
+144.43066940855692, 76.22671947924228
+74.62042222325533, 54.88358503448248
+54.223257892797164, 126.016950533918
+126.66769486447717, 145.4283789166075
+145.05375893005154, 71.68590371963168
+70.02726394158861, 54.36467253515319
+53.810152057802256, 130.6601853923898
+131.35869955016767, 145.73409602713713
+145.2505537635226, 66.94947578616056
+65.24779817531657, 54.2782841179011
+53.8356038965902, 135.48026575470973
+136.2169344828198, 145.59542125733822
+144.99732539730184, 62.05558204640319
+60.320776059503615, 54.64734391285045
+54.321719317506776, 140.43785223519518
+141.20248884115472, 144.99108042833117
+144.27364455097492, 57.04468765065076
+55.28719968205611, 55.491423477732134
+55.28719968205545, 145.49142347773213
+146.27333994128708, 143.90325449194026
+143.06258624465818, 51.959298406183414
+50.19003951248274, 56.826542336775695
+56.7471480444314, 150.5975630611991
+151.38564436314718, 142.317767500171
+141.35091188945484, 46.843665544816496
+45.07393518024659, 58.66499185511586
+58.71289907433278, 155.71126259178976
+156.4940447932515, 140.22425055280743
+139.12922702655928, 41.74347525158649
+39.984881498345004, 61.01518378461641
+61.19187396175789, 160.78623905946426
+161.5519906346375, 137.61628046204788
+136.39211349227406, 36.70552492934375
+34.969901731922036, 63.881524665109
+64.18746144852423, 165.77526443457072
+166.5120703365699, 134.49149202924082
+133.13823494407592, 31.777388269364124
+UP

UrumbotXY/code/examples/spiral2.xy

+UP
+100, 100
+DOWN
+99.51519037975366, 100.8746197071394
+98.45535185128726, 99.17852351482654
+101.45124045555097, 99.02151564609771
+99.69775586839465, 102.61669183129437
+96.82987368663943, 98.52093160984941
+102.79700505884907, 97.8937608302435
+100.06188715942415, 104.33729480441056
+95.1365953568189, 98.03320877555677
+104.02579042217515, 96.62529859019469
+100.60558898891843, 106.02222479805377
+93.38893967002284, 97.72041941560329
+105.1267108788285, 95.22547912579019
+101.32587871743289, 107.65744095330963
+91.60066153100698, 97.58668374856846
+106.08954892534304, 93.70439807203077
+102.2187985957482, 109.22912969244666
+89.78578566822227, 97.63515757138421
+106.90480296153505, 92.07285167263518
+103.27943204938063, 110.72376815814079
+87.95854318700115, 97.8680159644099
+107.56373214344245, 90.34228902395876
+104.50192392232108, 112.12818653627332
+86.13330719123334, 98.28644100377623
+108.05839817465576, 88.52476156995706
+105.87950460596421, 113.42962902225068
+84.32452771953322, 98.89061353201113
+108.38170387261927, 86.63287003904358
+107.40451796494892, 114.61581319557826
+82.54666624458787, 99.67970902318633
+108.52742835812127, 84.67970902318676
+109.06845295767648, 115.6749875730349
+80.81412998619115, 100.6518975638858
+108.49025872839056, 82.67880940839055
+110.8619788356909, 116.59598711722458
+79.14120628940815, 101.80434795630009
+108.26581808690631, 80.64407887377112
+112.77498379289658, 117.36828648450006
+77.54199731935877, 103.13323593469515
+107.85068981618087, 78.58974068375178
+114.79661692285761, 117.98205080424019
+76.03035532328582, 104.6337564714962
+107.24243799333685, 76.53027100442469
+116.91533333012342, 118.42818379018972
+74.61981870883731, 106.30014013424203
+106.43962386223181, 74.48033498084726
+119.11894222981385, 118.6983729940099
+73.3235491849078, 108.12567343984821
+105.44181829013311, 72.45472181693339
+121.39465785853353, 118.7851320213
+72.15427020792319, 110.10272313795309
+104.24961015146533, 70.46827910364792
+123.72915300909213, 118.68183954112111
+71.12420697210216, 112.22276434064749
+102.86461059589648, 68.53584664440089
+126.10861499163394, 118.3827749314171
+70.24502817708388, 114.47641240174482
+101.28945317294162, 66.67219002885643
+128.51880381452187, 117.88315041467494
+69.52778980029483, 116.85345843487346
+99.5277898002965, 64.8919342078081
+130.945112369808, 117.17913955063804
+68.98288109462398, 119.3429083461908
+97.58428257821765, 63.20949732232476
+133.37262840034322, 116.26790196584892
+68.6199730243795, 121.93302524444383
+95.46459146738515, 61.63902504003343
+135.78619801856973, 115.14760421320405
+68.44796934414302, 124.61137507848883
+93.17535786276856, 60.19432565018185
+138.17049054082634, 113.81743666851031
+68.47496051604216, 127.36487534024482
+90.7241841110387, 58.88880616699388
+140.5100643956007, 112.27762638519368
+68.70818065117678, 130.1798466595684
+88.11960903386671, 57.735409687888506
+142.78943385960343, 110.5294458427731
+69.15396765044932, 133.04206710642256
+85.37107953423602, 56.74655424918606
+144.99313637183388, 108.57521753943743
+69.81772670896055, 135.93682900548913
+82.48891837722138, 55.9340734172833
+147.1058001729711, 106.41831439398916
+70.70389733641755, 138.84899805859646
+79.48428825090328, 55.30915884766181
+149.1122120154658, 104.06315593750325
+71.81592403373813, 141.7630745613653
+76.36915222687288, 54.88230503771731
+150.997384688639, 101.51520029023837
+73.15623075323322, 144.66325649216319
+73.15623075323246, 54.66325649216319
+152.74662410291774, 98.7809319345794
+74.72619925652673, 147.53350424403453
+69.85895532593285, 54.66095751185915
+154.3455956780545, 95.86784531003255
+76.52615147060006, 150.35760676338163
+66.49141899690592, 54.88350480802733
+155.78038978179444, 92.78442427148258
+78.55533592833504, 153.11924885339624
+63.06832388934795, 55.33810313447827
+157.03758596793824, 89.54011746704664
+80.81191836544082, 155.8020793950908
+59.60492590203145, 56.03102412024218
+158.1043157662251, 86.14530970668076
+83.29297653100355, 158.38978023441456
+56.116976795235956, 56.967568474063164
+158.96832378049078, 82.61128940763109
+85.99449925380532, 160.86613548088405
+52.62066386131464, 58.152031721006296
+UP
\ No newline at end of file

UrumbotXY/code/urumbu_corexy.py

+import serial
+import time
+import multiprocessing
+import logging
+import argparse
+import numpy as np
+
+BAUDRATE_DEFAULT = 921600
+
+
+class Module:
+    def __init__(self, port, baudrate=BAUDRATE_DEFAULT):
+        self.port = None
+        self.baudrate = baudrate
+        try:
+            self.port = serial.Serial(port, baudrate)
+        except serial.SerialException:
+            logging.error(f"Cannot connect to {port}")
+
+    @property
+    def connected(self):
+        return self.port is not None
+
+    def write(self, txt):
+        self.port.write(txt)
+
+    def close(self):
+        self.port.close()
+
+    def pressed(self, nc=True):
+        self.write(b"?")
+        r = self.port.read(1)
+        if nc:
+            return r == b"1"
+        else:
+            return r == b"0"
+
+
+class Stepper(Module):
+    def __init__(self, steps_per_unit, port, baudrate=BAUDRATE_DEFAULT, reverse=False):
+        super().__init__(port, baudrate)
+        self.steps = 0
+        self.reverse = reverse
+        self.steps_per_unit = steps_per_unit
+
+    def step(self, forward):
+        self.steps += 1 if forward else -1
+        if self.reverse:
+            forward = not forward
+        self.write(b"f" if forward else b"r")
+
+
+class Servo(Module):
+    def __init__(self, pulse_min, pulse_max, port, baudrate=BAUDRATE_DEFAULT):
+        self.pulse_min = pulse_min
+        self.pulse_max = pulse_max
+        super().__init__(port, baudrate)
+        self.delay_us = 0
+
+    def pulse(self, delay_us):
+        self.write(delay_us.to_bytes(2, byteorder='little'))
+
+    def fraction(self, f):
+        p = int(self.pulse_min + (self.pulse_max - self.pulse_min) * f)
+        self.pulse(p)
+
+    def pressed(self, nc=True):
+        self.pulse(65535)
+        r = self.port.read(1)
+        if nc:
+            return r == b"1"
+        else:
+            return r == b"0"
+
+
+class Action:
+    pass
+
+
+class HomingAction(Action):
+    def __init__(self, axis, name, pos, feedrate, nc=True):
+        self.axis = axis
+        self.name = name
+        self.pos = np.array(pos)
+        self.feedrate = feedrate
+        self.nc = nc
+
+
+class PathAction(Action):
+    def __call__(self, t):
+        raise NotImplementedError()
+
+    def init(self, pos_start):
+        raise NotImplementedError()
+
+
+class WaitAction(Action):
+    def __init__(self, dt):
+        self.dt = dt
+
+    def __call__(self, dt):
+        return dt <= self.dt
+
+
+class ServoAction(Action):
+    def __init__(self, name, pulse, dt=0.01, wait=1.0):
+        self.name = name
+        self.pulse = pulse
+        self.dt = dt
+        self.wait = wait
+
+
+class Line(PathAction):
+    def __init__(self, pos_end, feedrate):
+        self.pos_start = np.zeros_like(pos_end)
+        self.pos_end = np.array(pos_end)
+        self.duration = -1
+        self.feedrate = feedrate
+
+    def init(self, pos_start):
+        self.pos_start = np.array(pos_start)
+        self.duration = np.linalg.norm(self.pos_end - self.pos_start) / self.feedrate
+
+    def __call__(self, t):
+        if t > self.duration:
+            # end move
+            return None
+        u = t / self.duration
+        return self.pos_start * (1 - u) + self.pos_end * u
+
+
+def transform_corexy(pos, pos_transform):
+    pos_transform[:] = pos[:]
+    pos_transform[0] = pos[0] + pos[1]
+    pos_transform[1] = pos[0] - pos[1]
+
+
+def modulesManager(action_queue, modules_config, pos_transformer=None):
+    logging.info("start loop")
+
+    modules = {}
+
+    modules_axis = {}
+
+    for name, config in modules_config.items():
+        if config["type"] == "stepper":
+            obj = Stepper(config["steps_per_unit"],
+                          config["port"],
+                          config["baudrate"],
+                          reverse=config.get("reverse", False))
+            modules[name] = obj
+            if "axis" in config:
+                modules_axis[config["axis"]] = obj
+        elif config["type"] == "servo":
+            modules[name] = Servo(config["pulse_min"],
+                                  config["pulse_max"],
+                                  config["port"],
+                                  config["baudrate"])
+
+    n_axis = len(modules_axis)
+    pos = np.zeros((n_axis,))
+    pos_motors = np.zeros((n_axis,))
+
+    def tick_motor():
+        if pos_transformer is None:
+            pos_motors[:] = pos[:]
+        else:
+            pos_transformer(pos, pos_motors)
+        for j in range(n_axis):
+            m = modules_axis[j]
+            s = int(pos_motors[j] * m.steps_per_unit)
+            if m.steps < s:
+                m.step(True)
+            elif m.steps > s:
+                m.step(False)
+
+    while True:
+        if not action_queue.empty():
+            action = action_queue.get()
+            t0 = time.perf_counter()
+
+            if isinstance(action, PathAction):
+                # time in s, ~us resolution
+                action.init(pos)
+
+                while True:
+                    t = time.perf_counter()
+
+                    # path is a time function
+                    pos_new = action(t - t0)
+
+                    if pos_new is None:
+                        # done
+                        break
+
+                    pos[:] = pos_new[:]
+                    tick_motor()
+            elif isinstance(action, WaitAction):
+                dt = 0
+                while action(dt):
+                    dt = time.perf_counter() - t0
+            elif isinstance(action, ServoAction):
+                dt1 = 0
+                t0_pwm = t0
+                action_wait = WaitAction(action.wait)
+                while action_wait(dt1):
+                    t1 = time.perf_counter()
+                    dt1 = t1 - t0
+                    dt2 = t1 - t0_pwm
+                    if dt2 >= action.dt:
+                        t0_pwm = t1
+                        modules[action.name].pulse(action.pulse)
+
+            elif isinstance(action, HomingAction):
+                line = Line(action.pos, action.feedrate)
+                line.init(pos)
+
+                while True:
+                    t = time.perf_counter()
+
+                    # path is a time function
+                    pos_new = line(t - t0)
+
+                    if pos_new is None:
+                        logging.error(f"Homing failed for axis {action.axis}")
+                        break
+
+                    pos[:] = pos_new[:]
+
+                    tick_motor()
+
+                    if modules[action.name].pressed(action.nc):
+                        logging.info(f"Homing axis {action.axis}")
+                        # homing success
+                        for i in range(n_axis):
+                            motor = modules_axis[i]
+                            motor.steps = 0
+                        pos[action.axis] = 0
+                        break
+
+
+def parse_arguments():
+    usage_text = (
+        "Usage:  python main.py [options]"
+    )
+    parser = argparse.ArgumentParser(description=usage_text)
+    parser.add_argument("-f", "--filename", type=str, required=True,
+                        help="filename for .xy file")
+    parser.add_argument("--feedrate", type=float, default=15,
+                        help="feedrate for XY motion")
+    parser.add_argument("-a", type=str, default="COM18",
+                        help="COM port for A")
+    parser.add_argument("-b", type=str, default="COM25",
+                        help="COM port for B")
+    parser.add_argument("-s", default="COM24",
+                        help="COM port for servo")
+    return parser.parse_known_args()
+
+
+def main():
+    multiprocessing.set_start_method('spawn')
+    action_queue = multiprocessing.Queue()
+
+    args, _ = parse_arguments()
+
+    modules_config = {
+        "a": {
+            "type": "stepper",
+            "port": args.a,
+            "baudrate": 921600,
+            "axis": 0,
+            "steps_per_unit": 6400 / 64,  # 64 mm / turn
+            "reverse": True
+        },
+        "b": {
+            "type": "stepper",
+            "port": args.b,
+            "baudrate": 921600,
+            "axis": 1,
+            "steps_per_unit": 6400 / 64,
+            "reverse": True
+        },
+        "servo": {
+            "type": "servo",
+            "port": args.s,
+            "pulse_min": 600,
+            "pulse_max": 2500,
+            "baudrate": 921600,
+        }
+    }
+
+    p1 = multiprocessing.Process(target=modulesManager, args=(action_queue, modules_config, transform_corexy))
+    p1.start()
+
+    feedrate = args.feedrate
+    feedrate_homing = 15
+
+    servo_up = ServoAction("servo", 1265, wait=0.5)
+    servo_down = ServoAction("servo", 800, wait=0.5)
+
+    action_queue.put(servo_up)
+    action_queue.put(HomingAction(0, "servo", [-200, 0], feedrate_homing))
+    action_queue.put(HomingAction(1, "b", [0, -200], feedrate_homing))
+
+    with open(args.filename, "r") as f:
+        for line in f.readlines():
+            if line.upper().startswith("UP"):
+                action_queue.put(servo_up)
+            elif line.upper().startswith("DOWN"):
+                action_queue.put(servo_down)
+            else:
+                action_queue.put(Line([float(x) for x in line.strip().split(",")], feedrate))
+
+    action_queue.put(servo_up)
+    action_queue.put(Line([0, 0], feedrate))
+
+
+if __name__ == "__main__":
+    main()

serialservo/serialservo.ino

@@ -4,6 +4,7 @@
 // serial servo PWM
 //
 // Neil Gershenfeld 7/28/21
+// Quentin Bolsee 7/12/2021 : add button
 //
 // This work may be reproduced, modified, distributed,
 // performed, and displayed for any purpose, but must
@@ -15,6 +16,7 @@
 #define LEDA 4
 #define LEDC 2
 #define SERVO 5
+#define BUTTON 31
 
 void setup() {
    SerialUSB.begin(0);
@@ -24,14 +26,21 @@ void setup() {
    pinMode(LEDC,OUTPUT);
    digitalWrite(SERVO,LOW);
    pinMode(SERVO,OUTPUT);
+   pinMode(BUTTON,INPUT_PULLUP);
    }
 
 void loop() {
    uint16_t duration;
    if (SerialUSB.available()) {
       SerialUSB.readBytes((char *)&duration,2);
+      if (duration == 65535L) {
+         int btn = digitalRead(BUTTON);
+         SerialUSB.write(btn ? '1' : '0');
+         }
+      else {
          digitalWrite(SERVO,HIGH);
          delayMicroseconds(duration);
          digitalWrite(SERVO,LOW);
          }
       }
+   }

serialstep/serialstep.ino

@@ -4,6 +4,7 @@
 // serial step-and-direction
 //
 // Neil Gershenfeld 4/11/21
+// Quentin Bolsee 7/12/2021 : add button
 //
 // This work may be reproduced, modified, distributed,
 // performed, and displayed for any purpose, but must
@@ -23,6 +24,7 @@
 #define DELAYHIGH 10
 #define DELAYLOW 1000
 #define BLINK 100
+#define BUTTON 31
 
 void setup() {
    SerialUSB.begin(0);
@@ -36,6 +38,7 @@ void setup() {
    pinMode(STEP,OUTPUT);
    digitalWrite(DIR,LOW);
    pinMode(DIR,OUTPUT);
+   pinMode(BUTTON, INPUT_PULLUP);
    /*
    // 1 step
    digitalWrite(M0,LOW);
@@ -72,11 +75,15 @@ void loop() {
          delayMicroseconds(2);
          digitalWrite(STEP,LOW);
          }
-      else {
+      else if (c == 'r') {
          digitalWrite(DIR,LOW);
          digitalWrite(STEP,HIGH);
          delayMicroseconds(2);
          digitalWrite(STEP,LOW);
          }
+      else if (c == '?') {
+         int btn = digitalRead(BUTTON);
+         SerialUSB.write(btn ? '1' : '0');
+         }
       }
    }