├── .gitattributes ├── meshes ├── arm.stl ├── lid.stl ├── base.stl ├── pendulum.dae ├── pendulum.stl └── motor plate.stl ├── diagrams ├── AS5600.drawio ├── DRV8825.drawio ├── Arduino Nano.drawio ├── system-with-batteries.jpg ├── system-with-batteries.drawio ├── system-without-batteries.drawio └── system-without-batteries.jpg ├── RotaryInvertedPendulum-arduino ├── TestSerial │ ├── README.md │ ├── TestSerial.ino │ └── measure_serial_rtt.jl ├── TestHeartbeat │ └── TestHeartbeat.ino ├── LowLevelServer │ ├── StepperUtils.h │ ├── README.md │ ├── LowLevelServer.ino │ └── client.jl ├── TestServer │ ├── README.md │ ├── TestServer.ino │ └── client.jl ├── TestMotor │ └── TestMotor.ino ├── TestEncoder │ └── TestEncoder.ino ├── RotaryInvertedPendulum │ └── RotaryInvertedPendulum.ino └── PIDControl │ └── PIDControl.ino ├── .gitmodules ├── RotaryInvertedPendulum-julia ├── Project.toml └── src │ ├── RotaryInvertedPendulum.jl │ ├── control_gamepad.jl │ └── control_pid.jl ├── LICENSE ├── RotaryInvertedPendulum-python ├── test │ ├── gamepad_test.py │ └── serial_test.py └── src │ └── gamepad_control.py ├── urdf └── model.urdf ├── .gitignore └── README.md /.gitattributes: -------------------------------------------------------------------------------- 1 | *.dae filter=lfs diff=lfs merge=lfs -text 2 | *.stl filter=lfs diff=lfs merge=lfs -text 3 | diagrams/** filter=lfs diff=lfs merge=lfs -text 4 | -------------------------------------------------------------------------------- /meshes/arm.stl: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:03246d09d086a3bdbe425100d10aa6cafbc1088fbb7488506d0c9ad60055339d 3 | size 3484384 4 | -------------------------------------------------------------------------------- /meshes/lid.stl: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:eabf9eba7fb95a99b6e1dbc2493a8772bc1820b9ff1192aa1e3567b4236256c5 3 | size 1723784 4 | -------------------------------------------------------------------------------- /meshes/base.stl: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:ab32db48261bbbe0ffea62ea8e434faea562782d0a71577909db0803e15c11c0 3 | size 4107184 4 | -------------------------------------------------------------------------------- /meshes/pendulum.dae: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:5566b875858e0bdf34fc7a9088acee797f1e8f4a74222767bfa5ef4217191313 3 | size 268268 4 | -------------------------------------------------------------------------------- /meshes/pendulum.stl: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:8d85484b65e7de094aa35383cbc860910acd73a166d444f390ce34117aab7209 3 | size 2637784 4 | -------------------------------------------------------------------------------- /diagrams/AS5600.drawio: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:2a3166acc1bcc55f31e349537492d4c684b63b467e9ec46e787b24c5480f6538 3 | size 232793 4 | -------------------------------------------------------------------------------- /diagrams/DRV8825.drawio: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:eaf98b237403c4c97c5a3d27893553fe084f46d7c7cea4a9b0906fc8eb33cb62 3 | size 1065456 4 | -------------------------------------------------------------------------------- /meshes/motor plate.stl: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:ee3187630f6ef6f56b97c2fba484d813a4d50e6e2e102933d1d5312e8bdba34e 3 | size 4875884 4 | -------------------------------------------------------------------------------- /diagrams/Arduino Nano.drawio: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:3a29a66d09695c80c93be002901bd0060b9c2278d2720dc51b3b4828ff07e0fd 3 | size 759821 4 | -------------------------------------------------------------------------------- /diagrams/system-with-batteries.jpg: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:54b91e26eb9f5367b15ec2e941c8207d3dd9a59a76984e21245d5cce4c8c6b6a 3 | size 1293811 4 | -------------------------------------------------------------------------------- /diagrams/system-with-batteries.drawio: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:e20d31552dff106f106333399639e34107d93e241d274452635922a37c6ce055 3 | size 2975797 4 | -------------------------------------------------------------------------------- /diagrams/system-without-batteries.drawio: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:8845ba31e25f7105afa3b3f97321431a35982a5b51471b2287262539c5231440 3 | size 477855 4 | -------------------------------------------------------------------------------- /diagrams/system-without-batteries.jpg: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:c39ab6d0fe103146e70ece7295c806a05610e806d0f4407ee116d71d69f1152c 3 | size 650566 4 | -------------------------------------------------------------------------------- /RotaryInvertedPendulum-arduino/TestSerial/README.md: -------------------------------------------------------------------------------- 1 | Instructions: 2 | 3 | 1. Flash the `TestSerial.ino` file to the Arduino Nano using e.g. Arduino IDE. 4 | 5 | 2. Run the Julia script to measure the round trip time with: 6 | 7 | ``` 8 | julia --project=. ./scripts/measure_serial_rtt.jl /dev/cu.usbserial-110 115200 9 | ``` 10 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "RotaryInvertedPendulum-arduino/libs/Seeed_Arduino_AS5600"] 2 | path = RotaryInvertedPendulum-arduino/libs/Seeed_Arduino_AS5600 3 | url = https://github.com/Seeed-Studio/Seeed_Arduino_AS5600.git 4 | [submodule "RotaryInvertedPendulum-arduino/libs/AccelStepper"] 5 | path = RotaryInvertedPendulum-arduino/libs/AccelStepper 6 | url = https://github.com/waspinator/AccelStepper.git 7 | -------------------------------------------------------------------------------- /RotaryInvertedPendulum-arduino/TestHeartbeat/TestHeartbeat.ino: -------------------------------------------------------------------------------- 1 | void setup() 2 | { 3 | Serial.begin(115200); 4 | Serial.println("Test Heartbeat"); 5 | pinMode(LED_BUILTIN, OUTPUT); 6 | } 7 | 8 | void loop() 9 | { 10 | digitalWrite(LED_BUILTIN, HIGH); 11 | delay(100); 12 | digitalWrite(LED_BUILTIN, LOW); 13 | delay(100); 14 | digitalWrite(LED_BUILTIN, HIGH); 15 | delay(100); 16 | digitalWrite(LED_BUILTIN, LOW); 17 | delay(1000); 18 | } 19 | -------------------------------------------------------------------------------- /RotaryInvertedPendulum-arduino/TestSerial/TestSerial.ino: -------------------------------------------------------------------------------- 1 | const long BAUD_RATE = 115200; 2 | 3 | void setup() 4 | { 5 | Serial.begin(BAUD_RATE); // Start serial communication at defined baud rate 6 | while (!Serial) { ; } // Wait for the serial port to connect 7 | } 8 | 9 | void loop() 10 | { 11 | if (Serial.available() > 0) 12 | { 13 | char incomingByte = Serial.read(); // Read the incoming byte 14 | Serial.write(incomingByte); // Echo it back to the sender 15 | } 16 | } 17 | -------------------------------------------------------------------------------- /RotaryInvertedPendulum-arduino/LowLevelServer/StepperUtils.h: -------------------------------------------------------------------------------- 1 | #ifndef STEPPER_UTILS_H 2 | #define STEPPER_UTILS_H 3 | 4 | const int microstepsPerRev = 1600; // 200 steps * 8 microsteps 5 | 6 | // Convert steps to degrees 7 | float stepsToDegrees(int steps) 8 | { 9 | return (steps / float(microstepsPerRev)) * 360.0; 10 | } 11 | 12 | // Convert steps to radians 13 | float stepsToRadians(int steps) 14 | { 15 | return (steps / float(microstepsPerRev)) * (2 * PI); 16 | } 17 | 18 | // Convert degrees to steps 19 | int degreesToSteps(float degrees) 20 | { 21 | return round((degrees / 360.0) * microstepsPerRev); 22 | } 23 | 24 | // Convert radians to steps 25 | int radiansToSteps(float radians) 26 | { 27 | return round((radians / (2 * PI)) * microstepsPerRev); 28 | } 29 | 30 | #endif 31 | -------------------------------------------------------------------------------- /RotaryInvertedPendulum-arduino/TestServer/README.md: -------------------------------------------------------------------------------- 1 | This example shows the Arduino running a very simple server for testing purposes. 2 | 3 | The Arduino Nano is keeping track of a sine and cosine wave over time and also listening to incoming requests over the serial via USB. 4 | 5 | If it receives a byte for 'S' or 'C', it replies with the value of the sine or cosine wave, respectively. 6 | 7 | Instructions: 8 | 9 | 1. Flash the `TestServer.ino` file to the Arduino Nano using e.g. Arduino IDE. 10 | 11 | 2. Navigate into the Julia project folder with: 12 | 13 | ``` 14 | cd ~/git/Rotary\ Inverted\ Pendulum/RotaryInvertedPendulum-julia 15 | ``` 16 | 17 | 3. Run the Julia script to measure the average communication frequency with: 18 | 19 | ``` 20 | julia --project=. ../RotaryInvertedPendulum-arduino/TestServer/client.jl 21 | ``` 22 | -------------------------------------------------------------------------------- /RotaryInvertedPendulum-julia/Project.toml: -------------------------------------------------------------------------------- 1 | name = "RotaryInvertedPendulum" 2 | uuid = "c64586be-f5aa-4369-9f08-31eaf2caf4c4" 3 | authors = ["Henrique Ferrolho "] 4 | version = "0.1.0" 5 | 6 | [deps] 7 | Dates = "ade2ca70-3891-5945-98fb-dc099432e06a" 8 | Electron = "a1bb12fb-d4d1-54b4-b10a-ee7951ef7ad3" 9 | Joysticks = "82f05805-4863-42dc-b1a7-0852bd62c632" 10 | LibSerialPort = "a05a14c7-6e3b-5ba9-90a2-45558833e1df" 11 | MeshCat = "283c5d60-a78f-5afe-a0af-af636b173e11" 12 | MeshCatMechanisms = "6ad125db-dd91-5488-b820-c1df6aab299d" 13 | Plots = "91a5bcdd-55d7-5caf-9e0b-520d859cae80" 14 | RigidBodyDynamics = "366cf18f-59d5-5db9-a4de-86a9f6786172" 15 | 16 | [compat] 17 | Electron = "5.1.0" 18 | Joysticks = "0.1" 19 | LibSerialPort = "0.5" 20 | MeshCat = "1.1.0" 21 | MeshCatMechanisms = "0.10.0" 22 | Plots = "1" 23 | RigidBodyDynamics = "2.5.0" 24 | julia = "1.6" 25 | -------------------------------------------------------------------------------- /RotaryInvertedPendulum-arduino/LowLevelServer/README.md: -------------------------------------------------------------------------------- 1 | This example demonstrates real-time control and visualisation of the device over serial via a USB cable. 2 | 3 | The Arduino is running a low-level server that handles commands from the laptop over the serial. 4 | 5 | The Julia "client" script is showing a 3D representation of the device, retrieving its state in real time, as well as controlling the motor to a target position which tracks a sine wave. 6 | 7 | Instructions: 8 | 9 | 1. Flash the `LowLevelServer.ino` file to the Arduino Nano using e.g. Arduino IDE. 10 | 11 | 2. Navigate into the Julia project folder with: 12 | 13 | ``` 14 | cd ~/git/Rotary\ Inverted\ Pendulum/RotaryInvertedPendulum-julia 15 | ``` 16 | 17 | 3. Run the Julia script to measure the average communication frequency with: 18 | 19 | ``` 20 | julia --project=. ../RotaryInvertedPendulum-arduino/LowLevelServer/client.jl --visualise 21 | ``` 22 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2024 Henrique Ferrolho 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /RotaryInvertedPendulum-python/test/gamepad_test.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import pygame 4 | 5 | 6 | def main(): 7 | pygame.init() 8 | 9 | # Set up the Xbox controller 10 | joystick = None 11 | for i in range(pygame.joystick.get_count()): 12 | if "Xbox" in pygame.joystick.Joystick(i).get_name(): 13 | joystick = pygame.joystick.Joystick(i) 14 | joystick.init() 15 | 16 | # Main loop 17 | running = True 18 | while running: 19 | for event in pygame.event.get(): 20 | if event.type == pygame.QUIT: 21 | running = False 22 | 23 | # Get the state of the controller 24 | pygame.event.pump() 25 | axes = [joystick.get_axis(i) for i in range(joystick.get_numaxes())] 26 | buttons = [joystick.get_button(i) for i in range(joystick.get_numbuttons())] 27 | hats = [joystick.get_hat(i) for i in range(joystick.get_numhats())] 28 | 29 | # Print controller state to the console 30 | print("Axes:", axes) 31 | print("Buttons:", buttons) 32 | print("Hats:", hats) 33 | 34 | # Sleep for a short time 35 | pygame.time.wait(100) 36 | 37 | pygame.quit() 38 | 39 | 40 | if __name__ == "__main__": 41 | main() 42 | -------------------------------------------------------------------------------- /RotaryInvertedPendulum-arduino/TestMotor/TestMotor.ino: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | // Define the stepper motor connections 4 | #define dirPin 2 // Direction 5 | #define stepPin 3 // Step 6 | 7 | // Create an instance of the AccelStepper class 8 | AccelStepper stepper(AccelStepper::DRIVER, stepPin, dirPin, 0, 0, false); 9 | 10 | #define stepsPerRevolution 200 * 8 // 200 steps per revolution * 8 microsteps 11 | 12 | void setup() 13 | { 14 | Serial.begin(115200); // Start the serial communication 15 | 16 | // Set the maximum speed and acceleration 17 | stepper.setMaxSpeed(20000); 18 | stepper.setAcceleration(10000); 19 | 20 | // Set the enable pin for the stepper motor driver and 21 | // invert it because we are using a DRV8825 board with an 22 | // active-low enable signal (LOW = enabled, HIGH = disabled) 23 | stepper.setEnablePin(5); 24 | stepper.setPinsInverted(false, false, true); 25 | 26 | // Set the initial position 27 | stepper.setCurrentPosition(0); 28 | 29 | // Enable the motor outputs 30 | stepper.enableOutputs(); 31 | 32 | Serial.println("Motor Test"); 33 | } 34 | 35 | void loop() 36 | { 37 | // Move the motor 1/4 of a revolution 38 | Serial.println("Moving motor 1/4 of a revolution"); 39 | stepper.runToNewPosition(stepsPerRevolution / 4); 40 | 41 | // Wait for 1 second 42 | delay(1000); 43 | 44 | // Move the motor back 1/4 of a revolution 45 | Serial.println("Moving motor back 1/4 of a revolution"); 46 | stepper.runToNewPosition(-stepsPerRevolution / 4); 47 | 48 | // Wait for 1 second 49 | delay(1000); 50 | } 51 | -------------------------------------------------------------------------------- /RotaryInvertedPendulum-julia/src/RotaryInvertedPendulum.jl: -------------------------------------------------------------------------------- 1 | module RotaryInvertedPendulum 2 | 3 | using Dates 4 | using Joysticks 5 | using LibSerialPort 6 | using Plots 7 | 8 | # Command constants 9 | const CHECK_READY_COMMAND = "1" 10 | const GET_POSITION_COMMAND = "2" 11 | const GET_POSITION_PENDULUM_COMMAND = "3" 12 | const SET_TARGET_COMMAND = "4" 13 | const START_MOTOR_COMMAND = "5" 14 | const STOP_MOTOR_COMMAND = "6" 15 | 16 | function wait_until_ready(arduino) 17 | ready = false 18 | response = "" 19 | timed_out = true 20 | 21 | while !ready 22 | try 23 | if timed_out 24 | println("Checking if the Arduino is ready...") 25 | end 26 | 27 | # Wait for the Arduino to send a response 28 | set_read_timeout(arduino, 1) # 1 second 29 | 30 | # Try to read a response from the Arduino 31 | response = readline(arduino) 32 | 33 | if timed_out 34 | println(" -- Arduino responded:") 35 | timed_out = false 36 | end 37 | catch e 38 | if isa(e, LibSerialPort.Timeout) 39 | println(" -- Serial port timed out. Arduino is not ready.") 40 | 41 | # Send the CHECK_READY command to the Arduino 42 | write(arduino, "$CHECK_READY_COMMAND\n") 43 | 44 | timed_out = true 45 | else 46 | rethrow(e) 47 | end 48 | else 49 | # Print the response 50 | println(" >> $response") 51 | 52 | # Check if the Arduino is ready 53 | ready = chomp(response) == "READY" 54 | end 55 | end 56 | 57 | println("Arduino is ready to receive commands.") 58 | end 59 | 60 | include("control_gamepad.jl") 61 | include("control_pid.jl") 62 | 63 | export check_ready 64 | 65 | end # module RotaryInvertedPendulum 66 | -------------------------------------------------------------------------------- /RotaryInvertedPendulum-python/test/serial_test.py: -------------------------------------------------------------------------------- 1 | import serial 2 | import time 3 | 4 | # Command constants 5 | CHECK_READY_COMMAND = "CHECK_READY" 6 | GET_POSITION_COMMAND = "GET_POSITION" 7 | SET_TARGET_COMMAND = "SET_TARGET" 8 | 9 | 10 | def check_ready(arduino): 11 | # Send the CHECK_READY command to the Arduino 12 | arduino.write(f"{CHECK_READY_COMMAND}\n".encode()) 13 | 14 | # Wait for the response 15 | response = arduino.readline().decode().strip() 16 | 17 | # Check if the Arduino is ready 18 | return response == "READY" 19 | 20 | 21 | def test_get_position(arduino): 22 | # Send the GET_POSITION command to the Arduino 23 | arduino.write(f"{GET_POSITION_COMMAND}\n".encode()) 24 | 25 | # Read the response from the Arduino 26 | response = arduino.readline().decode().strip() 27 | 28 | # Print the response 29 | print("Response from Arduino:", response) 30 | 31 | 32 | def test_set_target(arduino, target_position): 33 | # Send the SET_TARGET command to the Arduino 34 | arduino.write(f"{SET_TARGET_COMMAND} {target_position}\n".encode()) 35 | 36 | # Print the target position 37 | print(f"Sent target position: {target_position}") 38 | 39 | 40 | def main(): 41 | # Set up the serial connection 42 | arduino = serial.Serial("/dev/cu.usbserial-110", 9600, timeout=1) 43 | 44 | # Check if the Arduino is ready to receive commands 45 | while not check_ready(arduino): 46 | print("Arduino is not ready. Retrying...") 47 | time.sleep(0.1) # Wait for 100 ms before trying again 48 | 49 | print("Arduino is ready to receive commands.") 50 | 51 | for i in range(10): 52 | test_get_position(arduino) 53 | time.sleep(0.1) 54 | 55 | test_set_target(arduino, 1600) 56 | 57 | for i in range(10): 58 | test_get_position(arduino) 59 | time.sleep(0.1) 60 | 61 | # Close the serial connection 62 | arduino.close() 63 | 64 | 65 | if __name__ == "__main__": 66 | main() 67 | -------------------------------------------------------------------------------- /RotaryInvertedPendulum-arduino/TestServer/TestServer.ino: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | const long BAUD_RATE = 1000000; // Baud rate 4 | 5 | float time_counter = 0.0; // Time variable for wave tracking 6 | const float frequency = 1.0; // Frequency of the sine/cosine waves 7 | unsigned long previous_time = 0; // Previous time in microseconds 8 | 9 | void setup() 10 | { 11 | Serial.begin(BAUD_RATE); // Initialize serial communication 12 | while (!Serial) { ; } // Wait for the serial port to connect 13 | previous_time = micros(); // Initialize the previous time 14 | } 15 | 16 | void loop() 17 | { 18 | unsigned long current_time = micros(); // Get current time in microseconds 19 | float elapsed_time = (current_time - previous_time) / 1e6; // Calculate elapsed time in seconds 20 | previous_time = current_time; // Update the previous time 21 | 22 | time_counter += elapsed_time; // Increment time_counter by the actual elapsed time 23 | 24 | if (Serial.available() > 0) 25 | { 26 | char request = Serial.read(); // Read the incoming request character 27 | 28 | // Respond based on the request 29 | if (request == 'R') 30 | { 31 | // Respond with 'R' to indicate the Arduino is ready 32 | Serial.write('R'); 33 | } 34 | else if (request == 'S') 35 | { 36 | // Calculate sine value and scale to 2-byte signed integer 37 | float sine_value = sin(2 * PI * frequency * time_counter); 38 | int16_t scaled_sine = (int16_t)round(sine_value * 32767.0); 39 | // Send the 2 bytes of the scaled value 40 | Serial.write((byte *)&scaled_sine, sizeof(scaled_sine)); 41 | } 42 | else if (request == 'C') 43 | { 44 | // Calculate cosine value and scale to 2-byte signed integer 45 | float cosine_value = cos(2 * PI * frequency * time_counter); 46 | int16_t scaled_cosine = (int16_t)round(cosine_value * 32767.0); 47 | // Send the 2 bytes of the scaled value 48 | Serial.write((byte *)&scaled_cosine, sizeof(scaled_cosine)); 49 | } 50 | } 51 | } 52 | -------------------------------------------------------------------------------- /urdf/model.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | -------------------------------------------------------------------------------- /RotaryInvertedPendulum-arduino/TestSerial/measure_serial_rtt.jl: -------------------------------------------------------------------------------- 1 | using Dates 2 | using LibSerialPort 3 | 4 | # Function to measure RTT for a single trial 5 | function measure_single_rtt(port, test_byte::UInt8) 6 | t_start = now() 7 | LibSerialPort.write(port, [test_byte]) # Send test byte 8 | 9 | received_byte = UInt8(0) 10 | timeout_seconds = 1 11 | t_timeout = now() + Second(timeout_seconds) 12 | 13 | # Wait for response 14 | while now() < t_timeout 15 | if LibSerialPort.bytesavailable(port) > 0 16 | received_byte = LibSerialPort.read(port, UInt8) 17 | break 18 | end 19 | end 20 | t_end = now() 21 | 22 | return received_byte == test_byte ? Millisecond(t_end - t_start).value : nothing 23 | end 24 | 25 | # Function to measure RTT over multiple trials 26 | function measure_rtt(serial_port::String, baud_rate::Int; trials::Int=10, test_byte::UInt8=0x55) 27 | println("Testing with baud rate: $baud_rate...") 28 | 29 | # Open the serial port 30 | port = LibSerialPort.open(serial_port, baud_rate; parity=LibSerialPort.SP_PARITY_NONE) 31 | LibSerialPort.set_flow_control(port) 32 | sleep(1) # Allow time for the port to stabilize 33 | LibSerialPort.sp_flush(port, LibSerialPort.SP_BUF_BOTH) 34 | 35 | rtt_times = Float64[] 36 | for i in 1:trials 37 | rtt = measure_single_rtt(port, test_byte) 38 | if rtt !== nothing 39 | push!(rtt_times, rtt) 40 | else 41 | println("Warning: No response received on trial $i") 42 | end 43 | end 44 | 45 | LibSerialPort.close(port) # Close port 46 | 47 | # Analyze results 48 | if !isempty(rtt_times) 49 | println("Results for baud rate $baud_rate:") 50 | println(" Min RTT: $(minimum(rtt_times)) ms") 51 | println(" Max RTT: $(maximum(rtt_times)) ms") 52 | println(" Avg RTT: $(sum(rtt_times) / length(rtt_times)) ms") 53 | else 54 | println("No valid RTT measurements for baud rate $baud_rate!") 55 | end 56 | end 57 | 58 | # CLI entrypoint to parse command line arguments 59 | if !isinteractive() 60 | if length(ARGS) < 2 61 | println("Usage: julia --project=. ./scripts/measure_serial_rtt.jl ") 62 | println("Example: julia --project=. ./scripts/measure_serial_rtt.jl /dev/cu.usbserial-110 115200") 63 | return 64 | end 65 | 66 | # Parse CLI arguments 67 | serial_port = ARGS[1] # 1st argument: serial port 68 | baud_rate = parse(Int, ARGS[2]) # 2nd argument: baud rate 69 | trials = 100 # Number of RTT trials 70 | 71 | # Run RTT test 72 | measure_rtt(serial_port, baud_rate; trials=trials, test_byte=0x55) 73 | end 74 | -------------------------------------------------------------------------------- /RotaryInvertedPendulum-julia/src/control_gamepad.jl: -------------------------------------------------------------------------------- 1 | function gamepad_control(baud_rate::Int=2000000, control_frequency::Int=100) 2 | # Initialize motor variables 3 | actual_position_motor = 0 4 | target_position_motor = 0 5 | 6 | # Initialize the joystick 7 | js = open_joystick() 8 | jsaxes = JSState() 9 | jsbuttons = JSButtonState() 10 | async_read!(js, jsaxes, jsbuttons) 11 | 12 | # Joystick to motor velocity multiplier 13 | multiplier = 50.0 14 | 15 | LibSerialPort.open("/dev/cu.usbserial-110", baud_rate) do arduino 16 | # Wait until the Arduino is ready 17 | wait_until_ready(arduino) 18 | 19 | # Initialize variables for tracking time 20 | last_update_time = now() 21 | 22 | # Main loop 23 | running = true 24 | while running 25 | # Set the read and write timeouts 26 | set_read_timeout(arduino, 0.1) 27 | set_write_timeout(arduino, 0.1) 28 | 29 | # Get the current time 30 | current_time = now() 31 | 32 | # Calculate the elapsed time since the last update 33 | elapsed_time = current_time - last_update_time 34 | 35 | if elapsed_time >= Millisecond(1000 / control_frequency) 36 | # Get the actual position from the Arduino 37 | write(arduino, "$GET_POSITION_COMMAND\n") 38 | actual_position_motor = parse(Int, chomp(readline(arduino))) 39 | 40 | # Calculate the motor velocity based on the joystick input 41 | velocity = round(Int, jsaxes.x * multiplier) 42 | 43 | # Calculate the target position 44 | target_position_motor = actual_position_motor + velocity 45 | 46 | # Send the target position to the Arduino 47 | write(arduino, "$SET_TARGET_COMMAND $target_position_motor\n") 48 | 49 | # Update the last update time 50 | last_update_time = current_time 51 | 52 | # Print the actual and target positions 53 | print("Actual position: $actual_position_motor, ") 54 | println("Target position: $target_position_motor") 55 | end 56 | 57 | if jsbuttons.btn1.val # Xbox A-button 58 | # Start the motor 59 | write(arduino, "$START_MOTOR_COMMAND\n") 60 | elseif jsbuttons.btn2.val # Xbox B-button 61 | # Stop the motor 62 | write(arduino, "$STOP_MOTOR_COMMAND\n") 63 | elseif jsbuttons.btn5.val # Xbox Y-button 64 | # Stop the motor 65 | write(arduino, "$STOP_MOTOR_COMMAND\n") 66 | 67 | # Stop the main loop 68 | running = false 69 | end 70 | 71 | # Sleep for a short period of time 72 | sleep(Millisecond(10)) # 10 ms 73 | end 74 | end 75 | end 76 | 77 | export gamepad_control 78 | -------------------------------------------------------------------------------- /RotaryInvertedPendulum-arduino/TestEncoder/TestEncoder.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | // Create an instance of the AS5600 class 5 | AMS_5600 ams5600; 6 | 7 | // Plotting variables 8 | int counterPlot = 0; 9 | int frequencyPlot = 20; 10 | 11 | // Calibration variables 12 | const int numSamples = 100; // Number of samples to average 13 | const int calibrationDelay = 10; // Delay between samples (in milliseconds) 14 | long ams5600_initial_position = 0; 15 | 16 | void setup() 17 | { 18 | Serial.begin(115200); // Start the serial communication 19 | Wire.begin(); // Start the I2C communication 20 | 21 | while (!ams5600.detectMagnet()) 22 | { 23 | Serial.println("[AS5600] Waiting for magnet..."); 24 | delay(1000); // Wait for the magnet to be detected 25 | } 26 | 27 | // Print the current magnitude of the magnet 28 | Serial.print("[AS5600] Current magnitude: "); 29 | Serial.println(ams5600.getMagnitude()); 30 | 31 | // Print the magnet strength 32 | int magStrength = ams5600.getMagnetStrength(); 33 | if (magStrength == 1) 34 | { 35 | Serial.println("[AS5600] Magnet strength is too weak. ---"); 36 | } 37 | else if (magStrength == 2) 38 | { 39 | Serial.println("[AS5600] Magnet strength is just right! ✔"); 40 | } 41 | else if (magStrength == 3) 42 | { 43 | Serial.println("[AS5600] Magnet strength is too strong. +++"); 44 | } 45 | 46 | // Calibrate the initial position by averaging multiple readings 47 | ams5600_initial_position = calibrateRestPosition(); 48 | Serial.print("[AS5600] Calibrated ams5600_initial_position: "); 49 | Serial.println(ams5600_initial_position); 50 | } 51 | 52 | void loop() 53 | { 54 | // Increment counters 55 | counterPlot++; 56 | 57 | // Get the pendulum position 58 | float pendulum_actual_deg = convertRawAngleToDegrees(); 59 | 60 | // Print to the serial 61 | if (counterPlot % frequencyPlot == 0) 62 | { 63 | // Print the actual and target motor positions 64 | Serial.print("pendulum_actual_deg:"); 65 | Serial.print(pendulum_actual_deg); 66 | Serial.println(); 67 | } 68 | } 69 | 70 | /* 71 | * Convert the raw angle from the AS5600 magnetic encoder to degrees. 72 | */ 73 | float convertRawAngleToDegrees() 74 | { 75 | // Get the current position of the AS5600 76 | short ams5600_current_position = ams5600.getRawAngle(); 77 | 78 | // Calculate the difference between the current position and the initial position 79 | short difference = ams5600_current_position - ams5600_initial_position; 80 | 81 | if (difference < 0) 82 | { 83 | difference += 4096; 84 | } 85 | 86 | // Map the 0–4095 segments of the AS5600 to 0–360 degrees 87 | // 360 degrees / 4096 segments = 0.087890625 degrees per segment 88 | return difference * 0.087890625; 89 | } 90 | 91 | /* 92 | * Calibrate the rest position by averaging multiple readings 93 | */ 94 | long calibrateRestPosition() 95 | { 96 | long sum = 0; 97 | 98 | for (int i = 0; i < numSamples; i++) 99 | { 100 | sum += ams5600.getRawAngle(); 101 | delay(calibrationDelay); 102 | } 103 | 104 | return sum / numSamples; 105 | } 106 | -------------------------------------------------------------------------------- /RotaryInvertedPendulum-python/src/gamepad_control.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import pygame 4 | import time 5 | import serial 6 | 7 | # Constants 8 | BAUD_RATE = 115200 # Baud rate of the serial connection 9 | CONTROL_FREQUENCY = 100 # Frequency of the control loop in Hz 10 | 11 | # Command constants 12 | COMMAND_CHECK_READY = "CHECK_READY" 13 | COMMAND_GET_POSITION = "GET_POSITION" 14 | COMMAND_SET_TARGET = "SET_TARGET" 15 | 16 | 17 | def check_ready(arduino): 18 | # Send the CHECK_READY command to the Arduino 19 | arduino.write(f"{COMMAND_CHECK_READY}\n".encode()) 20 | 21 | # Wait for the response 22 | response = arduino.readline().decode().strip() 23 | 24 | # Check if the Arduino is ready 25 | return response == "READY" 26 | 27 | 28 | def get_position(arduino): 29 | """ 30 | Get the current position of the stepper motor from the Arduino. 31 | """ 32 | # Send the GET_POSITION command to the Arduino 33 | arduino.write(f"{COMMAND_GET_POSITION}\n".encode()) 34 | 35 | # Read the response from the Arduino 36 | response = arduino.readline().decode().strip() 37 | 38 | # Print the response 39 | print("Response from Arduino:", response) 40 | 41 | # Return the position 42 | return float(response) 43 | 44 | 45 | def set_target(arduino, target_position: int): 46 | """ 47 | Set the target position of the stepper motor on the Arduino. 48 | """ 49 | # Send the SET_TARGET command to the Arduino 50 | arduino.write(f"{COMMAND_SET_TARGET} {target_position}\n".encode()) 51 | 52 | # Print the target position 53 | print(f"Sent target position: {target_position}") 54 | 55 | 56 | def main(): 57 | # Initialize motor position 58 | actual_position_motor = 0 59 | target_position_motor = 0 60 | 61 | # Initialize pygame 62 | pygame.init() 63 | 64 | # Set up the Xbox controller 65 | joystick = None 66 | for i in range(pygame.joystick.get_count()): 67 | if "Xbox" in pygame.joystick.Joystick(i).get_name(): 68 | joystick = pygame.joystick.Joystick(i) 69 | joystick.init() 70 | 71 | # Set up the serial connection 72 | ser = serial.Serial("/dev/cu.usbserial-110", BAUD_RATE, timeout=1) 73 | 74 | # Wait for Arduino to be ready 75 | while not check_ready(ser): 76 | print("Arduino is not ready. Retrying...") 77 | time.sleep(0.1) # Wait for 100 ms before trying again 78 | 79 | print("Arduino is ready to receive commands.") 80 | 81 | # Initialize variables for tracking time 82 | last_update_time = time.time() 83 | 84 | # Main loop 85 | running = True 86 | while running: 87 | for event in pygame.event.get(): 88 | if event.type == pygame.QUIT: 89 | running = False 90 | 91 | # Get the state of the controller 92 | pygame.event.pump() 93 | x_axis = joystick.get_axis(0) # Get the x-axis value of the left joystick 94 | 95 | # Deadzone for the joystick 96 | if abs(x_axis) < 0.1: 97 | x_axis = 0.0 98 | 99 | # Set the motor speed multiplier 100 | multiplier = 50.0 101 | 102 | # Adjust motor position based on joystick input 103 | target_position_motor += int(x_axis * multiplier) 104 | 105 | # Clamp motor position between 0.0 and 1.0 106 | # target_position_motor = max(0.0, min(1.0, target_position_motor)) 107 | 108 | # Get current time 109 | current_time = time.time() 110 | 111 | # Check if it is time to update the motor 112 | if current_time - last_update_time >= 1.0 / CONTROL_FREQUENCY: 113 | # Get the current position of the motor 114 | actual_position_motor = get_position(ser) 115 | 116 | # Set the target position of the motor 117 | set_target(ser, target_position_motor) 118 | 119 | # Update last update time 120 | last_update_time = current_time 121 | 122 | # Sleep for a short period of time 123 | time.sleep(0.01) # 10 ms 124 | 125 | # Close the serial connection 126 | ser.close() 127 | 128 | # Quit pygame 129 | pygame.quit() 130 | 131 | 132 | if __name__ == "__main__": 133 | main() 134 | -------------------------------------------------------------------------------- /RotaryInvertedPendulum-arduino/LowLevelServer/LowLevelServer.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include "StepperUtils.h" 6 | 7 | // Communication speed 8 | const long BAUD_RATE = 2000000; 9 | 10 | // Command bytes 11 | #define CMD_READY 0x01 12 | #define CMD_GET_STATE 0x02 13 | #define CMD_SET_TARGET 0x03 14 | #define CMD_ENGAGE_MOTOR 0x04 15 | #define CMD_DISENGAGE_MOTOR 0x05 16 | 17 | // Define stepper motor pins 18 | #define DIR_PIN 2 19 | #define STEP_PIN 3 20 | 21 | // State variables 22 | AccelStepper stepper(AccelStepper::DRIVER, STEP_PIN, DIR_PIN, 0, 0, false); 23 | AMS_5600 ams5600; 24 | 25 | volatile bool motor_engaged = false; 26 | long motor_target_position = 0; 27 | 28 | // Timing for state updates 29 | unsigned long current_time = 0; 30 | 31 | // Function prototypes 32 | void handleCommand(); 33 | void sendState(); 34 | float convertRawAngleToRadians(); 35 | 36 | void setup() 37 | { 38 | Serial.begin(BAUD_RATE); 39 | Wire.begin(); 40 | 41 | // Initialize stepper motor 42 | stepper.setEnablePin(5); // Motor enable pin 43 | stepper.setPinsInverted(false, false, true); // DIR, STEP, ENABLE inverted 44 | stepper.setMaxSpeed(200000); // Set maximum motor speed 45 | stepper.setAcceleration(100000); // Set motor acceleration 46 | 47 | // Wait for serial connection 48 | while (!Serial) { ; } 49 | 50 | // Wait for pendulum magnet 51 | while (!ams5600.detectMagnet()) { delay(500); } 52 | } 53 | 54 | void loop() 55 | { 56 | if (Serial.available() > 0) 57 | { 58 | handleCommand(); 59 | } 60 | 61 | if (motor_engaged) 62 | { 63 | stepper.moveTo(motor_target_position); 64 | stepper.run(); 65 | } 66 | } 67 | 68 | /* 69 | * Handle incoming commands. 70 | */ 71 | void handleCommand() 72 | { 73 | uint8_t command = Serial.read(); // Read the command byte 74 | 75 | switch (command) 76 | { 77 | case CMD_READY: 78 | // Respond with a single byte to confirm readiness 79 | Serial.write(CMD_READY); 80 | break; 81 | 82 | case CMD_GET_STATE: 83 | sendState(); 84 | break; 85 | 86 | case CMD_SET_TARGET: 87 | // Wait until 4 bytes (size of float) are available in the buffer 88 | while (Serial.available() < sizeof(float)) { ; } 89 | // Now we can safely read the 4 bytes 90 | float target_radians; 91 | Serial.readBytes((char *)&target_radians, sizeof(float)); 92 | motor_target_position = radiansToSteps(target_radians); 93 | break; 94 | 95 | case CMD_ENGAGE_MOTOR: 96 | motor_engaged = true; 97 | stepper.enableOutputs(); 98 | break; 99 | 100 | case CMD_DISENGAGE_MOTOR: 101 | motor_engaged = false; 102 | stepper.disableOutputs(); 103 | break; 104 | 105 | default: 106 | // Ignore unknown commands 107 | break; 108 | } 109 | } 110 | 111 | /* 112 | * Send the current state of the system: 113 | * - Current time in microseconds (4 bytes) 114 | * - Stepper motor position in radians (4 bytes, float) 115 | * - Pendulum position in radians (4 bytes, float) 116 | */ 117 | void sendState() 118 | { 119 | current_time = micros(); // Get the current time 120 | 121 | float motor_position_radians = stepsToRadians(stepper.currentPosition()); 122 | float pendulum_position_radians = convertRawAngleToRadians(); 123 | 124 | // Flip the signs of the motor and pendulum positions 125 | motor_position_radians *= -1; 126 | pendulum_position_radians *= -1; 127 | 128 | // Pack and send the data 129 | Serial.write((byte *)¤t_time, sizeof(current_time)); 130 | Serial.write((byte *)&motor_position_radians, sizeof(motor_position_radians)); 131 | Serial.write((byte *)&pendulum_position_radians, sizeof(pendulum_position_radians)); 132 | } 133 | 134 | /* 135 | * Convert the raw angle from the AS5600 magnetic encoder to degrees. 136 | */ 137 | float convertRawAngleToRadians() 138 | { 139 | static long raw_prev = 0; 140 | static bool first_reading = true; 141 | static float position = 0.0f; 142 | 143 | // Get the current position of the AS5600 144 | long raw = ams5600.getRawAngle(); 145 | 146 | if (first_reading) 147 | { 148 | raw_prev = raw; 149 | first_reading = false; 150 | } 151 | long delta = raw - raw_prev; 152 | 153 | // Handle wrap around 154 | if (delta > 2047) delta -= 4096; 155 | if (delta < -2047) delta += 4096; 156 | 157 | // Map the 0–4095 segments of the AS5600 to 0–2pi radians (0–360 degrees) 158 | // 2pi radians / 4096 segments = 0.001533981 radians per segment 159 | // 360 degrees / 4096 segments = 0.087890625 degrees per segment 160 | position += (float)delta * 0.001533981; 161 | 162 | // Save the current raw angle for the next iteration 163 | raw_prev = raw; 164 | 165 | return position; 166 | } 167 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # === Global/macOS.gitignore 2 | 3 | # General 4 | .DS_Store 5 | .AppleDouble 6 | .LSOverride 7 | 8 | # Icon must end with two \r 9 | Icon 10 | 11 | # Thumbnails 12 | ._* 13 | 14 | # Files that might appear in the root of a volume 15 | .DocumentRevisions-V100 16 | .fseventsd 17 | .Spotlight-V100 18 | .TemporaryItems 19 | .Trashes 20 | .VolumeIcon.icns 21 | .com.apple.timemachine.donotpresent 22 | 23 | # Directories potentially created on remote AFP share 24 | .AppleDB 25 | .AppleDesktop 26 | Network Trash Folder 27 | Temporary Items 28 | .apdisk 29 | 30 | # === Julia.gitignore 31 | 32 | # Files generated by invoking Julia with --code-coverage 33 | *.jl.cov 34 | *.jl.*.cov 35 | 36 | # Files generated by invoking Julia with --track-allocation 37 | *.jl.mem 38 | 39 | # System-specific files and directories generated by the BinaryProvider and BinDeps packages 40 | # They contain absolute paths specific to the host computer, and so should not be committed 41 | deps/deps.jl 42 | deps/build.log 43 | deps/downloads/ 44 | deps/usr/ 45 | deps/src/ 46 | 47 | # Build artifacts for creating documentation generated by the Documenter package 48 | docs/build/ 49 | docs/site/ 50 | 51 | # File generated by Pkg, the package manager, based on a corresponding Project.toml 52 | # It records a fixed state of all packages used by the project. As such, it should not be 53 | # committed for packages, but should be committed for applications that require a static 54 | # environment. 55 | Manifest.toml 56 | 57 | # === Python.gitignore 58 | 59 | # Byte-compiled / optimized / DLL files 60 | __pycache__/ 61 | *.py[cod] 62 | *$py.class 63 | 64 | # C extensions 65 | *.so 66 | 67 | # Distribution / packaging 68 | .Python 69 | build/ 70 | develop-eggs/ 71 | dist/ 72 | downloads/ 73 | eggs/ 74 | .eggs/ 75 | lib/ 76 | lib64/ 77 | parts/ 78 | sdist/ 79 | var/ 80 | wheels/ 81 | share/python-wheels/ 82 | *.egg-info/ 83 | .installed.cfg 84 | *.egg 85 | MANIFEST 86 | 87 | # PyInstaller 88 | # Usually these files are written by a python script from a template 89 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 90 | *.manifest 91 | *.spec 92 | 93 | # Installer logs 94 | pip-log.txt 95 | pip-delete-this-directory.txt 96 | 97 | # Unit test / coverage reports 98 | htmlcov/ 99 | .tox/ 100 | .nox/ 101 | .coverage 102 | .coverage.* 103 | .cache 104 | nosetests.xml 105 | coverage.xml 106 | *.cover 107 | *.py,cover 108 | .hypothesis/ 109 | .pytest_cache/ 110 | cover/ 111 | 112 | # Translations 113 | *.mo 114 | *.pot 115 | 116 | # Django stuff: 117 | *.log 118 | local_settings.py 119 | db.sqlite3 120 | db.sqlite3-journal 121 | 122 | # Flask stuff: 123 | instance/ 124 | .webassets-cache 125 | 126 | # Scrapy stuff: 127 | .scrapy 128 | 129 | # Sphinx documentation 130 | docs/_build/ 131 | 132 | # PyBuilder 133 | .pybuilder/ 134 | target/ 135 | 136 | # Jupyter Notebook 137 | .ipynb_checkpoints 138 | 139 | # IPython 140 | profile_default/ 141 | ipython_config.py 142 | 143 | # pyenv 144 | # For a library or package, you might want to ignore these files since the code is 145 | # intended to run in multiple environments; otherwise, check them in: 146 | # .python-version 147 | 148 | # pipenv 149 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 150 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 151 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 152 | # install all needed dependencies. 153 | #Pipfile.lock 154 | 155 | # poetry 156 | # Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control. 157 | # This is especially recommended for binary packages to ensure reproducibility, and is more 158 | # commonly ignored for libraries. 159 | # https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control 160 | #poetry.lock 161 | 162 | # pdm 163 | # Similar to Pipfile.lock, it is generally recommended to include pdm.lock in version control. 164 | #pdm.lock 165 | # pdm stores project-wide configurations in .pdm.toml, but it is recommended to not include it 166 | # in version control. 167 | # https://pdm.fming.dev/latest/usage/project/#working-with-version-control 168 | .pdm.toml 169 | .pdm-python 170 | .pdm-build/ 171 | 172 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow and github.com/pdm-project/pdm 173 | __pypackages__/ 174 | 175 | # Celery stuff 176 | celerybeat-schedule 177 | celerybeat.pid 178 | 179 | # SageMath parsed files 180 | *.sage.py 181 | 182 | # Environments 183 | .env 184 | .venv 185 | env/ 186 | venv/ 187 | ENV/ 188 | env.bak/ 189 | venv.bak/ 190 | 191 | # Spyder project settings 192 | .spyderproject 193 | .spyproject 194 | 195 | # Rope project settings 196 | .ropeproject 197 | 198 | # mkdocs documentation 199 | /site 200 | 201 | # mypy 202 | .mypy_cache/ 203 | .dmypy.json 204 | dmypy.json 205 | 206 | # Pyre type checker 207 | .pyre/ 208 | 209 | # pytype static type analyzer 210 | .pytype/ 211 | 212 | # Cython debug symbols 213 | cython_debug/ 214 | 215 | # PyCharm 216 | # JetBrains specific template is maintained in a separate JetBrains.gitignore that can 217 | # be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore 218 | # and can be added to the global gitignore or merged into this file. For a more nuclear 219 | # option (not recommended) you can uncomment the following to ignore the entire idea folder. 220 | #.idea/ 221 | -------------------------------------------------------------------------------- /RotaryInvertedPendulum-arduino/TestServer/client.jl: -------------------------------------------------------------------------------- 1 | using Dates 2 | using LibSerialPort 3 | using Printf 4 | using Statistics 5 | 6 | # Define the serial port and baud rate 7 | serial_port = "/dev/cu.usbserial-110" # Replace with your serial port 8 | baud_rate = 1000000 9 | 10 | # Function to check if the Arduino is ready with retry mechanism 11 | function check_arduino_ready(port; retries=3, timeout_seconds=1) 12 | attempts = 0 13 | while attempts < retries 14 | println("Checking if Arduino is ready... (Attempt $(attempts + 1)/$retries)") # Debug: show attempt number 15 | LibSerialPort.write(port, UInt8('R')) # Send 'R' for ready check 16 | 17 | # Flush the serial buffer 18 | LibSerialPort.flush(port, LibSerialPort.SP_BUF_BOTH) 19 | 20 | # Wait for the response (1 byte for ready acknowledgment) 21 | response = UInt8[] 22 | t_timeout = now() + Second(timeout_seconds) 23 | 24 | while now() < t_timeout 25 | if LibSerialPort.bytesavailable(port) > 0 26 | byte = LibSerialPort.read(port, UInt8) 27 | println("Received byte: $(byte)") # Debug: show each byte received 28 | push!(response, byte) 29 | end 30 | end 31 | 32 | # Check if we received the expected response ('R') 33 | if length(response) == 1 && response[1] == UInt8('R') 34 | println("Arduino is ready!") 35 | return true 36 | else 37 | println("Error: Arduino is not responding correctly. Retrying...") 38 | attempts += 1 39 | sleep(0.5) # Small delay before retrying 40 | end 41 | end 42 | 43 | println("Failed to get a valid response after $retries attempts.") 44 | return false 45 | end 46 | 47 | # Function to query the Arduino for sine or cosine value 48 | function query_wave_value(port, request_char::Char) 49 | # Send the request to the Arduino 50 | # println("Sending request: $request_char") 51 | message = [UInt8(request_char)] 52 | LibSerialPort.sp_blocking_write(port.ref, message, 100) # Blocking write with a timeout of 100ms 53 | 54 | # Prepare to receive 2 bytes 55 | response = zeros(UInt8, 2) # Preallocate a buffer for 2 bytes 56 | nbytes_read = LibSerialPort.sp_blocking_read(port.ref, Ref(response, 1), 2, 500) # Blocking read with a 500ms timeout 57 | 58 | # Check if we received exactly 2 bytes 59 | if nbytes_read == 2 60 | # println("Received full response: $response") 61 | float_value = decode_wave_value(response) 62 | return float_value 63 | else 64 | println("Error: Expected 2 bytes, but received $nbytes_read bytes") 65 | return nothing 66 | end 67 | end 68 | 69 | function decode_wave_value(bytes::Vector{UInt8}) 70 | # Combine the two bytes into a signed 16-bit integer 71 | int_value = reinterpret(Int16, UInt8[bytes[1], bytes[2]])[1] 72 | # Scale back to float in the range [-1.0, 1.0] 73 | return int_value / 32767.0 74 | end 75 | 76 | # Main function to interact with the Arduino 77 | function main() 78 | # Open the serial port 79 | port = LibSerialPort.open(serial_port, baud_rate; parity=LibSerialPort.SP_PARITY_NONE) 80 | LibSerialPort.set_flow_control(port) 81 | 82 | sleep(1) # Allow time for the Arduino to initialize 83 | 84 | # Check if the Arduino is ready with retry mechanism 85 | if check_arduino_ready(port) 86 | # Track the start time of the entire loop 87 | start_time = now() 88 | loop_duration = Second(5) # Run the loop for 5 seconds 89 | last_frequency_time = now() 90 | 91 | # List to store iteration durations for moving average 92 | iteration_times = Float64[] 93 | 94 | # Start the loop for querying the sine and cosine values 95 | while now() - start_time < loop_duration 96 | iteration_start_time = now() # Start time of the current iteration 97 | 98 | # Query the Arduino for sine and cosine values 99 | sine_value = query_wave_value(port, 'S') 100 | cosine_value = query_wave_value(port, 'C') 101 | 102 | # if sine_value !== nothing 103 | # @printf("Sine Value: %.4f\n", sine_value) 104 | # end 105 | # if cosine_value !== nothing 106 | # @printf("Cosine Value: %.4f\n", cosine_value) 107 | # end 108 | 109 | # Track the duration of this iteration 110 | iteration_duration = (now() - iteration_start_time).value / 1000 # Convert to seconds 111 | push!(iteration_times, iteration_duration) 112 | 113 | # # Print the iteration duration 114 | # @printf("Iteration duration: %.4f seconds\n", iteration_duration) 115 | 116 | # Every 0.5 seconds, print the moving average loop frequency 117 | if now() - last_frequency_time >= Millisecond(500) 118 | if !isempty(iteration_times) 119 | avg_freq = 1 / mean(iteration_times) 120 | @printf("Moving average loop frequency: %.2f Hz\n", avg_freq) 121 | end 122 | last_frequency_time = now() # Update the last frequency print time 123 | 124 | # Clear the iteration times list for the next period 125 | empty!(iteration_times) 126 | end 127 | end 128 | else 129 | println("Error: Arduino not ready. Exiting.") 130 | end 131 | 132 | # Close the serial port 133 | LibSerialPort.close(port) 134 | end 135 | 136 | # Only run the main function if the script is executed directly 137 | if abspath(PROGRAM_FILE) == @__FILE__ 138 | main() 139 | end 140 | -------------------------------------------------------------------------------- /RotaryInvertedPendulum-arduino/RotaryInvertedPendulum/RotaryInvertedPendulum.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | // Define the stepper motor connections 6 | #define dirPin 2 // Direction 7 | #define stepPin 3 // Step 8 | 9 | // Create an instance of the AccelStepper class 10 | AccelStepper stepper(AccelStepper::DRIVER, stepPin, dirPin, 0, 0, false); 11 | 12 | // Create an instance of the AS5600 class 13 | AMS_5600 ams5600; 14 | long ams5600_initial_position = 0; 15 | 16 | bool motor_running = false; 17 | long motor_target_pos = 0; 18 | 19 | // Define the commands 20 | #define CHECK_READY "1" 21 | #define GET_POSITION "2" 22 | #define GET_POSITION_PENDULUM "3" 23 | #define SET_TARGET "4" 24 | #define START_MOTOR "5" 25 | #define STOP_MOTOR "6" 26 | 27 | void setup() 28 | { 29 | Serial.begin(2000000); // Start the serial communication 30 | Wire.begin(); // Start the I2C communication 31 | 32 | // Set the maximum speed and acceleration 33 | stepper.setMaxSpeed(200000); 34 | stepper.setAcceleration(100000); 35 | 36 | // Set the enable pin for the stepper motor driver and 37 | // invert it because we are using a DRV8825 board with an 38 | // active-low enable signal (LOW = enabled, HIGH = disabled) 39 | stepper.setEnablePin(5); 40 | stepper.setPinsInverted(false, false, true); 41 | 42 | while (!ams5600.detectMagnet()) 43 | { 44 | Serial.println("[AS5600] Waiting for magnet..."); 45 | delay(1000); // Wait for the magnet to be detected 46 | } 47 | 48 | // Print the current magnitude of the magnet 49 | Serial.print("[AS5600] Current magnitude: "); 50 | Serial.println(ams5600.getMagnitude()); 51 | 52 | // Print the magnet strength 53 | int magStrength = ams5600.getMagnetStrength(); 54 | if (magStrength == 1) 55 | { 56 | Serial.println("[AS5600] Magnet strength is too weak. ---"); 57 | } 58 | else if (magStrength == 2) 59 | { 60 | Serial.println("[AS5600] Magnet strength is just right! ✔"); 61 | } 62 | else if (magStrength == 3) 63 | { 64 | Serial.println("[AS5600] Magnet strength is too strong. +++"); 65 | } 66 | 67 | ams5600_initial_position = ams5600.getRawAngle(); 68 | Serial.print("[AS5600] ams5600_initial_position: "); 69 | Serial.println(ams5600_initial_position); 70 | } 71 | 72 | void loop() 73 | { 74 | while (Serial.available() > 0) 75 | { 76 | char receivedChar = Serial.read(); 77 | if (receivedChar == '\n') 78 | { 79 | handleCommand(); 80 | } 81 | else 82 | { 83 | // Append characters to the received message 84 | parseReceivedMessage(receivedChar); 85 | } 86 | } 87 | 88 | if (motor_running) 89 | { 90 | // Move the motor to the target position 91 | stepper.moveTo(motor_target_pos); 92 | 93 | // Run the stepper motor 94 | stepper.run(); 95 | } 96 | } 97 | 98 | /* 99 | * Convert the raw angle from the AS5600 magnetic encoder to degrees. 100 | */ 101 | float convertRawAngleToDegrees() 102 | { 103 | // Get the current position of the AS5600 104 | short ams5600_current_position = ams5600.getRawAngle(); 105 | 106 | // Calculate the difference between the current position and the initial position 107 | short difference = ams5600_current_position - ams5600_initial_position; 108 | 109 | if (difference < 0) 110 | { 111 | difference += 4096; 112 | } 113 | 114 | // Map the 0–4095 segments of the AS5600 to 0–360 degrees 115 | // 360 degrees / 4096 segments = 0.087890625 degrees per segment 116 | return difference * 0.087890625; 117 | } 118 | 119 | String receivedMessage; 120 | 121 | void parseReceivedMessage(char receivedChar) 122 | { 123 | // Append characters to the received message 124 | receivedMessage += receivedChar; 125 | } 126 | 127 | void handleCommand() 128 | { 129 | // Implement logic to handle different commands 130 | if (receivedMessage.startsWith(CHECK_READY)) 131 | { 132 | // Send a ready signal to indicate that Arduino is ready to receive commands 133 | Serial.println("READY"); 134 | } 135 | else if (receivedMessage.startsWith(GET_POSITION)) 136 | { 137 | // Send the current position of the stepper motor over serial 138 | Serial.println(stepper.currentPosition()); 139 | } 140 | else if (receivedMessage.startsWith(GET_POSITION_PENDULUM)) 141 | { 142 | // Get the pendulum position 143 | float pendulum_actual_deg = convertRawAngleToDegrees(); 144 | 145 | // Send the current position of the pendulum over serial 146 | Serial.println(pendulum_actual_deg); 147 | } 148 | else if (receivedMessage.startsWith(SET_TARGET)) 149 | { 150 | // Extract the target position from the message 151 | // For example, "4 1000" will set the target position to 1000 152 | String targetPosString = receivedMessage.substring(2); 153 | 154 | // Set the new target position 155 | motor_target_pos = targetPosString.toInt(); 156 | } 157 | else if (receivedMessage.startsWith(START_MOTOR)) 158 | { 159 | // Enable the motor outputs 160 | stepper.enableOutputs(); 161 | 162 | // Set the motor_running flag to true 163 | motor_running = true; 164 | } 165 | else if (receivedMessage.startsWith(STOP_MOTOR)) 166 | { 167 | // Stop the motor 168 | stepper.stop(); 169 | 170 | // Disable the motor outputs 171 | stepper.disableOutputs(); 172 | 173 | // Set the motor_running flag to false 174 | motor_running = false; 175 | } 176 | 177 | // Reset the received message 178 | receivedMessage = ""; 179 | } 180 | -------------------------------------------------------------------------------- /RotaryInvertedPendulum-arduino/LowLevelServer/client.jl: -------------------------------------------------------------------------------- 1 | import Electron 2 | import MeshCat 3 | import MeshCatMechanisms 4 | import RigidBodyDynamics as RBD 5 | 6 | using LibSerialPort 7 | using Statistics 8 | using RotaryInvertedPendulum 9 | 10 | # Define the serial port and baud rate 11 | serial_port = "/dev/cu.usbserial-110" # Replace with your serial port 12 | baud_rate = 2000000 13 | 14 | # Command bytes 15 | const CMD_READY = UInt8(0x01) 16 | const CMD_GET_STATE = UInt8(0x02) 17 | const CMD_SET_TARGET = UInt8(0x03) 18 | const CMD_ENGAGE_MOTOR = UInt8(0x04) 19 | const CMD_DISENGAGE_MOTOR = UInt8(0x05) 20 | 21 | # Function to check if the Arduino is ready 22 | function check_arduino_ready(port; retries=3, timeout_seconds=1) 23 | for attempt in 1:retries 24 | println("Checking if Arduino is ready... (Attempt $attempt/$retries)") 25 | LibSerialPort.write(port, [CMD_READY]) 26 | LibSerialPort.flush(port, LibSerialPort.SP_BUF_BOTH) 27 | 28 | response = UInt8[] 29 | t_timeout = time_ns() + timeout_seconds * 1e9 30 | 31 | while time_ns() < t_timeout 32 | if LibSerialPort.bytesavailable(port) > 0 33 | push!(response, LibSerialPort.read(port, UInt8)) 34 | break 35 | end 36 | end 37 | 38 | if length(response) == 1 && response[1] == CMD_READY 39 | println("Arduino is ready!") 40 | return true 41 | else 42 | println("No valid response. Retrying...") 43 | end 44 | 45 | sleep(0.5) 46 | end 47 | println("Failed to get a valid response after $retries attempts.") 48 | return false 49 | end 50 | 51 | # Function to retrieve state from Arduino 52 | function get_pendulum_state(port) 53 | LibSerialPort.write(port, [CMD_GET_STATE]) # Send command 54 | # LibSerialPort.sp_blocking_write(port.ref, [CMD_GET_STATE], 500) # Send command 55 | state_data = zeros(UInt8, 12) # Expect 12 bytes of data (4 + 4 + 4) 56 | nbytes = LibSerialPort.sp_blocking_read(port.ref, Ref(state_data, 1), 12, 500) # Read 12 bytes with timeout 57 | 58 | if nbytes == 12 59 | current_time = reinterpret(Int32, state_data[1:4])[1] # Microseconds 60 | motor_position = reinterpret(Float32, state_data[5:8])[1] # Motor position in radians 61 | pendulum_position = reinterpret(Float32, state_data[9:12])[1] # Pendulum position in radians 62 | return current_time, motor_position, pendulum_position 63 | else 64 | println("Error: Expected 12 bytes, but received $nbytes bytes.") 65 | return nothing 66 | end 67 | end 68 | 69 | # Function to set the motor target position (in radians) 70 | function set_motor_target(port, target_position::Float32) 71 | # Convert target_position to an array of bytes 72 | target_bytes = reinterpret(UInt8, [target_position]) 73 | @assert length(target_bytes) == 4 74 | # Send the command and the target position bytes 75 | LibSerialPort.write(port, [CMD_SET_TARGET; target_bytes]) 76 | # LibSerialPort.sp_blocking_write(port.ref, [CMD_SET_TARGET; target_bytes], 500) 77 | end 78 | 79 | # Function to engage the motor 80 | function engage_motor(port) 81 | LibSerialPort.write(port, [CMD_ENGAGE_MOTOR]) 82 | end 83 | 84 | # Function to disengage the motor 85 | function disengage_motor(port) 86 | LibSerialPort.write(port, [CMD_DISENGAGE_MOTOR]) 87 | end 88 | 89 | # Main function to interact with the Arduino 90 | function main(; visualise=false) 91 | visualise && @info "Visualising flag has been set to true. Opening visualiser..." 92 | 93 | if visualise 94 | app = Electron.Application() 95 | vis = MeshCat.Visualizer() 96 | open(vis, app) 97 | 98 | MeshCat.setprop!(vis["/Cameras/default/rotated/"], "fov", 40) 99 | 100 | package_path = joinpath(pkgdir(RotaryInvertedPendulum), "..") 101 | filename = joinpath(package_path, "urdf/model.urdf") 102 | mechanism = RBD.parse_urdf(filename) 103 | 104 | state = RBD.MechanismState(mechanism) 105 | urdfvisuals = MeshCatMechanisms.URDFVisuals(filename, package_path=[package_path]) 106 | mvis = MeshCatMechanisms.MechanismVisualizer(state.mechanism, urdfvisuals, vis["model"]) 107 | 108 | last_vis_update_time = time_ns() 109 | end 110 | 111 | port = LibSerialPort.open(serial_port, baud_rate; parity=LibSerialPort.SP_PARITY_NONE) 112 | LibSerialPort.set_flow_control(port) 113 | 114 | sleep(1) # Allow time for Arduino to initialize 115 | 116 | if !check_arduino_ready(port) 117 | println("Error: Arduino not ready. Exiting.") 118 | LibSerialPort.close(port) 119 | return 120 | end 121 | 122 | println("Engaging motor...") 123 | engage_motor(port) 124 | 125 | println("Starting data retrieval and control loop...") 126 | 127 | start_time = time_ns() 128 | loop_duration_s = 120 # in seconds 129 | loop_duration_ns = loop_duration_s * 1e9 130 | iteration_times = Float64[] 131 | last_frequency_time = time_ns() 132 | 133 | rate_stats_print = 0.5 * 1e9 # every 0.5 seconds in nanoseconds 134 | first_loop = true 135 | initial_arduino_time = 0 136 | 137 | vis_fps = 60 # frames per second of the visualisation 138 | vis_update_interval = 1 / vis_fps # in seconds 139 | vis_update_interval_ns = vis_update_interval * 1e9 140 | 141 | while time_ns() - start_time < loop_duration_ns 142 | iteration_start_time = time_ns() 143 | 144 | state = get_pendulum_state(port) 145 | if state !== nothing 146 | current_time, motor_position, pendulum_position = state 147 | 148 | if first_loop 149 | initial_arduino_time = current_time 150 | first_loop = false 151 | end 152 | 153 | # Update visualization only if sufficient time has passed 154 | if visualise && (iteration_start_time - last_vis_update_time) >= vis_update_interval_ns 155 | q = [motor_position, pendulum_position] 156 | RBD.set_configuration!(mvis, q) 157 | last_vis_update_time = iteration_start_time 158 | end 159 | 160 | # Compute target position 161 | 𝐴 = 45 # Amplitude of the sine wave (degrees) 162 | 𝑓 = 1.0 # Frequency of the sine wave (Hz) 163 | 𝑡 = (current_time - initial_arduino_time) / 1e6 # Convert elapsed time from microseconds to seconds 164 | target_position = deg2rad(𝐴) * sin(2π * 𝑓 * 𝑡) # Sine wave 165 | 166 | # Send target position to motor 167 | set_motor_target(port, Float32(target_position)) 168 | 169 | # println("Time: $(current_time) µs, Motor: $(motor_position) rad, Pendulum: $(pendulum_position) rad, Target: $(target_position) rad") 170 | end 171 | 172 | iteration_duration = (time_ns() - iteration_start_time) / 1e9 # Duration in seconds 173 | push!(iteration_times, iteration_duration) 174 | 175 | if time_ns() - last_frequency_time >= rate_stats_print 176 | if !isempty(iteration_times) 177 | avg_freq = 1 / mean(iteration_times) 178 | println("Average loop frequency: $(round(avg_freq, digits=2)) Hz") 179 | empty!(iteration_times) # Reset for next calculation 180 | end 181 | last_frequency_time = time_ns() 182 | end 183 | end 184 | 185 | println("Disengaging motor...") 186 | disengage_motor(port) 187 | 188 | println("Data retrieval and control complete.") 189 | LibSerialPort.close(port) 190 | 191 | if visualise 192 | MeshCat.close(app) 193 | end 194 | end 195 | 196 | # Execute the main function if run directly 197 | if abspath(PROGRAM_FILE) == @__FILE__ 198 | visualise = "--visualise" in ARGS 199 | main(visualise=visualise) 200 | end 201 | -------------------------------------------------------------------------------- /RotaryInvertedPendulum-julia/src/control_pid.jl: -------------------------------------------------------------------------------- 1 | function convertDegreesToSteps(degrees; microstepping=8) 2 | steps_per_revolution = 200 * microstepping 3 | degrees_per_revolution = 360.0 4 | steps_per_degree = steps_per_revolution / degrees_per_revolution 5 | steps = round(Int, degrees * steps_per_degree) 6 | return steps 7 | end 8 | 9 | # [hf] Is this the best way to define the State module? 10 | baremodule State 11 | WAITING = 1 12 | BALANCING = 2 13 | end 14 | 15 | function pid_control(baud_rate=2000000, control_frequency=200) 16 | # Initialise motor variables 17 | actual_position_motor = 0 18 | target_position_motor = 0 19 | 20 | # Initialise pendulum variables 21 | actual_position_pendulum = 0.0 22 | target_position_pendulum = 180.0 23 | 24 | # Ziegler-Nichols parameters 25 | Ku = 2.0 26 | Tu = 100 27 | 28 | # Initialise the PID controller variables 29 | pid_prev_error = 0.0 30 | pid_integral = 0.0 31 | 32 | # [hf] could the joystick be slowing down the loop? 33 | # # Initialise the joystick 34 | # js = open_joystick() 35 | # jsaxes = JSState() 36 | # jsbuttons = JSButtonState() 37 | # async_read!(js, jsaxes, jsbuttons) 38 | 39 | # Initialise the system state 40 | state = State.WAITING 41 | 42 | LibSerialPort.open("/dev/cu.usbserial-110", baud_rate) do arduino 43 | # Wait until the Arduino is ready 44 | wait_until_ready(arduino) 45 | 46 | # Initialise variables for tracking time 47 | last_update_time = time() 48 | 49 | # Main loop 50 | running = true 51 | while running 52 | # Set the read and write timeouts 53 | set_read_timeout(arduino, 0.05) # 50 ms 54 | set_write_timeout(arduino, 0.01) # 10 ms 55 | 56 | # # Update Ku based on the joystick input 57 | # Ku += 0.01 * -jsaxes.y 58 | # Ku = clamp(Ku, 0.0, 10.0) 59 | # # Update Tu based on the joystick input 60 | # Tu += 0.1 * -jsaxes.u 61 | # Tu = clamp(Tu, 0.0, 200.0) 62 | 63 | # # Temporary values for finding the critical gain 64 | # Kp = Ku 65 | # Ki = 0.0 66 | # Kd = 0.0 67 | 68 | # # Calculate the PID controller gains 69 | # Kp = 0.6 * Ku 70 | # Ki = 2.0 * Kp / Tu 71 | # Kd = Kp * Tu / 8.0 72 | 73 | # Manually-tuned PID controller gains 74 | Kp = 2.2 75 | Ki = 1.6 76 | Kd = 0.005 77 | 78 | # Get the actual position of the motor from the Arduino 79 | write(arduino, "$GET_POSITION_COMMAND\n") 80 | actual_position_motor = parse(Int, chomp(readline(arduino))) 81 | 82 | # Get the actual position of the pendulum from the Arduino 83 | write(arduino, "$GET_POSITION_PENDULUM_COMMAND\n") 84 | actual_position_pendulum = parse(Float64, chomp(readline(arduino))) 85 | 86 | # Calculate the error of the pendulum position 87 | pid_error = target_position_pendulum - actual_position_pendulum 88 | 89 | # Only send the control signal if the pendulum 90 | # is within this margin of the target position 91 | margin_in_deg = 25.0 # in degrees 92 | pendulum_close_to_vertical = abs(pid_error) < margin_in_deg 93 | 94 | if state == State.WAITING 95 | # If the pendulum is close to the vertical position, 96 | # switch to the balancing state 97 | if pendulum_close_to_vertical 98 | println("Switching state to 'balancing'") 99 | # Switch the state to 'balancing' 100 | state = State.BALANCING 101 | # Engage the motor 102 | write(arduino, "$START_MOTOR_COMMAND\n") 103 | # Set the motor target position to the current position 104 | target_position_motor = actual_position_motor 105 | end 106 | elseif state == State.BALANCING 107 | # If the pendulum is not close to the vertical position, 108 | # switch to the waiting state 109 | if !pendulum_close_to_vertical 110 | println("Switching state to 'waiting'") 111 | # Switch the state to 'waiting' 112 | state = State.WAITING 113 | # Disengage the motor 114 | write(arduino, "$STOP_MOTOR_COMMAND\n") 115 | # Reset the PID controller variables 116 | pid_prev_error = 0.0 117 | pid_integral = 0.0 118 | end 119 | end 120 | 121 | # Get the current time 122 | current_time = time() 123 | 124 | # Calculate the elapsed time since the last update 125 | elapsed_time = current_time - last_update_time 126 | 127 | # Update the last update time 128 | last_update_time = current_time 129 | 130 | if elapsed_time >= 1 / control_frequency 131 | if state == State.BALANCING 132 | # Calculate the integral 133 | pid_integral += pid_error * elapsed_time 134 | 135 | # Calculate the derivative 136 | pid_derivative = (pid_error - pid_prev_error) / elapsed_time 137 | pid_prev_error = pid_error # Update the last error 138 | 139 | # Calculate the output of the PID controller 140 | output = Kp * pid_error + Ki * pid_integral + Kd * pid_derivative 141 | 142 | # Clamp the output to prevent the motor from moving too fast 143 | output_limit = convertDegreesToSteps(10.0) 144 | output = clamp(output, -output_limit, output_limit) 145 | 146 | # Calculate the target position of the motor 147 | target_position_motor = actual_position_motor + round(Int, output) 148 | 149 | # Clamp the target position to prevent the motor from choking the wires 150 | motor_limit = convertDegreesToSteps(90.0) 151 | target_position_motor = clamp(target_position_motor, -motor_limit, motor_limit) 152 | 153 | # Send the target position to the Arduino 154 | write(arduino, "$SET_TARGET_COMMAND $target_position_motor\n") 155 | end 156 | end 157 | 158 | let 159 | # Print pendulum position and loop frequency 160 | println("Actual pendulum position: $actual_position_pendulum, Loop frequency: $(1 / elapsed_time)") 161 | 162 | # # Print the Ziegler-Nichols parameters and the target motor position 163 | # println("Ku: $Ku, Tu: $Tu, actual_position_pendulum: $actual_position_pendulum, target_position_motor: $target_position_motor") 164 | 165 | # # Print the PID controller gains 166 | # println("Kp: $Kp, Ki: $Ki, Kd: $Kd") 167 | 168 | # # Print the actual and target motor positions 169 | # print("Actual position: $actual_position_motor, ") 170 | # println("Target position: $target_position_motor") 171 | 172 | # # Print the actual and target pendulum positions 173 | # print("Actual pendulum position: $actual_position_pendulum, ") 174 | # println("Target pendulum position: $target_position_pendulum") 175 | 176 | # # Print the PID controller gains and the actual pendulum position 177 | # println("Kp: $Kp, Ki: $Ki, Kd: $Kd, Actual pendulum position: $actual_position_pendulum, Actual motor position: $actual_position_motor, Target motor position: $target_position_motor") 178 | end 179 | 180 | # if jsbuttons.btn1.val # Xbox A-button 181 | # println("[Xbox] A-button pressed") 182 | # # Start the motor 183 | # write(arduino, "$START_MOTOR_COMMAND\n") 184 | # elseif jsbuttons.btn2.val # Xbox B-button 185 | # println("[Xbox] B-button pressed") 186 | # # Stop the motor 187 | # write(arduino, "$STOP_MOTOR_COMMAND\n") 188 | # elseif jsbuttons.btn5.val # Xbox Y-button 189 | # println("[Xbox] Y-button pressed") 190 | # # Stop the motor 191 | # write(arduino, "$STOP_MOTOR_COMMAND\n") 192 | # # Stop the main loop 193 | # running = false 194 | # end 195 | 196 | # Sleep for a short period of time 197 | # sleep(Millisecond(1)) 198 | end 199 | end 200 | end 201 | 202 | export pid_control 203 | -------------------------------------------------------------------------------- /RotaryInvertedPendulum-arduino/PIDControl/PIDControl.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | // Define the stepper motor connections 6 | #define dirPin 2 // Direction 7 | #define stepPin 3 // Step 8 | #define LED_PIN 13 9 | 10 | #define frac(x) (int(1000 * (x - int(x)))) 11 | 12 | // Create an instance of the AccelStepper class 13 | AccelStepper stepper(AccelStepper::DRIVER, stepPin, dirPin, 0, 0, false); 14 | 15 | #define stepsPerRevolution 200 * 8 // 200 steps per revolution * 8 microsteps 16 | 17 | // Create an instance of the AS5600 class 18 | AMS_5600 ams5600; 19 | float pendulum_initial_position = 0.0f; 20 | float pendulum_target_deg = 180.0; 21 | float motor_target_pos = 0.0; 22 | 23 | float prevError = 0.0; 24 | float integral = 0.0; 25 | 26 | // // Variables for PID control 27 | // float Kp = 3.0; 28 | // float Ki = 0.0; 29 | // float Kd = 0.0; 30 | 31 | // Ziegler-Nichols parameters 32 | float Ku = 2.0; // critical gain 33 | float Tu = 0.1; // oscillation period 34 | 35 | // Variables for PID control 36 | float Kp = 0.6 * Ku; 37 | float Ki = 2 * Kp / Tu; 38 | float Kd = Kp * Tu / 8; 39 | 40 | // Define the control frequency and period 41 | const int controlFrequency = 1000; // in Hertz 42 | const float controlPeriod = 1 / controlFrequency; // in seconds 43 | 44 | // Define variables for moving average filter 45 | double pendulum_actual_deg; 46 | 47 | // State machine variables 48 | enum State 49 | { 50 | WAITING, 51 | BALANCING 52 | }; 53 | State state = WAITING; 54 | 55 | bool can_print = false; 56 | 57 | void tare_pendulum_encoder() 58 | { 59 | pendulum_initial_position = 0.0; 60 | pendulum_initial_position = convertRawAngleToDegrees(); 61 | } 62 | 63 | // Computes the smoothing coefficient for an exponential low-pass filter based on the desired cutoff frequency. 64 | // This coefficient, `alpha`, determines how much of the previous state and the new value contribute to the filtered result. 65 | // 66 | // The coefficient is calculated based on the following parameters: 67 | // - `freq`: Desired cutoff frequency in Hz (cycles per second). This defines the point below which the signal will pass 68 | // through the filter and above which it will be attenuated. Frequencies higher than this cutoff will be filtered out. 69 | // - `dt`: Time interval between updates (in seconds). This value is the time step between consecutive filter updates. 70 | // 71 | // The coefficient `alpha` is bounded between 0.0 and 1.0: 72 | // - 0.0 means no smoothing (only the latest value is used, and no memory of previous states). 73 | // - 1.0 means no update (only the previous state is retained, and the new values are ignored). 74 | // 75 | // The function computes `alpha` using the following formula, where `omega` is the angular frequency corresponding to the desired cutoff frequency: 76 | // omega = 2 * π * freq 77 | // alpha = (1 - omega * dt / 2) / (1 + omega * dt / 2) 78 | // 79 | // After computing the coefficient, it is clamped between 0.0 and 1.0 to ensure stability and prevent extreme values. 80 | // 81 | // Example usage: 82 | // double alpha = alpha_from_freq(500.0, 0.002); // For a 500 Hz cutoff with a 2ms time step. 83 | // 84 | // Returns: 85 | // The clamped smoothing coefficient `alpha`, which is used in the exponential low-pass filter. 86 | double alpha_from_freq(double freq, double dt) 87 | { 88 | // Compute the angular frequency (omega) 89 | double omega = 2.0 * M_PI * freq; 90 | 91 | // Compute the smoothing coefficient (alpha) 92 | double coeff = (1.0 - omega * dt / 2.0) / (1.0 + omega * dt / 2.0); 93 | 94 | // Clamp the coefficient between 0.0 and 1.0 to prevent instability 95 | if (coeff < 0.0) 96 | { 97 | coeff = 0.0; 98 | } 99 | if (coeff > 1.0) 100 | { 101 | coeff = 1.0; 102 | } 103 | 104 | return coeff; 105 | } 106 | 107 | void print_magnet_info() 108 | { 109 | // Magnet strength 110 | int magStrength = ams5600.getMagnetStrength(); 111 | if (magStrength == 1) 112 | { 113 | Serial.println("[AS5600] Magnet strength is too weak. ---"); 114 | } 115 | else if (magStrength == 2) 116 | { 117 | Serial.println("[AS5600] Magnet strength is just right! ✔"); 118 | } 119 | else if (magStrength == 3) 120 | { 121 | Serial.println("[AS5600] Magnet strength is too strong. +++"); 122 | } 123 | // Magnet current 124 | Serial.print("[AS5600] Current magnitude: "); 125 | Serial.println(ams5600.getMagnitude()); 126 | } 127 | 128 | void print_plot(float target, float pos) 129 | { 130 | static unsigned long t0 = 0; 131 | if (can_print) 132 | { 133 | unsigned long t1 = millis(); 134 | if (t1 - t0 > 10) 135 | { 136 | t0 = t1; 137 | // Print the actual and target motor positions 138 | Serial.print(stepper.currentPosition()); 139 | Serial.print(","); 140 | Serial.print(target); 141 | Serial.print(","); 142 | Serial.println(pos); 143 | } 144 | } 145 | } 146 | 147 | void check_serial() 148 | { 149 | if (Serial.available()) 150 | { 151 | char buffer[1024]; 152 | int size = Serial.readBytes(buffer, 1024); 153 | if (size >= 0) 154 | { 155 | char cmd = buffer[0]; 156 | switch (cmd) 157 | { 158 | case 'P': 159 | case 'p': 160 | can_print = !can_print; 161 | break; 162 | case 'M': 163 | case 'm': 164 | print_magnet_info(); 165 | break; 166 | case 'T': 167 | case 't': 168 | tare_pendulum_encoder(); 169 | break; 170 | } 171 | } 172 | } 173 | } 174 | 175 | void setup() 176 | { 177 | Serial.begin(115200); // Start the serial communication 178 | Wire.begin(); // Start the I2C communication 179 | 180 | pinMode(LED_PIN, OUTPUT); 181 | digitalWrite(LED_PIN, HIGH); 182 | 183 | // Set the maximum speed and acceleration 184 | stepper.setMaxSpeed(200000); 185 | stepper.setAcceleration(100000); 186 | 187 | // Set the enable pin for the stepper motor driver and 188 | // invert it because we are using a DRV8825 board with an 189 | // active-low enable signal (LOW = enabled, HIGH = disabled) 190 | stepper.setEnablePin(5); 191 | stepper.setPinsInverted(false, false, true); 192 | 193 | while (!ams5600.detectMagnet()) 194 | { 195 | delay(1000); // Wait for the magnet to be detected 196 | } 197 | 198 | // Set initial position 199 | tare_pendulum_encoder(); 200 | 201 | digitalWrite(LED_PIN, LOW); 202 | 203 | pendulum_actual_deg = convertRawAngleToDegrees(); 204 | } 205 | 206 | void blink() 207 | { 208 | static unsigned long t0 = 0; 209 | static bool is_on = false; 210 | int period = can_print ? 500 : 100; 211 | unsigned long t1 = millis(); 212 | if (t1 - t0 > period) 213 | { 214 | digitalWrite(LED_PIN, is_on ? HIGH : LOW); 215 | is_on = !is_on; 216 | t0 = t1; 217 | } 218 | } 219 | 220 | void loop() 221 | { 222 | blink(); 223 | 224 | check_serial(); 225 | 226 | static unsigned long prevTimeUS = 0; 227 | unsigned long currentTimeUS = micros(); 228 | unsigned long dt = currentTimeUS - prevTimeUS; 229 | float elapsedTime = (float)dt * 1e-6; 230 | prevTimeUS = currentTimeUS; 231 | 232 | // first, we wait for a person to move the pendulum close to the vertical position. 233 | // then we start the motor and we try to balance it. 234 | // we should not command the motor beyond +-90 degrees from its starting position. 235 | 236 | double alpha = alpha_from_freq(500.0, (double)dt * 1e-6); 237 | // Get the pendulum position 238 | pendulum_actual_deg = alpha * pendulum_actual_deg + (1.0 - alpha) * convertRawAngleToDegrees(); 239 | 240 | // Find closest upright target 241 | int revs = pendulum_actual_deg / 360; 242 | pendulum_target_deg = 180.0f * (pendulum_actual_deg > 0 ? 1.0f : -1.0f) + 360.0f * (float)revs; 243 | 244 | float margin_in_deg = 25.0; // in degrees 245 | bool pendulum_close_to_vertical = fabs(pendulum_target_deg - pendulum_actual_deg) <= margin_in_deg; 246 | 247 | if (state == WAITING) 248 | { 249 | if (pendulum_close_to_vertical) 250 | { 251 | // Switch the state to 'balancing' 252 | state = BALANCING; 253 | 254 | // Enable the motor outputs 255 | stepper.enableOutputs(); 256 | 257 | // Set the motor target position to the current position 258 | motor_target_pos = stepper.currentPosition(); 259 | } 260 | } 261 | else if (state == BALANCING) 262 | { 263 | if (!pendulum_close_to_vertical) 264 | { 265 | // Switch the state to 'waiting' 266 | state = WAITING; 267 | 268 | // Stop the motor 269 | stepper.stop(); 270 | 271 | // Disable the motor outputs 272 | stepper.disableOutputs(); 273 | 274 | // Reset the PID control variables 275 | prevError = 0.0; 276 | integral = 0.0; 277 | } 278 | } 279 | 280 | if (state == BALANCING && elapsedTime >= controlPeriod) 281 | { 282 | // Calculate the error 283 | float error = pendulum_target_deg - pendulum_actual_deg; 284 | 285 | // Calculate the integral 286 | integral += error * elapsedTime; 287 | 288 | // Calculate the derivative 289 | float derivative = (error - prevError) / elapsedTime; 290 | prevError = error; 291 | 292 | // Calculate the PID output 293 | float output = Kp * error + Ki * integral + Kd * derivative; 294 | 295 | // Limit the output to prevent the motor from moving too fast 296 | long output_limit = convertDegreesToSteps(10); 297 | if (output > output_limit) 298 | { 299 | output = output_limit; 300 | } 301 | else if (output < -output_limit) 302 | { 303 | output = -output_limit; 304 | } 305 | 306 | // Update motor target position based on PID output 307 | motor_target_pos = stepper.currentPosition() + output; 308 | 309 | // Limit the motor target position to prevent the motor from moving beyond +-90 degrees 310 | if (abs(motor_target_pos) > convertDegreesToSteps(90)) 311 | { 312 | motor_target_pos = stepper.currentPosition(); 313 | } 314 | 315 | // Move the motor 316 | stepper.moveTo(motor_target_pos); 317 | } 318 | 319 | if (state == BALANCING) 320 | { 321 | // Move the motor to the target position 322 | stepper.moveTo(motor_target_pos); 323 | 324 | // Run the stepper motor 325 | stepper.run(); 326 | } 327 | 328 | print_plot(motor_target_pos, pendulum_actual_deg); 329 | } 330 | 331 | long convertDegreesToSteps(float degrees) 332 | { 333 | // Convert degrees to steps 334 | return degrees * stepsPerRevolution / 360; 335 | } 336 | 337 | /* 338 | * Convert the raw angle from the AS5600 magnetic encoder to degrees. 339 | */ 340 | float convertRawAngleToDegrees() 341 | { 342 | static long raw_prev = 0; 343 | static bool first_reading = true; 344 | static float position = 0.0f; 345 | // Get the current position of the AS5600 346 | long raw = ams5600.getRawAngle(); 347 | if (first_reading) 348 | { 349 | raw_prev = raw; 350 | first_reading = false; 351 | } 352 | long delta = raw - raw_prev; 353 | // Handle wrap around 354 | if (delta > 2047) 355 | delta -= 4096; 356 | if (delta < -2047) 357 | delta += 4096; 358 | // Map the 0–4095 segments of the AS5600 to 0–360 degrees 359 | // 360 degrees / 4096 segments = 0.087890625 degrees per segment 360 | position += (float)delta * 0.087890625; 361 | raw_prev = raw; 362 | 363 | return position; 364 | } 365 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Rotary Inverted Pendulum 2 | 3 | > [!WARNING] 4 | > Documentation and software are still a work in progress. Hardware (mechanical design and electronics) are mostly finished. 5 | 6 | The rotary inverted pendulum is a classic control problem that is used to demonstrate the principles of control theory. The system consists of a pendulum that is mounted on a rotary base, and the goal is to balance the pendulum in the upright position by controlling the rotary base. The system is inherently unstable, which makes it a challenging controls problem. 7 | 8 | The goal of this project is to build a simple and affordable rotary inverted pendulum that can be used to teach control theory to students and hobbyists. 9 | 10 | **Table of Contents** 11 | - [Rotary Inverted Pendulum](#rotary-inverted-pendulum) 12 | - [Mechanics](#mechanics) 13 | - [3D Printing](#3d-printing) 14 | - [Electronics](#electronics) 15 | - [Circuit Diagram](#circuit-diagram) 16 | - [Time Estimates:](#time-estimates) 17 | - [Current Limiting](#current-limiting) 18 | - [Software](#software) 19 | - [Arduino](#arduino) 20 | - [Julia](#julia) 21 | - [Python](#python) 22 | - [Related Work](#related-work) 23 | - [Purchasing Options](#purchasing-options) 24 | - [Acknowledgments](#acknowledgments) 25 | 26 | ## Mechanics 27 | 28 | The rotary inverted pendulum uses off-the-shelf components that are easy to source and assemble. The mechanical design is done in [Onshape](https://www.onshape.com/en/) and the STL files are available in the [meshes](meshes) folder. If you are looking for the actual project in Onshape, you can find it [here](https://cad.onshape.com/documents/fa8afe5031ca70c78442e408/w/5519455d45464bacd4cf9b1d/e/79273ac76c3305af463951de). 29 | 30 | ### 3D Printing 31 | 32 | I have been printint all the pieces in my [Bambu Lab A1 mini](https://bambulab.com/en-gb/a1-mini) — which at the time of me writing this costs £170, and it is great value for the money. I have had success printing in both PLA and PETG using [Bambu Studio](https://bambulab.com/en/download/studio) with the default print settings and with 'Tree' support for the enclosure and arm STL files. When printing the pendulum, add a pause on layer 21 — this is the time when you should place a 2 pence coin (or other similarly sized coin) in the slot, before the printer resumes to cover the slot for the weight. As for the lid STL, I have been printing it in two parts so I can manually change the filament for having a different colour for the 45-degree angle indicator lines. I will add more detailed instructions about the 3D printing here in the future, and maybe even make a video of the printing process for all the parts required. 33 | 34 | ## Electronics 35 | 36 | There are two models of the rotary inverted pendulum: one with batteries and one without. The model with batteries is more portable and can be used without any cables, while the model without batteries requires a power supply to be connected at all times. However, both devices require a USB cable if you want to control them from a computer. 37 | 38 | ### Circuit Diagram 39 | 40 | | System without batteries | System with batteries | 41 | | -------------------------------------------------------------- | ----------------------------------------------------------- | 42 | | | | 43 | 44 | > [!CAUTION] 45 | > The circuit diagram above for the system with batteries is not correct and needs updating. The BMS needs to be capable of managing 3 cells and the batteries required are 3 not 2. In general, the system with batteries is just a prototype at this stage; I have not tried to put the electronics together yet, and the 3D printed parts are also not designed with the extra storage required for holding the batteries and BMS. 46 | 47 | The **battery management system (BMS)** is a circuit that manages the batteries in the device. It performs the following functions: 48 | - balance charging (ensures that all cells in the battery pack are charged to the same voltage) 49 | - short-circuit protection (prevents the batteries from being damaged in case of a short circuit) 50 | - overcharge protection (prevents the batteries from being overcharged) 51 | - overdischarge protection (prevents the batteries from being overdischarged) 52 | 53 | I have spent countless hours debugging prototype boards that I put together when building the first few versions of these pendulum. At first, I used a wire that was too thick (24 AWG) and then I switched to a thinner wire (30 AWG). The jump in size was a lot more than I expected — but then again, I had no prior experience or notion about the AWG scale. My first protoboard using only the thick wire (24 AWG) worked first try, but then I realised the components were not placed correctly for it to fit properly into the enclosure while accomodating space in the correct places for the device's switch and DC jack. As such, I prepared another protoboard, this time with the header pins positioned correctly; however, after turning the pendulum on,it could not balance properly and the boards (both the Arduino and stepper motor driver) started overheating. I burned through 2 Arduino Nano until I was convinced that there was actualy something wrong. I tested the connectivities again, and everything checked out, so I didn't know what was going on or how to debug it. Thus, I started anew from a new protoboard and tried again. Similarly, nothing worked even though the connectivity with the multimeter checked out. I realised that making boards from scratch until one of them worked was not a viable and sane path, so I picked the latest board — which was looking quite good! — and decided to first try and get one of the subsystems working before moving to the next; I started with the magnetic encoder. I wrote a small test script so I could plot the signal via the serial on the Arduino IDE plotter. As I suspected, the signal was terrible and nonsensical. At this point, I had no idea why that was. I tried debraiding the wires, but that didn't work. I picked up a new magnetic encoder and tried soldering new wires, but it didn't work. I braided the cables again, and then tested, and then debraided them again. In the end, I somehow tried soldering the wire points on the magnetic encoder (which are really tiny) from both front and back — and voila! That did the trick! Now I have a very solid signal, stable, not noisy — and I can also confirm that you can braid the wires, as that was not the culprit after all. Now that the sensing subsystem is working, let's move on to debugging the actuation subsystem. 54 | 55 | Tip - It is better to cut the wires slightly longer than you think they'll need to be rather than cutting them exactly the length they need to be (or shorter!) and then struggle with the soldering task. 56 | 57 | ### Time Estimates: 58 | 59 | **1h–1.5h** 60 | - Solder male pins to the Arduino Nano 61 | - Solder header pins to the protoboard 62 | - Solder connection wires on the protoboard 63 | 64 | 65 | ### Current Limiting 66 | 67 | The Nema17 motor of the device is rated for 1 A, but we will use 0.9 A (10% lower) to be on the safe side. The current limit of the stepper driver is set using a potentiometer, and the formula to calculate the current limit is different for each driver: 68 | 69 | | Driver | Vref | Formula | 70 | | ------- | ---- | --------------------------------------------------------------------------- | 71 | | A4988 | 0.72 | Vref = current limit * (8 x Rcs) | 72 | | DRV8825 | 0.45 | Vref = current limit / 2 | 73 | | TMC2209 | 0.66 | [TMC220X Vref Calculator](https://printpractical.github.io/VrefCalculator/) | 74 | 75 | ## Software 76 | 77 | There are two ways to control the rotary inverted pendulum: 78 | 79 | **1 – Using a microcontroller** 80 | In this case, the microcontroller is used to read sensor data and control the motors. The microcontroller runs a control algorithm that calculates the motor commands based on the sensor data. This architecture is simple and easy to implement, but it is limited in terms of computational power and flexibility. 81 | 82 | **2 – Using a computer** 83 | In this case, the microcontroller is used to read sensor data and control the motors, while the computer is used to run the control algorithms. The microcontroller communicates with the computer over a serial connection, and the computer sends commands to the microcontroller to control the system. The computer also receives sensor data from the microcontroller and uses it to update the control algorithm. This architecture allows for real-time control of the system and makes it easy to experiment with different control algorithms. 84 | 85 | | Processing Unit | Advantages | Disadvantages | 86 | | -------------------------------- | ----------------------------------------- | ----------------------------------------------------------- | 87 | | Arduino Nano | Self-contained device. | Limited computational power and flexibility. | 88 | | Computer (via serial connection) | High computational power and flexibility. | Requires an external computer to be connected at all times. | 89 | 90 | *— Do you want to take the device around and be able to run it without a computer?* 91 | Then you should aim to write a control algorithm that runs straight on the Arduino. You can use the code provided in this repository as a starting point. 92 | 93 | *— Do you want to use your favourite programming language and are not concerned about portability?* 94 | In that case, you should connect the device to a computer and communicate with it over a serial connection — this is the approach used in the Julia and Python code examples provided in this repository. 95 | 96 | ### Arduino 97 | 98 | You can find the Arduino code in the [RotaryInvertedPendulum-arduino](RotaryInvertedPendulum-arduino) folder. The code is written in C++ and is intended to be run on an Arduino Nano (onboard the device), but it can also be run on other Arduino boards if you want to tinker with and modify the device. The folder contains code for balancing the pendulum directly from the Arduino, but it also contains code for controlling the pendulum from a computer over a serial connection. 99 | 100 | The Arduino code relies on the following two libraries: 101 | - [AccelStepper](https://www.airspayce.com/mikem/arduino/AccelStepper/) for controlling the stepper motor. 102 | - [AS5600](https://github.com/Seeed-Studio/Seeed_Arduino_AS5600) for reading the AS5600 magnetic encoder. 103 | 104 | ### Julia 105 | 106 | You can find the Julia code in the [RotaryInvertedPendulum-julia](RotaryInvertedPendulum-julia) folder. 107 | 108 | ### Python 109 | 110 | You can find the Python code in the [RotaryInvertedPendulum-python](RotaryInvertedPendulum-python) folder. 111 | 112 | ## Related Work 113 | 114 | Here are some related projects that you might find interesting: 115 | 116 | - https://build-its-inprogress.blogspot.com/2016/08/desktop-inverted-pendulum-part-2-control.html 117 | - https://build-its-inprogress.blogspot.com/search/label/Pendulum 118 | - https://journals.sagepub.com/doi/full/10.1177/00202940211035406 119 | - https://tecsolutions.us/sites/default/files/quanser/The%20Rotary%20Control%20Lab%20Brochure_4.pdf 120 | - https://www.dagor.dev/blog/furuta-pendulum 121 | - https://www.youtube.com/watch?v=2koXcs0IhOc 122 | - https://www.youtube.com/watch?v=bY4t6yfBA24 123 | - https://www.youtube.com/watch?v=VVQ-PGfJMuA 124 | 125 | ## Purchasing Options 126 | 127 | | Description | Price (USD) | Link | 128 | | ---------------------: | :---------: | :------------------------------------------------------------------------------------------------------------------------------- | 129 | | AliExpress – DIY Kit | $100 ~ $200 | [aliexpress.com/w/wholesale-rotary-inverted-pendulum.html](https://www.aliexpress.com/w/wholesale-rotary-inverted-pendulum.html) | 130 | | Quanser – QUBE Servo 2 | > $2,000 | [quanser.com/products/qube-servo-2](https://www.quanser.com/products/qube-servo-2) | 131 | 132 | ## Acknowledgments 133 | 134 | I would like to thank the following people for their contributions to this project: 135 | - [Mykha](https://github.com/Mika412) for early discussions about this project over a beer in the park. 136 | - [André](https://github.com/Esser50K), [Rafael](https://github.com/rkourdis), and [Vlad](https://github.com/VladimirIvan) for technical discussions, feedback, and support. 137 | - [Vivek](https://github.com/svrkrishnavivek) for his invaluable help and feedback on the electronics of the system. 138 | - [心诺 (Xinnuo)](https://github.com/XinnuoXu) for her company and support while working on this project. 139 | 140 | Finally, I would like to thank the open-source community in general for providing the tools and resources that have also helped make this project possible. 141 | --------------------------------------------------------------------------------