diff --git a/.idea/.gitignore b/.idea/.gitignore new file mode 100644 index 0000000..13566b8 --- /dev/null +++ b/.idea/.gitignore @@ -0,0 +1,8 @@ +# Default ignored files +/shelf/ +/workspace.xml +# Editor-based HTTP Client requests +/httpRequests/ +# Datasource local storage ignored files +/dataSources/ +/dataSources.local.xml diff --git a/.idea/inspectionProfiles/profiles_settings.xml b/.idea/inspectionProfiles/profiles_settings.xml new file mode 100644 index 0000000..105ce2d --- /dev/null +++ b/.idea/inspectionProfiles/profiles_settings.xml @@ -0,0 +1,6 @@ + + + + \ No newline at end of file diff --git a/.idea/misc.xml b/.idea/misc.xml new file mode 100644 index 0000000..051da15 --- /dev/null +++ b/.idea/misc.xml @@ -0,0 +1,7 @@ + + + + + + \ No newline at end of file diff --git a/.idea/modules.xml b/.idea/modules.xml new file mode 100644 index 0000000..8582b58 --- /dev/null +++ b/.idea/modules.xml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/.idea/ossm_overkill_edition.iml b/.idea/ossm_overkill_edition.iml new file mode 100644 index 0000000..dfc0d25 --- /dev/null +++ b/.idea/ossm_overkill_edition.iml @@ -0,0 +1,11 @@ + + + + + + + + + + + \ No newline at end of file diff --git a/.idea/vcs.xml b/.idea/vcs.xml new file mode 100644 index 0000000..35eb1dd --- /dev/null +++ b/.idea/vcs.xml @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/README.md b/README.md index 38e4f7a..e8d1617 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,3 @@ # ossm_overkill_edition My take on the open source sex machine, but completely overkill. + diff --git a/software/__init__.py b/software/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/software/apis/__init__.py b/software/apis/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/software/helper_scripts/.downloads/ESP32_GENERIC_S3-20240222-v1.22.2.bin b/software/helper_scripts/.downloads/ESP32_GENERIC_S3-20240222-v1.22.2.bin new file mode 100644 index 0000000..c76fb45 Binary files /dev/null and b/software/helper_scripts/.downloads/ESP32_GENERIC_S3-20240222-v1.22.2.bin differ diff --git a/software/helper_scripts/flash.py b/software/helper_scripts/flash.py new file mode 100644 index 0000000..e736c8a --- /dev/null +++ b/software/helper_scripts/flash.py @@ -0,0 +1,39 @@ +import esptool +import requests +from pathlib import Path + +DOWNLOADS_FOLDER = Path('.downloads') + +DEFAULT_BINARY = "https://micropython.org/resources/firmware/ESP32_GENERIC_S3-20240222-v1.22.2.bin" +DEFAULT_PORT = "/dev/ttyACM0" + + +def save_binary(url: str, file_path: Path): + response = requests.get(url) + response.raise_for_status() + + file_path.parent.mkdir(exist_ok=True) + file_path.write_bytes(response.content) + + +def flash_binary(file_path: Path, port): + esptool.main(["--chip", "esp32s3", "--port", port, "erase_flash"]) + print(["-chip", "esp32s3", "--port", port, "write_flash", "-z", "0", file_path]) + esptool.main(["--chip", "esp32s3", "--port", port, "write_flash", "-z", "0", str(file_path)]) + """ + esptool.py --chip esp32s3 --port /dev/ttyACM0 erase_flash +esptool.py --chip esp32s3 --port /dev/ttyACM0 write_flash -z 0 board-20210902-v1.17.bin +""" + + +if __name__ == '__main__': + binary_url = None or DEFAULT_BINARY + downloads_folder = None or DOWNLOADS_FOLDER + + esp_port = None or DEFAULT_PORT + + binary_file_path = Path(downloads_folder, Path(binary_url).name) + if not binary_file_path.exists(): + save_binary(binary_url, binary_file_path) + + flash_binary(binary_file_path, esp_port) diff --git a/software/motion_controller/__init__.py b/software/motion_controller/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/software/motion_controller/accel_stepper.py b/software/motion_controller/accel_stepper.py new file mode 100644 index 0000000..1a6a345 --- /dev/null +++ b/software/motion_controller/accel_stepper.py @@ -0,0 +1,360 @@ +""" +https://github.com/pedromneto97/AccelStepper-MicroPython/tree/master +""" +from math import sqrt, fabs + +from machine import Pin +from utime import ticks_us, sleep_ms + + +def constrain(val, min_val, max_val): + return min(max_val, max(min_val, val)) + + +DIRECTION_CCW = 0, # Counter-Clockwise +DIRECTION_CW = 1 # Clockwise + +FUNCTION = 0 # Use the functional interface, implementing your own driver functions (internal use only) +DRIVER = 1 # Stepper Driver, 2 driver pins required +FULL2WIRE = 2 # 2 wire stepper, 2 motor pins required +FULL3WIRE = 3 # 3 wire stepper, such as HDD spindle, 3 motor pins required +FULL4WIRE = 4 # 4 wire full stepper, 4 motor pins required +HALF3WIRE = 6 # 3 wire half stepper, such as HDD spindle, 3 motor pins required +HALF4WIRE = 8 # 4 wire half stepper, 4 motor pins required + + +class AccelStepper: + def __init__(self, *args): + self._currentPos = 0 + self._targetPos = 0 + self._speed = 0.0 + self._maxSpeed = 1.0 + self._acceleration = 0.0 + self._sqrt_twoa = 1.0 + self._stepInterval = 0 + self._minPulseWidth = 1 + self._enablePin = 0xff + self._lastStepTime = 0 + self._pin = [0, 0, 0, 0] + self._enableInverted = False + self._n = 0 + self._c0 = 0.0 + self._cn = 0.0 + self._cmin = 1.0 + self._direction = DIRECTION_CCW + self._pinInverted = [0, 0, 0, 0] + + if len(args) == 6: + self._interface = args[0] + self._pin[0] = args[1] + self._pin[1] = args[2] + self._pin[2] = args[3] + self._pin[3] = args[4] + if args[5]: + self.enable_outputs() + elif len(args) == 2: + self._interface = 0 + self._forward = args[0] + self._backward = args[1] + elif len(args) == 3: + self._interface = args[0] + self._pin[0] = args[1] + self._pin[1] = args[2] + + + self.set_acceleration(1) + + def move_to(self, absolute: int) -> None: + if self._targetPos != absolute: + self._targetPos = absolute + self.compute_new_speed() + + def move(self, relative: int) -> None: + self.move_to(self._currentPos + relative) + + def run_speed(self) -> bool: + if not self._stepInterval: + return False + time = ticks_us() + if (time - self._lastStepTime) >= self._stepInterval: + if self._direction == DIRECTION_CW: + self._currentPos += 1 + else: + self._currentPos -= 1 + self.step(self._currentPos) + self._lastStepTime = time + return True + else: + return False + + def distance_to_go(self) -> int: + return self._targetPos - self._currentPos + + def target_position(self) -> int: + return self._targetPos + + def current_position(self) -> int: + return self._currentPos + + def set_current_position(self, position: int) -> None: + self._targetPos = self._currentPos = position + self._n = 0 + self._stepInterval = 0 + self._speed = 0.0 + + def compute_new_speed(self) -> None: + distance_to = self.distance_to_go() + steps_to_stop = int((self._speed * self._speed) / (2.0 * self._acceleration)) + if distance_to == 0 and steps_to_stop <= 1: + self._stepInterval = 0 + self._speed = 0.0 + self._n = 0 + return + if distance_to > 0: + if self._n > 0: + if (steps_to_stop >= distance_to) or self._direction == DIRECTION_CCW: + self._n = -steps_to_stop + elif self._n < 0: + if (steps_to_stop < distance_to) and self._direction == DIRECTION_CW: + self._n = -self._n + elif distance_to < 0: + if self._n > 0: + if (steps_to_stop >= -distance_to) or self._direction == DIRECTION_CW: + self._n = -steps_to_stop + elif self._n < 0: + if (steps_to_stop < -distance_to) and self._direction == DIRECTION_CCW: + self._n = -self._n + if self._n == 0: + self._cn = self._c0 + self._direction = DIRECTION_CW if distance_to > 0 else DIRECTION_CCW + else: + self._cn = self._cn - ((2.0 * self._cn) / ((4.0 * self._n) + 1)) + self._cn = max(self._cn, self._cmin) + self._n += 1 + self._stepInterval = self._cn + self._speed = 1000000.0 / self._cn + if self._direction == DIRECTION_CCW: + self._speed = -self._speed + + def run(self) -> bool: + if self.run_speed(): + self.compute_new_speed() + return self._speed != 0.0 or self.distance_to_go() != 0 + + def set_max_speed(self, speed: float) -> None: + if speed < 0.0: + speed = -speed + if self._maxSpeed != speed: + self._maxSpeed = speed + self._cmin = 1000000.0 / speed + if self._n > 0: + self._n = int((self._speed * self._speed) / (2.0 * self._acceleration)) + self.compute_new_speed() + + def max_speed(self): + return self._maxSpeed + + def set_acceleration(self, acceleration: float) -> None: + if acceleration == 0.0: + return + if acceleration < 0.0: + acceleration = -acceleration + if self._acceleration != acceleration: + self._n = self._n * (self._acceleration / acceleration) + self._c0 = 0.676 * sqrt(2.0 / acceleration) * 1000000.0 + self._acceleration = acceleration + self.compute_new_speed() + + def set_speed(self, speed: float) -> None: + if speed == self._speed: + return + speed = constrain(speed, -self._maxSpeed, self._maxSpeed) + if speed == 0.0: + self._stepInterval = 0 + else: + self._stepInterval = fabs(1000000.0 / speed) + self._direction = DIRECTION_CW if speed > 0.0 else DIRECTION_CCW + self._speed = speed + + def speed(self) -> float: + return self._speed + + def step(self, step: int) -> None: + if self._interface is FUNCTION: + self.step0(step) + elif self._interface is DRIVER: + self.step1(step) + elif self._interface is FULL2WIRE: + self.step2(step) + elif self._interface is FULL3WIRE: + self.step3(step) + elif self._interface is FULL4WIRE: + self.step4(step) + elif self._interface is HALF3WIRE: + self.step6(step) + elif self._interface is HALF4WIRE: + self.step8(step) + + def set_output_pins(self, mask: int) -> None: + num_pins = 2 + if self._interface is FULL4WIRE or self._interface is HALF4WIRE: + num_pins = 4 + elif self._interface is FULL3WIRE or self._interface is HALF3WIRE: + num_pins = 3 + for i in range(num_pins): + self._pin[i].value(True ^ self._pinInverted[i] if mask & (1 << i) else False ^ self._pinInverted[i]) + + def step0(self, step: int) -> None: + if self._speed > 0: + self._forward() + else: + self._backward() + + def step1(self, step: int) -> None: + # self.set_output_pins(0b10 if self._direction else 0b00) + self.set_output_pins(0b11 if self._direction else 0b01) + sleep_ms(self._minPulseWidth) + self.set_output_pins(0b10 if self._direction else 0b00) + + def step2(self, step: int) -> None: + aux = step & 0x3 + if aux == 0: + self.set_output_pins(0b10) + elif aux == 1: + self.set_output_pins(0b11) + elif aux == 2: + self.set_output_pins(0b01) + elif aux == 3: + self.set_output_pins(0b00) + + def step3(self, step: int) -> None: + aux = step % 3 + if aux == 0: + self.set_output_pins(0b100) + elif aux == 1: + self.set_output_pins(0b001) + elif aux == 2: + self.set_output_pins(0b010) + + def step4(self, step: int) -> None: + aux = step & 0x3 + if aux == 0: + self.set_output_pins(0b0101) + elif aux == 1: + self.set_output_pins(0b0110) + elif aux == 2: + self.set_output_pins(0b1010) + elif aux == 3: + self.set_output_pins(0b1001) + + def step6(self, step: int) -> None: + aux = step % 6 + if aux == 0: + self.set_output_pins(0b100) + elif aux == 1: + self.set_output_pins(0b101) + elif aux == 2: + self.set_output_pins(0b001) + elif aux == 3: + self.set_output_pins(0b011) + elif aux == 4: + self.set_output_pins(0b010) + elif aux == 5: + self.set_output_pins(0b110) + + def step8(self, step: int) -> None: + aux = step & 0x7 + if aux == 0: + self.set_output_pins(0b0001) + elif aux == 1: + self.set_output_pins(0b0101) + elif aux == 2: + self.set_output_pins(0b0100) + elif aux == 3: + self.set_output_pins(0b0110) + elif aux == 4: + self.set_output_pins(0b0010) + elif aux == 5: + self.set_output_pins(0b1010) + elif aux == 6: + self.set_output_pins(0b1000) + elif aux == 7: + self.set_output_pins(0b1001) + + def disable_outputs(self) -> None: + if not self._interface: + return + self.set_output_pins(0) + if self._enablePin != 0xff: + self._enablePin = Pin(self._enablePin, Pin.OUT) + self._enablePin.value(False ^ self._enableInverted) + + def enable_outputs(self) -> None: + if not self._interface: + return + self._pin[0] = Pin(self._pin[0], Pin.OUT) + self._pin[1] = Pin(self._pin[1], Pin.OUT) + if self._interface is FULL4WIRE or self._interface is HALF4WIRE: + self._pin[2] = Pin(self._pin[2], Pin.OUT) + self._pin[3] = Pin(self._pin[3], Pin.OUT) + elif self._interface is FULL3WIRE or self._interface is HALF3WIRE: + self._pin[2] = Pin(self._pin[2], Pin.OUT) + if self._enablePin != 0xff: + self._enablePin = Pin(self._enablePin, Pin.OUT) + self._enablePin.value(True ^ self._enableInverted) + + def set_min_pulse_width(self, min_width: int) -> None: + self._minPulseWidth = min_width + + def set_enable_pin(self, enable_pin: int) -> None: + self._enablePin = enable_pin + if self._enablePin != 0xff: + self._enablePin = Pin(self._enablePin, Pin.OUT) + self._enablePin.value(True ^ self._enableInverted) + + def set_pins_inverted(self, *args) -> None: + if len(args) == 3: + self.set_2_pins(args[0], args[1], args[2]) + elif len(args) == 5: + self.set_4_pins(args[0], args[1], args[2], args[3], args[4]) + + def set_2_pins(self, direction_invert: bool, step_invert: bool, enable_invert: bool) -> None: + self._pinInverted[0] = step_invert + self._pinInverted[1] = direction_invert + self._enableInverted = enable_invert + + def set_4_pins(self, pin_1_invert: bool, pin_2_invert: bool, pin_3_invert: bool, pin_4_invert: bool, + enable_invert: bool) -> None: + self._pinInverted[0] = pin_1_invert + self._pinInverted[1] = pin_2_invert + self._pinInverted[2] = pin_3_invert + self._pinInverted[3] = pin_4_invert + self._enableInverted = enable_invert + + def run_to_position(self) -> None: + while self.run(): + pass + + def run_speed_to_position(self) -> bool: + if self._targetPos == self._currentPos: + return False + if self._targetPos > self._currentPos: + self._direction = DIRECTION_CW + else: + self._direction = DIRECTION_CCW + return self.run_speed() + + def run_to_new_position(self, position: int) -> None: + self.move_to(position) + self.run_to_position() + + def stop(self) -> None: + if self._speed != 0.0: + steps_to_stop = int((self._speed * self._speed) / (2.0 * self._acceleration)) + 1 + if self._speed > 0: + self.move(steps_to_stop) + else: + self.move(-steps_to_stop) + + def is_running(self) -> bool: + return not (self._speed == 0 and self._targetPos == self._currentPos) diff --git a/software/motion_controller/main.py b/software/motion_controller/main.py new file mode 100644 index 0000000..3815546 --- /dev/null +++ b/software/motion_controller/main.py @@ -0,0 +1,80 @@ +import machine +from machine import Pin +from time import sleep +from neopixel import NeoPixel +from motion_controller.accel_stepper import AccelStepper, DRIVER +from utime import ticks_us, sleep_us + +"""CL57T(V4.0) Closed-Loop Stepper Driver +10 +Figure 10: Sequence chart of control signals +Remark: +a) t1: ENA must be ahead of DIR by at least 200ms. Usually, ENA+ and ENA- are NC (not connected). See +“Connector P1 Configurations” for more information. +b) t2: DIR must be ahead of PUL effective edge by 2us to ensure correct direction; +c) t3: Pulse width not less than 1us; +d) t4: Low level width not less than 1us; +e) Duty cycle of PUL signal is recommended 50%""" + +def machine_main(): + # machine.freq() # get the current frequency of the CPU + # machine.freq(240000000) # set the CPU frequency to 240 MHz + + thrust_step_pin = Pin("D10", Pin.OUT) + thrust_dir_pin = Pin("D9", Pin.OUT) + thrust_step_pin.value(0) + thrust_dir_pin.value(0) + + def step(): + thrust_step_pin.value(1) + sleep_us(1) + thrust_step_pin.value(0) + + def fwd(): + thrust_dir_pin.value(1) + sleep_us(2) + step() + + def rev(): + thrust_dir_pin.value(0) + sleep_us(2) + step() + + thrust_axis = AccelStepper(fwd, rev) + thrust_axis_endstop_estop_pin = Pin("D21", Pin.IN, Pin.PULL_UP) + + pin = Pin("D6", Pin.OUT) # Pin number for v1 of the above DevKitC, use pin 38 for v1.1 + np = NeoPixel(pin, 3) # "1" = one RGB led on the "led bus" + for pixel in range(3): + np[pixel] = (0, 255, 0) + np.write() + + thrust_steps_per_revolution = 3200 + thrust_max_speed = 20 * thrust_steps_per_revolution + + thrust_homing_speed = 8 * thrust_steps_per_revolution + thrust_max_acceleration = 5 * thrust_steps_per_revolution + + thrust_axis.set_max_speed(thrust_max_speed) + thrust_axis.set_acceleration(thrust_max_acceleration) + thrust_axis.set_speed(thrust_homing_speed) + + thrust_axis.move(-100*thrust_steps_per_revolution) + + print("Homing") + while thrust_axis_endstop_estop_pin.value(): + thrust_axis.run() + print(f"{thrust_axis.current_position()}:{thrust_axis.target_position()}") + print("Homed") + thrust_axis.set_current_position(0) + + thrust_axis.set_max_speed(thrust_max_speed) + thrust_axis.run_to_new_position(200) + + for i in range(3): + thrust_axis.run_to_new_position(5000) + thrust_axis.run_to_new_position(200) + + + +machine_main() diff --git a/software/pendant/__init__.py b/software/pendant/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/software/pendant/main.py b/software/pendant/main.py new file mode 100644 index 0000000..e69de29