# Meet YOR ![](/files/6YuE2BT4aT31cV3Z0NdG) YOR (Your Own Robot) is a fully open-source mobile manipulator with a \~$10k BOM price designed to make robotics and mobile manipulation research accessible. With a mobile base, adjustable lift, and 6-DOF bimanual setup, YOR can perform a wide range of household tasks while costing a fraction of comparable commercial platforms. Most importantly, since **YOR is open source**, researchers and hobbyists can build, modify, and improve upon it and share it back to the world. You can find out more about YOR on the [official website](https://yourownrobot.ai/). # Before you start ### Tools Required * Allen key set (M3, M4, M5, M6) * Wire stripper and crimping tool * Ratcheting Wrench with M5 bit * Multimeter * Zip ties or cable sleeves * Lithium grease * Medium-strength thread locker (Loctite) * Double Sided Tape ### Base Assembly — Swerve Drive Preparation Begin the [assembly](https://docs.revrobotics.com/ion-build/build-guides/maxswerve-module-assembly) process by **assembling and testing all swerve drive modules using the mid speed drive motor pinion**. * Verify that **all four swerve drives are fully functional** before integrating them into the base. * The configuration must include: * **Two left-oriented swerve drivetrains** * **Two right-oriented swerve drivetrains** * Confirm correct motor operation, steering response, encoder feedback, and wiring integrity for each module. > **Important:** Once the base frame is fully assembled, removing or servicing the swerve drives will be **difficult and time-consuming**. Thorough verification at this stage is critical to avoid rework later in the build. Proceed only after all swerve drives have been tested and verified to operate correctly. ### Flex Dock Preparation {% embed url="" %} # 1. Parts and Materials This page contains the list of all the parts and materials you will need, we have tried to keep the order of operation as intuitive as possible. BOM Link : Hardware Files : **The arm attachment plate is manufactured by** [**SendCutSend**](https://sendcutsend.com/?srsltid=AfmBOoqe8gDqVy-SR9D9ib74CQWq6QgBaG3dh7UmRSLre41PAuWpHRQO) **using 304 stainless steel with a thickness of 0.187 inches (4.7 mm). The Piper plate includes two 45° downward bends, while the Nero plate includes two 90° downward bends. Both plates have a matte black anodized finish.** ### Mechanical Components

Component	Quantity	Notes
3-inch MAX Swerve Drive Modules	4	2 left-oriented, 2 right-oriented
Aluminum Extrusion – 2" × 1" × 6.5"	2
Aluminum Extrusion – 2" × 1" × 10"	2
Aluminum Extrusion – 2" × 1" × 15"	2
Aluminum Extrusion – 2" × 1" × 4.75"	8
Aluminum Extrusion – 1" × 1" × 17"	2
Aluminum Extrusion – 1" × 1" × 11.5"	2
90° Aluminum Clamps	As required
Interior L-Shape Corner Connectors	As required
T-Clamps	As required
T-Plates	As required
M4 x 25 mm Screws	10 Pack
M4 Nuts	10 Pack
M5 × 10 mm Screws	50 Pack
M5 Extrusion Nuts	50 Pack
M5 x 25mm Hex nut	10 Pack
M5 Lock Nuts	10 Pack
M6 Screws	50 Pack
M6 Nuts	50 Pack
M6 Washers	40 Pack
10-32 Screws	20 Pack	Used for controller mounting
Flexispot E6 Dual-Motor Lift	1
Lift Attachment Brackets (L-shaped)	As required	Required for lift-to-frame mounting
Cable Carrier (Drag Chain)	1	Required for moving lift cables
Piper Robotic Arms	2
Arm Attachment Plate	1
Dynamixel XL430 Motors	2
Threadlocker (Loctite 243)	1	Use on all metal-to-metal fasteners
Lithium Grease	1	Apply to moving interfaces only

### Electrical Components

Component	Quantity	Notes
24V 20Ah Lithium Battery + Charger	1
Anderson Battery Connector	1	Required for battery quick-disconnect
Power Distribution Board	1
Fuse Box	1	All high-current lines must be fused
Emergency Stop Switch	1	Must cut main system power
24V → 24V, 6A DC-DC Converter	2	Dedicated rails for lift and arms
24V → 12V DC-DC Converter	1
24V → 5V, 5A DC-DC Converter	1	Powers compute and low-level control
Motor Controllers with Flex Dock	4	One per swerve module
CAN Bus Module	1
Lift Motor Controller (BTS7960)	1
U2D2	1	Required for Dynamixel communication
U2D2 Power Hub Board	1	Required for Dynamixel power
Raspberry Pi 5 (16 GB)	1
Raspberry Pi 5 Active Cooler	1	Required to prevent thermal throttling
Raspberry Pi Pico	1
Raspberry Pi Pico Breakout Board	1
USB Hub	1
Ethernet (CAT) Cable	1
USB-C to USB-C Cable	1
Wireless Gamepad	1	Optional for teleoperation
Wago Inline Connectors	As required
Wago 2-Port Connectors	As required
Wago 3-Port Connector	As required
Terminal Block	1
Barrel Connector	2
Wire Crimps (10–12 AWG)	As required	Required for battery and high-current lines
Wire Crimps (16–22 AWG)	As required

### 3D-Printed Parts | Component | Quantity | | ---------------------------- | :------: | | Motor Controller Mount Plate | 2 | | Buck Converter Mount | 4 | | Jetson Mount | 1 | | ZED Camera Mount | 1 | | Raspberry Pi Mount | 1 | | Cable Channels | 2 | | 5V 5A Buck Converter Mount | 1 | | Cover Plates | 16 | | Top Cable Tray | 1 | ### List of Cables

Type	Quantity	Attachment
USB-A to Micro-USB	1	Raspberry Pi 5 to Raspberry Pi Pico
USB-A to Micro-USB (at least 2m long)	1	Raspberry Pi 5 to U2D2 (at the top)
CAT6 Ethernet (at least 2m long)	1	Raspberry Pi 5 to Jetson Orin
USB-A male to USB-A female (at least 2m long)	1	Raspberry Pi 5 to USB hub (at the top)
USB-A to Type-C	1	Jetson Orin to ZED 2i

# 2. Base Assembly Assemble all swerve drive units before integrating them into the base. ### Step 1. Setting up the base extrusions Take both 10-inch aluminum extrusion pieces and slide four M6 Extrusion nuts into the extrusion channels, as shown in the reference image.\ Ensure the nuts are fully seated inside the slots and oriented correctly for later assembly.\ Repeat this process for both 10-inch extrusions.

Take the 6.5-inch aluminum extrusion and insert two M6 Extrusion nuts along with two aluminum extrusion L-shaped inside corner brackets, as highlighted in the reference image. Using a Sharpie, mark the center of the 6.5-inch extrusion on the inside face where the nuts are located. Make sure the nuts and corner brackets are positioned correctly before proceeding to the next step.

Marking the Center of the 6.5 inch extrusion will help in further steps.

### Step 2. Attaching the Swerve Drive Now take one left swerve drivetrain and attach it to one end of the 6.5-inch extrusion using M5 Extrusion nuts and screws, with three fasteners at the top. Ensure that the motor wires are oriented toward the extrusion. Attach one right swerve drivetrain to the opposite end of the same extrusion. During this assembly, make sure that the M6 Extrusion nuts inserted in Step 1 are facing toward the inside of the base.

### Step 3. Assembling the Base Now attach the 10-inch extrusions using the three M5 Extrusion nuts at the top. The completed base should match the reference image, with the nuts facing toward the inside of the base and the motor wires pointing toward the 6.5-inch extrusion.

**Note: When attaching the motors to the extrusions, ensure that the motor wires exit toward the 6.5-inch extrusion. Two motors are left-oriented and two motors are right-oriented. Make sure the left-oriented motors are mounted diagonally opposite each other, and likewise for the right-oriented motors.** ### Step 4. Assembling the center support extrusion Mark the center on the base extrusions, all four as well as the 15-inch long extrusion. Now mark 2.2cm (0.85 in) from the center on each side. Insert 2 M6 Extrusion nuts on each side in the 15 inches long extrusion as shown in the pictures. Also mark the center of each edge.

Take eight M6 washers, eight M6 screws, and four 90-degree clamps, and attach the clamps to both sides of the 15-inch extrusion. Next, attach the entire 15-inch extrusion assembly onto the base. Place the ends of the 15-inch extrusion onto the 6.5-inch extrusions, ensuring that the center of the 15-inch extrusion aligns with the center mark on the 6.5-inch extrusion. Refer to the image for proper alignment. Note: While attaching the 90-degree clamps, keep the screws slightly loose so the clamps can move freely. Align all center marks first, then fully tighten all screws.

Prepare all 4.75-inch connectors by attaching a 90-degree clamp using an M6 extrusion nut and washer, as in the previous step. Also insert an additional M6 extrusion nut as shown in the reference image; this will be used for mounting the lift.

Flip the base assembly. Attach the 4.75-inch extrusions to the 15-inch and 10-inch extrusions using the alignment markings from the earlier step, following the numbering shown in the reference image. Ensure that the additional extrusion nut is oriented toward the motor side and not the wheel side.

Now attach the T plates as shown in the figure, take your time going back and forth with the alignment making sure that you get it right, as these center extrusions will hold the lift so, make sure they are flush with the outer edges and perpendicular and flat. Also attach the bottom 6 M5 screws and nuts for each wheel.

Make sure screws are tight and secure. Now flip the base back onto its wheels, and you’re all set to start mounting the rest of the components. # 3. Adding Components to the Base Keep the base frame assembled and set aside, ready for the installation of additional components. ### Step 1. Motor controller mount (3D-printed plate +Buck converter mount) Take the 3D-printed motor controller mount plate and the motor controller. Using five 10-32 countersunk screws, attach the motor controller to the mount plate and fully tighten the screws. Repeat the step for both the sides.

Four Motor Controllers with the 2 3D-printed plates.

Mount the controller and plate assembly onto the aluminum extrusion, align it properly, and then tighten all fasteners. Attach the buck converter mount onto the mount plate using four 10-32 countersunk screws.

### Step 2. Wiring the motors and motor Controllers **Note:** *The main goal of routing the wires is to make sure they never come into contact with the spinning motors of the swerve drive. You can route the wires as needed, as long as you are confident they will not touch the motors while Yor robot is running.* Cut the motor controller output wires to minimize slack and ensure they cannot come into contact with the spinning motors.

Connect the controller-to-motor power wires for all four rotational motors (the smaller ones). Strip the insulation from both the controller wires and the motor wires. Connect the wires using Wago 2 conductor connectors, matching colors exactly: Black → Black, White → White, Red → Red. Neatly tuck and secure the connected wires so they remain clear of all moving motor components. Repeat this procedure for all four motors. Refer to the reference image for proper routing and placement. {% columns %} {% column %}

Give the connector a firm pull to make sure it is securely connected.

{% endcolumn %} {% column %}

{% endcolumn %} {% endcolumns %} Plug each wheel rotational motor’s encoder feedback wire into the corresponding encoder port on the motor controller, making sure each motor is matched to the correct port. Repeat this for all four motors. {% columns %} {% column %}

{% endcolumn %} {% column valign="middle" %}

{% endcolumn %} {% endcolumns %} Route the encoder cable (the “bore encoder wire” coming from the main motor of the swerve drive).\ Route the cable as shown in the reference image and plug it into the motor controller. {% columns %} {% column %}

{% endcolumn %} {% column %}

{% endcolumn %} {% endcolumns %} ### Step 3. Mounting the Buck Converter Install the DC-DC converters on the buck converter mount. Mount the 24V, 6A buck converter on the right side and the 12V, 30A buck converter on the left side. Orient both converters so that their input and output wires face toward the inside of Yor. Secure the assembly using M5 × 100 mm screws on both sides in the following order:\ buck converter mount → buck converter → buck converter mount {% columns %} {% column %}

These screws are just put in to secure it, there are no nuts on them.

{% endcolumn %} {% column %}

{% endcolumn %} {% endcolumns %} Now start stripping and crimping each wire from the 8 motors and motor controller (except the ones connected using the Wago connectors) with the appropriate gauge crimps.

Gently tug on each crimp to make sure it is properly secured.

**Note: Ignore the lift mounting steps if you have already mounted the lift.** ### Step 4. Mounting the Lift To mount the lift, first prepare the lift assembly by attaching the 90-degree brackets using M6 × 10 mm hex screws and washers. Loosely tighten the screws at this stage to allow for alignment during mounting.

Screw the longer leg of the 90-degree bracket to the lift first, ensuring it is properly aligned with the lift frame. {% columns %} {% column %}

{% endcolumn %} {% column %}

{% endcolumn %} {% endcolumns %} Now use the M6 extrusion nuts that were inserted during the base assembly to mount the lift onto the base.

Place the lift onto the base and start threading four M6 10 mm hex screws into the pre-installed M6 extrusion nuts. Do not fully tighten the screws yet. Push the lift forward in the direction indicated by the blue arrow in the reference image until it can no longer move. Once the lift is fully seated, make sure it is square with the base, then fully tighten all four screws to secure the lift in place.

### Step 5. Mounting the Remaining Buck Converters and Fuse box connections Mount the 24V 6A converter, on the right side, on top of the 24V 6A converter, and mount the 5V 5A converter on the left side on top of the 12V 30A converter, you will need to use the 3D-printed mount plate. Attach the fuse box to the lift using double-sided tape, as shown in the figure below. Connect the swerve drive motor controller output wires to the fuse box as shown in the reference image. You can route the wire through the 3D-printed buck converter mount, as shown in the reference image.

This wiring connection is reiterated in the subsequent wiring section for clarity.

Final view of the base after installing the lift assembly and DC-DC buck converters.

# 4. Wiring and Final Assembly We now proceed to the critical wiring stage of YOR. This section covers the primary power and communication connections required for system operation. The block diagram below provides a high-level overview of YOR’s wiring architecture, illustrating how power is distributed and how the major subsystems are connected.

First, connect the motor drive input power wires. Connect the positive leads to the fused power rail and the negative leads to the common ground rail. Ensure that all eight motor drive inputs are properly connected before proceeding.

Note: The buck converter connections to the fuse box are not shown in this image.

Next, connect the output wires (yellow and black) from the 12V 30A DC-DC converter to the positive and negative terminals of the fuse box, respectively. Verify correct polarity before securing the connections. Connect the emergency stop switch to the base using the normally closed contact. Splice the positive rail from the Anderson connector and route it through the normally closed terminals of the emergency stop switch. From the switch, connect the positive output to the terminal block. Connect the negative rail from the Anderson connector directly to the terminal block.

Connect the positive input terminals of all four buck converters together on the positive rail, and connect the negative input terminals together on the negative rail. Before making these connections, ensure that there is no electrical connection between the positive and negative rails. **Note: Verify using a multimeter that there is no continuity between the positive and negative sides before proceeding. This step is critical.**

Now install the battery, behind the lift. Put a white foam sheet that comes in the battery box to secure the battery.

Now attach the 4.75-inch vertical aluminum extrusions at the center of each side of the base frame, using the extrusion nuts and L-brackets installed in [Base Assembly Step 1](https://build.yourownrobot.ai/pages/eY9g7BiKVwKLXDxhtCot#step-1.-setting-up-the-base-extrusions).

Slide two M6 extrusion nuts into each 11.5-inch aluminum extrusion. Using the 90-degree extrusion connectors, mount the 17-inch and 11.5-inch extrusions to complete the base assembly at the locations indicated by the red arrows in the reference image below.

Attach the Raspberry Pi mount to the front vertical 4.75-inch aluminum extrusion above the fuse box using M5 screws and extrusion nuts.

Now we move one to wiring up the lift. Cut off the connector from the cable coming out of the lift, then strip back the outer black sheathing. The cable should contain six individual wires inside. {% columns %} {% column %}

{% endcolumn %} {% column valign="middle" %}

{% endcolumn %} {% endcolumns %} Connect the output of the 24V, 6A DC-DC converter to the input terminals of the lift motor controller (BTS7960), connecting positive to Battery+ and negative to Battery−. Then connect the lift motor wiring as follows: * Lift motor+ (blue wire) → Motor+ on the BTS7960 * Lift motor− (brown wire) → Motor− on the BTS7960 Connect the lift motor wiring to the motor driver and the Raspberry Pi Pico using male-to-female and female-to-female jumper cable. Connect the lift encoder and motor driver wiring as follows: Encoder connections: (Lift to Pico) * Encoder A (Orange) → GPIO 27 * Encoder B (White) → GPIO 28 * Encoder V+ (Red) → 3.3 V * Encoder GND (Black) → GND * Place a 1 µF capacitor across Encoder V+ and Encoder GND BTS7960 motor driver logic connections: (Lift Motor Driver to Pico) * RPWM → GPIO 2 * LPWM → GPIO 3 * R\_EN → GPIO 4 * L\_EN → GPIO 5 * VCC → 3.3 V * GND → GND After completing the wiring, attach the Raspberry Pi Pico and the lift motor controller using double-sided tape, as shown in the reference image.

Extend the output wires from the 24V, 6A DC-DC converter using two inline Wago connectors. Ensure the extended wires are at least 1.2 meters long to allow routing to the arm power system. Attach two 3-port Wago connectors at the opposite end of the extended wires. Also, add two sets of positive and negative wires from the fuse box, each at least 1.2 meters long, to supply power to the Orin and the U2D2. Terminate these wires with barrel jacks, one for the Orin and one for the U2D2 power board. Ensure that none of the output wires from the buck converters are left exposed or hanging loose, except for the 5V, 5A Type-C output used to power the Raspberry Pi. Put eight 10 Amps fuse in for all the swerve motor drivers and two 5 amps fuse in for the Orin and the U2D2. ### Assigning CAN ID's We will now proceed to a critical step: updating the firmware on the swerve drives and assigning the appropriate CAN IDs. Update the firmware on all swerve motors to version 25.X.X, then assign a unique CAN ID to each motor. The front of the robot is defined as the side where the fuse box is located. Based on this reference frame: * Front Left drive motor → CAN ID 1 * Front Left steering motor → CAN ID 5 * Front Right drive motor → CAN ID 4 * Front Right steering motor → CAN ID 8 * Rear Left drive motor → CAN ID 2 * Rear Left steering motor → CAN ID 6 * Rear Right drive motor → CAN ID 3 * Rear Right steering motor → CAN ID 7

To complete this step, you will need a Windows laptop with the [REV Hardware Client](https://docs.revrobotics.com/rev-hardware-client) installed, Wago inline connectors, type-C cable and two 120 Ω resistors. On the laptop, load the motor configurations from the hardware Google Drive. There are two configuration files in .json format: one for the drive motor and one for the steering motor. Place these files in the following directory: ``` C:\Users\$User$\AppData\Roaming\REV Hardware Client\plugins\SparkMax\device-configurations ``` Each motor driver has four CAN wires: two yellow and two green. The yellow wires correspond to CAN High, and the green wires correspond to CAN Low. CAN communication requires two 120 Ω termination resistors placed at the terminal ends of the CAN bus. For this procedure, install both resistors across one set of yellow (CAN High) and green (CAN Low) wires for the motor being configured. Connect the motor driver to the laptop using a USB Type-C cable. Open the REV Hardware Client; the motor driver should appear with CAN ID 0. Load the appropriate configuration file based on the motor type (drive or steering), then update the firmware. **Note: This process is delicate. Ensure all wire connections are stable and secure, especially during the firmware update.** # 5. Mounting the Piper Arms This section covers mounting the Piper arms to the YOR base using the YOR Piper arm attachment plate. After all the motors are updated and configured correctly, we now move on to mounting the arms on the [Piper Arms attachment plate](https://drive.google.com/file/d/1Atg4aZnIYiiF9OHxO3UI7b0Fv37e5vMs/view?usp=sharing). Take the 4.7 mm stainless steel mounting plate and attach the two Piper arms using eight M5 × 25 mm hex bolts and M5 lock nuts. Tighten the fasteners using a ratcheting wrench with an M5 socket and an M5 Allen key. **Note: Make sure orientation of the arms is exactly as shown in the figure.**

**Note: Only the arm orientation differs; all other steps are identical for both the Nero arms and the Piper arms.** Now attach the Jetson mount to the arm plate by sliding the slot on the Jetson mount into the arm plate.

Now, we mount the arms to the lift using the four holes on the arm plate, we need the B screws that comes with the lift and M6 washers. Put the washer in and mount the arms plate using four B screws from the lift pack as shown in the figure.

Now attach the ZED mount and the top cable tray using four M4 × 25 mm hex bolts and nuts. Slide the ZED mount into position as shown in the reference image, then secure it using two M4 hex bolts and nuts. Secure the top cable tray using the remaining two M4 × 25 mm hex bolts.

Now attach the ZED 2i and mount the Orin on top. Once these steps are complete, YOR should match the configuration shown in the reference image. Now we can attach the ZED 2i and mount the Orin on top.

# 5.1 Mounting the Nero Arms This section covers mounting the Nero arms to the YOR base using the YOR Nero arm attachment plate. After all the motors are updated and configured correctly, we now move on to mounting the arms on the [Nero Arms attachment plate](https://drive.google.com/file/d/1CYvqHdz5FVxU29ZuSA-94ZRtbvt0eQae/view?usp=sharing). Take the 4.7 mm stainless steel mounting plate and attach the two Nero arms using eight M5 × 25 mm hex bolts and M5 lock nuts. Tighten the fasteners using a ratcheting wrench with an M5 socket and an M5 Allen key. **Note: Make sure orientation of the arms is exactly as shown in the figure.**

Mount the arms on the plate such that the connectors are facing the floor

# 6. Pico Lift Controller Setup This section covers the setup for the Raspberry Pi Pico (MicroPython) + Raspberry Pi control stack. #### 1) Raspberry Pi Pico (MicroPython) — Firmware + `main.py` Upload **1.1 Flash MicroPython UF2 onto the Pico** 1. Hold **BOOTSEL** on the Pico and plug it into your computer. 2. The Pico mounts as a USB drive called `RPI-RP2`. 3. Drag-and-drop the **MicroPython UF2** file onto `RPI-RP2`. 4. The Pico will reboot automatically. **Verify:** Pico shows up as a serial device after reboot: * **macOS:** `/dev/tty.usbmodem*` * **Linux:** `/dev/ttyACM0` (or similar) **1.2 Install `mpremote` (upload tool)** On the machine you will use to upload code to the Pico (Pi or laptop): ```bash pip install mpremote ``` **1.3 Create `main.py` (Pico Lift Controller CLI)** Save the following as **`main.py`** (this is the code that will run on boot on the Pico). ```python # main.py — Pico Lift CLI with Encoder + Height + Safe Limit Toggle (MicroPython) # Now using BTS7960 H-bridge instead of mechanical relays. # # Motor driver pins (BTS7960, logic side): # RPWM = GP2 # LPWM = GP3 # R_EN = GP4 # L_EN = GP5 # # Encoder: A=GP27, B=GP28 (pull-ups). import machine, utime, sys, json try: import uselect as select except ImportError: import select # ===== Config ===== # Old relay pins (no longer used for motor power – keep them unconnected or for debugging only) UP_PIN_NUM = 0 # (was relay UP) DN_PIN_NUM = 1 # (was relay DOWN) ACTIVE_HIGH = True # kept for compatibility with gpio/debug commands DEADTIME_MS = 10 # break-before-make when reversing # BTS7960 motor driver pins (logic side) RPWM_PIN_NUM = 2 # GP2 -> BTS7960 RPWM LPWM_PIN_NUM = 3 # GP3 -> BTS7960 LPWM LEN_PIN_NUM = 4 # GP4 -> BTS7960 R_EN REN_PIN_NUM = 5 # GP5 -> BTS7960 L_EN MOTOR_PWM_FREQ = 20000 # 20 kHz MOTOR_SPEED = 1.0 # duty (0.0–1.0) ENC_A_PIN_NUM = 27 # GP27 ENC_B_PIN_NUM = 28 # GP28 MM_PER_COUNTS = 0.20018 HOME_HEIGHT_MM = 0.0 # Limit/stop when no encoder ticks while moving: LIMIT_ENABLED = True NO_TICK_LIMIT_MS = 100 GRACE_MS = 100 # Status codes STATUS_READY, STATUS_MAX, STATUS_MIN, STATUS_MOVING, STATUS_HOME = 0, 1, 2, 3, 4 DIR_OFF, DIR_UP, DIR_DOWN = 0, 1, 2 last_move = DIR_OFF # Motion timing & arming for limit logic move_start_ms = utime.ticks_ms() armed_by_tick = False # ===== GPIO: Legacy relay pins (debug only) ===== up_pin = machine.Pin(UP_PIN_NUM, machine.Pin.OUT) dn_pin = machine.Pin(DN_PIN_NUM, machine.Pin.OUT) def _apply(pin, on): if ACTIVE_HIGH: pin.value(1 if on else 0) else: pin.value(0 if on else 1) def relay_on(pin): _apply(pin, True) def relay_off(pin): _apply(pin, False) # ===== BTS7960 Motor Driver ===== rpwm = machine.PWM(machine.Pin(RPWM_PIN_NUM)) lpwm = machine.PWM(machine.Pin(LPWM_PIN_NUM)) rpwm.freq(MOTOR_PWM_FREQ) lpwm.freq(MOTOR_PWM_FREQ) ren = machine.Pin(REN_PIN_NUM, machine.Pin.OUT) len_ = machine.Pin(LEN_PIN_NUM, machine.Pin.OUT) def bts_init(): ren.value(1) len_.value(1) rpwm.duty_u16(0) lpwm.duty_u16(0) def bts_drive(direction): duty = int(65535 * MOTOR_SPEED) if direction == DIR_UP: rpwm.duty_u16(duty) lpwm.duty_u16(0) elif direction == DIR_DOWN: lpwm.duty_u16(duty) rpwm.duty_u16(0) else: rpwm.duty_u16(0) lpwm.duty_u16(0) bts_init() def set_move(direction): global last_move, move_start_ms, armed_by_tick if (last_move == DIR_UP and direction == DIR_DOWN) or (last_move == DIR_DOWN and direction == DIR_UP): bts_drive(DIR_OFF) utime.sleep_ms(DEADTIME_MS) bts_drive(DIR_OFF) if direction in (DIR_UP, DIR_DOWN): bts_drive(direction) move_start_ms = utime.ticks_ms() armed_by_tick = False last_move = direction dir_name = "OFF" if direction == DIR_UP: dir_name = "UP" elif direction == DIR_DOWN: dir_name = "DOWN" print("MOVE ->", dir_name) # ===== Encoder (simple A-channel tick ISR) ===== ENC_A = machine.Pin(ENC_A_PIN_NUM, machine.Pin.IN, machine.Pin.PULL_UP) ENC_B = machine.Pin(ENC_B_PIN_NUM, machine.Pin.IN, machine.Pin.PULL_UP) encoder_count = 0 last_tick_ms = utime.ticks_ms() def enc_isr(pin): global encoder_count, last_tick_ms, armed_by_tick if last_move == DIR_UP: encoder_count += 1 elif last_move == DIR_DOWN: encoder_count -= 1 else: return last_tick_ms = utime.ticks_ms() armed_by_tick = True ENC_A.irq(trigger=machine.Pin.IRQ_RISING, handler=enc_isr) # ===== Height via offset ===== offset_counts = 0.0 def get_height_mm(): return (encoder_count - offset_counts) * MM_PER_COUNTS def set_height_mm(mm): global offset_counts offset_counts = encoder_count - (mm / MM_PER_COUNTS) # ===== Serial non-blocking line input ===== poll = select.poll() poll.register(sys.stdin, select.POLLIN) _buf = "" def readline_nb(): global _buf if not poll.poll(0): return None ch = sys.stdin.read(1) if not ch: return None if ch == "\r": return None if ch == "\n": s = _buf.strip() _buf = "" return s if s else None _buf += ch return None # ===== Reporting ===== def report_json(status_val): print(json.dumps({ "status": int(status_val), "Height": float("{:.2f}".format(get_height_mm())), "Count": int(encoder_count), "Limit": {"enabled": LIMIT_ENABLED, "grace_ms": GRACE_MS} })) def report_line(status_val): names = {0:"READY",1:"MAX",2:"MIN",3:"MOVING",4:"HOMING"} print("[{}] Height: {:.2f} mm | Count: {} | Limit:{} (grace {} ms{})".format( names.get(int(status_val), "?"), get_height_mm(), encoder_count, "ON" if LIMIT_ENABLED else "OFF", GRACE_MS, ", armed" if armed_by_tick else "")) # ===== CLI ===== def help_text(): print("Commands:") print(" up / down / stop") print(" status | count") print(" set | zero") print(" home | homeheight ") print(" json on|off | rate ") print(" limit on|off") print(" grace ") print(" gpio up 0|1 | gpio down 0|1") print(" active high|low") print(" test") print(" help") def prompt(): sys.stdout.write("> ") try: sys.stdout.flush() except: pass status = STATUS_READY homing = False REPORT_HZ = 100 REPORT_DT_MS = int(1000 / REPORT_HZ) last_report_ms = utime.ticks_ms() json_stream = False def cmd_gpio(which, val): v = 1 if int(val) != 0 else 0 if which == "up": set_move(DIR_UP if v == 1 else DIR_OFF) elif which == "down": set_move(DIR_DOWN if v == 1 else DIR_OFF) print("GPIO cmd -> which={}, val={}, last_move={}".format(which, v, last_move)) def handle_cmd(cmd): global homing, status, REPORT_DT_MS, REPORT_HZ, HOME_HEIGHT_MM, ACTIVE_HIGH global LIMIT_ENABLED, GRACE_MS, json_stream c = cmd.strip().lower() if c == "up": homing = False set_move(DIR_UP) status = STATUS_MOVING elif c == "down": homing = False set_move(DIR_DOWN) status = STATUS_MOVING elif c == "stop": set_move(DIR_OFF) status = STATUS_READY elif c == "status": report_line(status) elif c == "count": print("Count:", encoder_count) elif c.startswith("set "): try: mm = float(c.split(None, 1)[1]) set_height_mm(mm) report_line(status) except: print("ERR: usage: set ") elif c == "zero": set_height_mm(0.0) report_line(status) elif c == "home": homing = True set_move(DIR_DOWN) status = STATUS_HOME elif c.startswith("homeheight "): try: HOME_HEIGHT_MM = float(c.split(None, 1)[1]) print("HOME_HEIGHT_MM set to {:.2f} mm".format(HOME_HEIGHT_MM)) except: print("ERR: usage: homeheight ") elif c.startswith("json"): parts = c.split() if len(parts) == 2 and parts[1] in ("on", "off"): json_stream = (parts[1] == "on") print("JSON stream:", "ON" if json_stream else "OFF") else: print("ERR: usage: json on|off") elif c.startswith("rate "): try: hz = int(float(c.split(None, 1)[1])) hz = 1 if hz < 1 else (50 if hz > 50 else hz) REPORT_HZ = hz REPORT_DT_MS = int(1000 / REPORT_HZ) print("Report rate set to {} Hz".format(REPORT_HZ)) except: print("ERR: usage: rate ") elif c.startswith("limit "): parts = c.split() if len(parts) == 2 and parts[1] in ("on", "off"): LIMIT_ENABLED = (parts[1] == "on") print("Limit:", "ON" if LIMIT_ENABLED else "OFF") else: print("ERR: usage: limit on|off") elif c.startswith("grace "): try: GRACE_MS = int(float(c.split(None, 1)[1])) if GRACE_MS < 0: GRACE_MS = 0 print("Grace set to {} ms".format(GRACE_MS)) except: print("ERR: usage: grace ") elif c.startswith("gpio "): parts = c.split() if len(parts) == 3 and parts[1] in ("up", "down"): try: cmd_gpio(parts[1], int(parts[2])) except: print("ERR: gpio up|down 0|1") else: print("ERR: gpio up|down 0|1") elif c.startswith("active "): parts = c.split() if len(parts) == 2 and parts[1] in ("high", "low"): relay_off(up_pin) relay_off(dn_pin) ACTIVE_HIGH = (parts[1] == "high") print("Polarity set to ACTIVE_{}".format("HIGH" if ACTIVE_HIGH else "LOW")) else: print("ERR: active high|low") elif c == "test": print("Test pulse up/down via set_move()...") set_move(DIR_UP); utime.sleep_ms(120); set_move(DIR_OFF); utime.sleep_ms(150) set_move(DIR_DOWN); utime.sleep_ms(120); set_move(DIR_OFF) print("Test done.") elif c in ("help", "h", "?"): help_text() else: print("Unknown cmd. Type 'help'") print("Lift CLI ready. BTS7960 motor driver: RPWM=GP{}, LPWM=GP{}, REN=GP{}, LEN=GP{}." .format(RPWM_PIN_NUM, LPWM_PIN_NUM, REN_PIN_NUM, LEN_PIN_NUM)) print("Encoder: A=GP{}, B=GP{}.".format(ENC_A_PIN_NUM, ENC_B_PIN_NUM)) print("Limit default = {}. Enable/disable with: limit on|off".format("ON" if LIMIT_ENABLED else "OFF")) print("Type 'help' for commands.") prompt() status = STATUS_READY last_report_ms = utime.ticks_ms() while True: line = readline_nb() if line is not None: handle_cmd(line) prompt() now = utime.ticks_ms() if LIMIT_ENABLED and last_move in (DIR_UP, DIR_DOWN): since_move_ms = utime.ticks_diff(now, move_start_ms) no_ticks_ms = utime.ticks_diff(now, last_tick_ms) if since_move_ms >= GRACE_MS and armed_by_tick and no_ticks_ms >= NO_TICK_LIMIT_MS: prev_dir = last_move set_move(DIR_OFF) if homing: irq = machine.disable_irq() encoder_count = 0 offset_counts = 0.0 armed_by_tick = False last_tick_ms = now machine.enable_irq(irq) set_height_mm(HOME_HEIGHT_MM) homing = False status = STATUS_MIN print("HOMED -> encoder_count=0, height set to {:.2f} mm".format(HOME_HEIGHT_MM)) else: if prev_dir == DIR_DOWN: set_height_mm(0.0) status = STATUS_MIN else: status = STATUS_MAX if last_move == DIR_OFF and not homing and status not in (STATUS_MIN, STATUS_MAX): status = STATUS_READY if utime.ticks_diff(now, last_report_ms) >= REPORT_DT_MS: last_report_ms = now if json_stream: report_json(status) else: if status in (STATUS_MOVING, STATUS_HOME): report_line(status) utime.sleep_ms(1) ``` **1.4 Upload `main.py` to the Pico** ```bash mpremote connect auto fs cp main.py :main.py mpremote connect auto reset ``` **Verify (REPL):** ```bash mpremote connect auto repl # inside: import main ``` # 7. Control PC Setup # 8. Teleoperation