diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json new file mode 100644 index 0000000..ada839e --- /dev/null +++ b/.vscode/c_cpp_properties.json @@ -0,0 +1,18 @@ +{ + "configurations": [ + { + "name": "Pico", + "includePath": [ + "${workspaceFolder}/**", + "${env:PICO_SDK_PATH}/**" + ], + "defines": [], + "compilerPath": "${env:PICO_INSTALL_PATH}/gcc-arm-none-eabi/bin/arm-none-eabi-gcc.exe", + "cStandard": "c11", + "cppStandard": "c++11", + "intelliSenseMode": "linux-gcc-arm", + "configurationProvider": "ms-vscode.cmake-tools" + } + ], + "version": 4 +} diff --git a/.vscode/cmake-kits.json b/.vscode/cmake-kits.json new file mode 100644 index 0000000..4b24eb7 --- /dev/null +++ b/.vscode/cmake-kits.json @@ -0,0 +1,7 @@ +[ + { + "name": "Pico ARM GCC", + "description": "Pico SDK Toolchain with GCC arm-none-eabi", + "toolchainFile": "${env:PICO_SDK_PATH}/cmake/preload/toolchains/pico_arm_gcc.cmake" + } +] diff --git a/.vscode/extensions.json b/.vscode/extensions.json new file mode 100644 index 0000000..03ba0af --- /dev/null +++ b/.vscode/extensions.json @@ -0,0 +1,9 @@ +{ + "recommendations": [ + "marus25.cortex-debug", + "ms-vscode.cmake-tools", + "ms-vscode.cpptools", + "ms-vscode.cpptools-extension-pack", + "ms-vscode.vscode-serial-monitor" + ] +} diff --git a/.vscode/launch.json b/.vscode/launch.json new file mode 100644 index 0000000..c370c53 --- /dev/null +++ b/.vscode/launch.json @@ -0,0 +1,58 @@ +{ + "version": "0.2.0", + "configurations": [ + { + "name": "Pico Debug (Cortex-Debug)", + "cwd": "${workspaceFolder}", + "executable": "${command:cmake.launchTargetPath}", + "request": "launch", + "type": "cortex-debug", + "servertype": "openocd", + "gdbPath": "arm-none-eabi-gdb", + "device": "RP2040", + "configFiles": [ + "interface/cmsis-dap.cfg", + "target/rp2040.cfg" + ], + "svdFile": "${env:PICO_SDK_PATH}/src/rp2040/hardware_regs/rp2040.svd", + "runToEntryPoint": "main", + "openOCDLaunchCommands": [ + "adapter speed 5000" + ] + }, + { + "name": "Pico Debug (Cortex-Debug with external OpenOCD)", + "cwd": "${workspaceFolder}", + "executable": "${command:cmake.launchTargetPath}", + "request": "launch", + "type": "cortex-debug", + "servertype": "external", + "gdbTarget": "localhost:3333", + "gdbPath": "arm-none-eabi-gdb", + "device": "RP2040", + "svdFile": "${env:PICO_SDK_PATH}/src/rp2040/hardware_regs/rp2040.svd", + "runToEntryPoint": "main" + }, + { + "name": "Pico Debug (C++ Debugger)", + "type": "cppdbg", + "request": "launch", + "cwd": "${workspaceFolder}", + "program": "${command:cmake.launchTargetPath}", + "MIMode": "gdb", + "miDebuggerPath": "arm-none-eabi-gdb", + "miDebuggerServerAddress": "localhost:3333", + "debugServerPath": "openocd", + "debugServerArgs": "-f interface/cmsis-dap.cfg -f target/rp2040.cfg -c \"adapter speed 5000\"", + "serverStarted": "Listening on port .* for gdb connections", + "filterStderr": true, + "stopAtEntry": true, + "hardwareBreakpoints": { + "require": true, + "limit": 4 + }, + "preLaunchTask": "Flash", + "svdPath": "${env:PICO_SDK_PATH}/src/rp2040/hardware_regs/rp2040.svd" + } + ] +} diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..80c85c5 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,32 @@ +{ + // These settings tweaks to the cmake plugin will ensure + // that you debug using cortex-debug instead of trying to launch + // a Pico binary on the host + "cmake.statusbar.advanced": { + "debug": { + "visibility": "hidden" + }, + "launch": { + "visibility": "hidden" + }, + "build": { + "visibility": "hidden" + }, + "buildTarget": { + "visibility": "hidden" + } + }, + "cmake.buildBeforeRun": true, + "cmake.configureOnOpen": true, + "cmake.configureSettings": { + "CMAKE_MODULE_PATH": "${env:PICO_INSTALL_PATH}/pico-sdk-tools" + }, + "cmake.generator": "Ninja", + "C_Cpp.default.configurationProvider": "ms-vscode.cmake-tools", + "cmake.sourceDirectory": "C:/Users/arcti/Desktop/githubprojects/modbot/swerve_pico", + "files.associations": { + "iostream": "cpp", + "*.pio": "cpp", + "stdexcept": "cpp" + } +} diff --git a/.vscode/tasks.json b/.vscode/tasks.json new file mode 100644 index 0000000..8de7263 --- /dev/null +++ b/.vscode/tasks.json @@ -0,0 +1,29 @@ +{ + "version": "2.0.0", + "tasks": [ + { + "label": "Flash", + "type": "shell", + "command": "openocd", + "args": [ + "-f", + "interface/cmsis-dap.cfg", + "-f", + "target/rp2040.cfg", + "-c", + "adapter speed 5000; program {${command:cmake.launchTargetPath}} verify reset exit" + ], + "problemMatcher": [] + }, + { + "label": "Build", + "type": "cmake", + "command": "build", + "problemMatcher": "$gcc", + "group": { + "kind": "build", + "isDefault": true + } + } + ] +} diff --git a/dev/basic_swerve/CMakeLists.txt b/dev/basic_swerve/CMakeLists.txt new file mode 100644 index 0000000..7e1ada4 --- /dev/null +++ b/dev/basic_swerve/CMakeLists.txt @@ -0,0 +1,55 @@ +# Generated Cmake Pico project file + +cmake_minimum_required(VERSION 3.5) + +set(CMAKE_C_STANDARD 11) +set(CMAKE_CXX_STANDARD 17) + +# Initialise pico_sdk from installed location +# (note this can come from environment, CMake cache etc) + +set(PICO_BOARD pico CACHE STRING "Board type") + +# Pull in Raspberry Pi Pico SDK (must be before project) +include(pico_sdk_import.cmake) + +if (PICO_SDK_VERSION_STRING VERSION_LESS "1.4.0") + message(FATAL_ERROR "Raspberry Pi Pico SDK version 1.4.0 (or later) required. Your version is ${PICO_SDK_VERSION_STRING}") +endif() + +project(basic_swerve_pico C CXX ASM) + +# Initialise the Raspberry Pi Pico SDK +pico_sdk_init() + +# Add executable. Default name is the project name, version 0.1 + +add_executable(basic_swerve_pico basic_swerve_pico.c ) + +pico_set_program_name(basic_swerve_pico "basic_swerve_pico") +pico_set_program_version(basic_swerve_pico "0.1") + +pico_enable_stdio_uart(basic_swerve_pico 0) +pico_enable_stdio_usb(basic_swerve_pico 1) + +# Add the standard library to the build +target_link_libraries(basic_swerve_pico + pico_stdlib + pico_i2c_slave + hardware_i2c + hardware_pwm + ) + +# Add the standard include files to the build +target_include_directories(basic_swerve_pico PRIVATE + ${CMAKE_CURRENT_LIST_DIR} + ${CMAKE_CURRENT_LIST_DIR}/.. # for our common lwipopts or any other standard includes, if required +) + +# Add any user requested libraries +target_link_libraries(basic_swerve_pico + hardware_i2c + ) + +pico_add_extra_outputs(basic_swerve_pico) + diff --git a/dev/basic_swerve/Controller.py b/dev/basic_swerve/Controller.py new file mode 100644 index 0000000..af696be --- /dev/null +++ b/dev/basic_swerve/Controller.py @@ -0,0 +1,203 @@ +import math +import threading + +from inputs import get_gamepad # Import the get_gamepad function from the inputs module +from procon import ProCon # Import the ProCon class from the procon module + + +# This class represents a PS4 Controller +class PS4_Controller(object): + def __init__(self): + self.MAX_TRIG_VAL = math.pow(2, 8) # Maximum value for trigger input + self.MAX_JOY_VAL = math.pow(2, 7) # Maximum value for joystick input + self.THRESHOLD = 0.04 # Threshold for joystick deadzone + self.reset_vars() # Reset all controller variables to their initial state + self.start_thread(()) # Start a new thread to monitor the controller + + # This method resets all controller variables to their initial state + def reset_vars(self): + # Initialize all controller variables to 0 + self.LeftJoystickY = 0 + self.LeftJoystickX = 0 + self.RightJoystickY = 0 + self.RightJoystickX = 0 + self.LeftTrigger = 0 + self.RightTrigger = 0 + self.LeftBumper = 0 + self.RightBumper = 0 + self.A = 0 + self.X = 0 + self.Y = 0 + self.B = 0 + self.LeftThumb = 0 + self.RightThumb = 0 + self.Back = 0 + self.Start = 0 + self.LeftDPad = 0 + self.RightDPad = 0 + self.UpDPad = 0 + self.DownDPad = 0 + + # This method starts a new thread to monitor the controller + def start_thread(self, thread_args=()): + self._monitor_thread = threading.Thread( + target=self._monitor_controller, args=thread_args + ) + self._monitor_thread.daemon = ( + True # Set the thread as a daemon so it will end when the main program ends + ) + self._monitor_thread.start() # Start the thread + + # This method returns the current state of all buttons/triggers + def read(self): + return [ + self.LeftJoystickY, + self.LeftJoystickX, + self.RightJoystickY, + self.RightJoystickX, + self.LeftTrigger, + self.RightTrigger, + self.LeftBumper, + self.RightBumper, + self.A, + self.B, + self.X, + self.Y, + self.LeftThumb, + self.RightThumb, + self.Back, + self.Start, + self.LeftDPad, + self.RightDPad, + self.UpDPad, + self.DownDPad, + ] + + # This method returns the controller object itself + def read_self(self): + return self + + # This method applies a threshold to a value + def threshold(self, val): + return val - 1.0 if abs(val - 1.0) > self.THRESHOLD else 0 + + def _monitor_controller(self): + while True: + events = get_gamepad() + for event in events: + if event.code == "ABS_Y": + self.LeftJoystickY = self.threshold( + event.state / self.MAX_JOY_VAL + ) # normalize between -1 and 1 + elif event.code == "ABS_X": + self.LeftJoystickX = self.threshold( + event.state / self.MAX_JOY_VAL + ) # normalize between -1 and 1 + elif event.code == "ABS_RY": + self.RightJoystickY = self.threshold( + event.state / self.MAX_JOY_VAL + ) # normalize between -1 and 1 + elif event.code == "ABS_RX": + self.RightJoystickX = self.threshold( + event.state / self.MAX_JOY_VAL + ) # normalize between -1 and 1 + elif event.code == "ABS_Z": + self.LeftTrigger = self.threshold( + event.state / self.MAX_TRIG_VAL + ) # normalize between 0 and 1 + elif event.code == "ABS_RZ": + self.RightTrigger = self.threshold( + event.state / self.MAX_TRIG_VAL + ) # normalize between 0 and 1 + elif event.code == "BTN_TL": + self.LeftBumper = event.state + elif event.code == "BTN_TR": + self.RightBumper = event.state + elif event.code == "BTN_SOUTH": + self.A = event.state + elif event.code == "BTN_NORTH": + self.Y = event.state # previously switched with X + elif event.code == "BTN_WEST": + self.X = event.state # previously switched with Y + elif event.code == "BTN_EAST": + self.B = event.state + elif event.code == "BTN_THUMBL": + self.LeftThumb = event.state + elif event.code == "BTN_THUMBR": + self.RightThumb = event.state + elif event.code == "BTN_SELECT": + self.Back = event.state + elif event.code == "BTN_START": + self.Start = event.state + elif event.code == "BTN_TRIGGER_HAPPY1": + self.LeftDPad = event.state + elif event.code == "BTN_TRIGGER_HAPPY2": + self.RightDPad = event.state + elif event.code == "BTN_TRIGGER_HAPPY3": + self.UpDPad = event.state + elif event.code == "BTN_TRIGGER_HAPPY4": + self.DownDPad = event.state + + +# This class represents the Xbox Controller in WRP used for the CPSRC GEM +class Gem_Xbox_Controller(PS4_Controller): + def __init__(self): + self.MAX_TRIG_VAL = math.pow(2, 8) # Maximum value for trigger input + self.MAX_JOY_VAL = math.pow(2, 15) # Maximum value for joystick input + self.THRESHOLD = 0.03 # Threshold for joystick deadzone + + self.reset_vars() # Reset all controller variables to their initial state + self.start_thread(()) # Start a new thread to monitor the controller + + +# This class represents the Nintendo Pro Controller +class Nintendo_Pro_Controller(PS4_Controller): + def __init__(self): + self.MAX_TRIG_VAL = math.pow(2, 8) # Maximum value for trigger input + self.MAX_JOY_VAL = math.pow(2, 15) # Maximum value for joystick input + self.THRESHOLD = 0.1 # Threshold for joystick deadzone + self.controller = ProCon() # Initialize the ProCon controller + + self.reset_vars() # Reset all controller variables to their initial state + self.start_thread( + self.procon_callback_func + ) # Start a new thread to monitor the controller + + # This method is called when the ProCon controller state changes + def procon_callback_func(self, buttons, l_stick, r_stick, *_): + # Update the controller variables based on the new state + # The joystick values are normalized between -1 and 1 + # The threshold method is used to apply a deadband to the joystick values + # The button values are either 0 or 1 + self.LeftJoystickX = self.threshold(l_stick[0] / self.MAX_JOY_VAL) + self.LeftJoystickY = self.threshold(l_stick[1] / self.MAX_JOY_VAL) + self.RightJoystickX = self.threshold(r_stick[0] / self.MAX_JOY_VAL) + self.RightJoystickY = self.threshold(r_stick[1] / self.MAX_JOY_VAL) + self.LeftTrigger = self.threshold(buttons[ProCon.Button.ZL]) + self.RightTrigger = self.threshold(buttons[ProCon.Button.ZR]) + self.LeftBumper = buttons[ProCon.Button.L] + self.RightBumper = buttons[ProCon.Button.R] + self.A = buttons[ProCon.Button.A] + self.B = buttons[ProCon.Button.B] + self.X = buttons[ProCon.Button.X] + self.Y = buttons[ProCon.Button.Y] + self.LeftThumb = buttons[ProCon.Button.LS] + self.RightThumb = buttons[ProCon.Button.RS] + self.Back = buttons[ProCon.Button.MINUS] + self.Start = buttons[ProCon.Button.PLUS] + self.LeftDPad = buttons[ProCon.Button.LEFT] + self.RightDPad = buttons[ProCon.Button.RIGHT] + self.UpDPad = buttons[ProCon.Button.UP] + self.DownDPad = buttons[ProCon.Button.DOWN] + + +if __name__ == "__main__": + joy = PS4_Controller() # Initialize a PS4 controller + # joy = Gem_Xbox_Controller() # Initialize a Gem Xbox controller + # joy = Nintendo_Pro_Controller() # Initialize a Nintendo Pro controller + while True: + try: + print(joy.read()) # Print the current state of the controller + except Exception as e: + print("error!", e) # Print any errors that occur + break # Exit the loop if an error occurs diff --git a/dev/basic_swerve/basic_swerve_pi.py b/dev/basic_swerve/basic_swerve_pi.py new file mode 100644 index 0000000..dd40ba2 --- /dev/null +++ b/dev/basic_swerve/basic_swerve_pi.py @@ -0,0 +1,132 @@ +import fcntl +import os +import time +import struct + +from Controller import PS4_Controller + +I2C_PRIM = 0x0703 # I2C primary address + +# Open i2c devices (sudo apt install i2c-tools) +i2c_fd = os.open("/dev/i2c-1", os.O_RDWR) # File descriptor for the i2c device + +# Set the i2c address of pico +pico_address = 0x08 # Address of the pico on the i2c bus +fcntl.ioctl( + i2c_fd, I2C_PRIM, pico_address +) # Set the address of the i2c device to communicate with + +# Send data to pico +joy = PS4_Controller() # Initialize the controller +delay = 0.05 # Delay between each read/write operation + +import math + +# radius of swerve drive base in meters +radius = 1 + + +# add two vectors +def add_two_vec(v1, v2): + # get difference between two vectors, treating one vector as perpendicular to x axis + theta_diff = v2[1] - v1[1] + theta_diff = theta_diff + + # since vec1 is x axis, vector 1 contributes in only the x axis + # when breaking into components, just add vec2 in x to vec1 magnitude to get x + # vec2 in y to get y + # sqrt(a^2+b^2) to get magnitude, and arctan(y/x) to get angle relative to x (add to vec1 orientation) + x_comp = v1[0] + math.cos(theta_diff) * v2[0] + y_comp = math.sin(theta_diff) * v2[0] + f_mag = math.sqrt(x_comp**2 + y_comp**2) + f_angle = math.atan2(y_comp, x_comp) + v1[1] + f_angle = f_angle + if f_angle < -math.pi: + f_angle += 2 * math.pi + elif f_angle > math.pi: + f_angle -= 2 * math.pi + return [f_mag, f_angle] + + +# input velocity [speed (m/s), orientation (deg)] in local reference frame +# rotation (rad/s) +def convert(v_vec, omega): + + # the vector for each motor in global reference frame is given by adding + # 1) velocity (relative to global reference frame) + # 2) rotation vector (vector perpendicular to each motor relative to the body reference frame) + # because the frame is circular, magnitude of vector is calculated by V = rw (formula for tangential speed relative to radius and rad/s) + + # setting all vectors to velocity relative to local reference frame + m1 = [v_vec[0], v_vec[1]] + m2 = [v_vec[0], v_vec[1] - 2 * math.pi / 3] + m3 = [v_vec[0], v_vec[1] - 4 * math.pi / 3] + + # create magnitude for each vector + rot_mag = omega * radius + dir = 1 + + if rot_mag < 0: + dir *= -1 + rot_mag *= -1 + # add two vectors (in local frame) based on direction and magnitude + m1 = add_two_vec(m1, [rot_mag, math.pi / 2 * dir]) + m2 = add_two_vec(m2, [rot_mag, math.pi / 2 * dir]) + m3 = add_two_vec(m3, [rot_mag, math.pi / 2 * dir]) + + return [m1, m2, m3] + + +while True: # Infinite loop + status = joy.read_self() # Read the status of the controller + x = status.LeftJoystickX # Get the X position of the left joystick + y = -1 * status.LeftJoystickY # Get the Y position of the left joystick + w = status.RightJoystickX # Get the X position of the right joystick + + v = (math.sqrt(x**2 + y**2), math.atan2(y, x)) + m = convert(v, w) + + m1_v = struct.pack("f", m[0][0]) + m1_w = struct.pack("f", math.degrees(m[0][1])) + + m2_v = struct.pack("f", m[1][0]) + m2_w = struct.pack("f", math.degrees(m[1][1])) + + m3_v = struct.pack("f", m[2][0]) + m3_w = struct.pack("f", math.degrees(m[2][1])) + + data = ( + bytes([0xFA]) + m1_v + m1_w + m2_v + m2_w + m3_v + m3_w + bytes([0xFB]) + ) # Prepare the data to be sent + # data = ( + # bytes([0xFA]) + # + struct.pack("f", 0.5) + # + struct.pack("f", 0.5) + # + struct.pack("f", 0.5) + # + struct.pack("f", 0.5) + # + struct.pack("f", 0.5) + # + struct.pack("f", 0.5) + # + bytes([0xFB]) + # ) # Prepare the data to be sent + + try: + os.write(i2c_fd, data) # Write the data to the i2c device + # os.write(i2c_fd, bytes(data)) # Write the data to the i2c device + time.sleep(delay) # Wait for a while + print("Sent data to Pico: ", list(data)) # Print the data that was sent + except OSError: + print( + "Remote I/O Error" + ) # Print an error message if there was a problem with the write operation + + # Read data from pico + try: + incoming_data = os.read(i2c_fd, 1) # Read 1 byte from the i2c device + time.sleep(delay) # Wait for a while + # print( + # "Received data from Pico: ", list(incoming_data) + # ) # Print the data that was received + except TimeoutError: + print("Timeout Error") # Print an error message if there was a timeout error + except OSError: + print("Remote I/O Error") diff --git a/dev/basic_swerve/basic_swerve_pico.c b/dev/basic_swerve/basic_swerve_pico.c new file mode 100644 index 0000000..9f4bf53 --- /dev/null +++ b/dev/basic_swerve/basic_swerve_pico.c @@ -0,0 +1,393 @@ +#include +#include +#include +#include +#include +#include +#include + +// Define constants for I2C communication +#define I2C_PICO_ADDR 0x08 +#define I2C_SDA_PIN 0 +#define I2C_SCL_PIN 1 +#define I2C_PORT i2c0 +#define I2C_BAUDRATE 100 * 1000 + +// Define the length of the data packet +#define I2C_DATA_LENGTH 128 + +#define MESSAGE_START 0xFA +#define MESSAGE_STOP 0xFB + +// digital low on a/b pins indicates direction, both high is no signal (stop) +// pwm signal on pwm pin controls speed + +#define wheel_1_pwm 2 +#define wheel_1_slice 1 +#define wheel_1_channel PWM_CHAN_A +#define wheel_1a 3 +#define wheel_1b 4 + +#define turn_1a 5 +#define turn_1b 6 +#define turn_1_pwm 7 +#define turn_1_slice 3 +#define turn_1_channel PWM_CHAN_B + +#define wheel_2_pwm 8 +#define wheel_2_slice 4 +#define wheel_2_channel PWM_CHAN_A +#define wheel_2a 9 +#define wheel_2b 10 + +#define turn_2a 11 +#define turn_2b 12 +#define turn_2_pwm 13 +#define turn_2_slice 6 +#define turn_2_channel PWM_CHAN_B + +#define turn_3_pwm 16 +#define turn_3_slice 0 +#define turn_3_channel PWM_CHAN_A +#define turn_3b 17 +#define turn_3a 18 + +#define wheel_3b 19 +#define wheel_3a 20 +#define wheel_3_pwm 21 +#define wheel_3_slice 2 +#define wheel_3_channel PWM_CHAN_B + +// #define freq 500 // note: use clock management frequencies to set frequency +#define count_max 65535 + +// Status of the input data +int input_status = 0; + +// Last event that occurred +int last_event = 0; + +// Index of the current data byte +int data_index = 0; + +int count = 0; + +// Buffer for the input data +typedef struct dataT +{ + uint8_t data[I2C_DATA_LENGTH]; + uint8_t len; +} *data; + +typedef struct buffT +{ + data buff[I2C_DATA_LENGTH]; + uint8_t head; + uint8_t tail; + uint8_t full; +} *buff; + +buff buffer; +data tmp; + +// Initialize the circular buffer +void init_buff(buff *b, data *d) +{ + *b = malloc(sizeof(struct buffT)); + (*b)->head = 0; + (*b)->tail = 0; + (*b)->full = 0; + for (int i = 0; i < I2C_DATA_LENGTH; i++) + { + (*b)->buff[i] = malloc(sizeof(struct dataT)); + (*b)->buff[i]->len = 0; + for (int j = 0; j < I2C_DATA_LENGTH; j++) + { + (*b)->buff[i]->data[j] = 0; + } + } + + *d = malloc(sizeof(struct dataT)); + (*d)->len = 0; + for (int i = 0; i < I2C_DATA_LENGTH; i++) + { + (*d)->data[i] = 0; + } +} + +// Handler for I2C events +static void i2c_handler(i2c_inst_t *i2c, i2c_slave_event_t event) +{ + switch (event) + { + case I2C_SLAVE_RECEIVE: + { + // Read the data + uint8_t tmp_byte = i2c_read_byte_raw(i2c); + // Check if the data is valid + if (tmp->len >= I2C_DATA_LENGTH) + { + printf("Invalid data %x, len %x\n", tmp_byte, tmp->len); + break; + } + // Store the data + tmp->data[tmp->len] = tmp_byte; + tmp->len++; + // set the event status to received + last_event = 1; + + break; + } + + case I2C_SLAVE_REQUEST: // Pi is requesting data + // Write the data into the void + i2c_write_byte_raw(i2c, (uint8_t)input_status); + // set the event status to sent + last_event = 2; + break; + + case I2C_SLAVE_FINISH: // Pi has signalled Stop / Restart + // if the last event was a receive event and the data is valid + if (last_event == 1 && !buffer->full) + { + if (tmp->data[0] == MESSAGE_START && tmp->data[tmp->len - 1] == MESSAGE_STOP) + { + for (int i = 0; i < tmp->len; i++) + { + buffer->buff[buffer->tail]->data[i] = tmp->data[i]; + } + buffer->buff[buffer->tail]->len = tmp->len; + + // set the input status to ready + input_status = 1; + + // move the tail of the buffer + buffer->tail = (buffer->tail + 1) % I2C_DATA_LENGTH; + + // check if the buffer is full + if (buffer->tail == buffer->head) + { + buffer->full = 1; + } + tmp->len = 0; + } + } + else + { + // printf("Buffer full, attempted to put this into buffer:\n"); + // for (int i = 0; i < tmp->len; i++) + // { + // printf("%d ", tmp->data[i]); + // } + // printf("\n"); + tmp->len = 0; + } + break; + default: + break; + } +} + +// linear & angular velocity for each wheel +float v1 = 0.0f; +float v2 = 0.0f; +float v3 = 0.0f; + +float w1 = 0.0f; +float w2 = 0.0f; +float w3 = 0.0f; + +const int num_floats = 6; + +float rot1, rot2, rot3; // current working rotation to calc delta +float delt1, delt2, delt3; // change in rotation after subtracting omega +float u1, u2, u3; // interim omega values + +int flip1 = 1, flip2 = 1, flip3 = 1; +// const float scale = + +void setup_pins(int a, int b, int pwm, int slice, int channel) +{ + gpio_init(a); + gpio_init(b); + gpio_init(pwm); + gpio_set_dir(a, GPIO_OUT); + gpio_set_dir(b, GPIO_OUT); + gpio_set_function(pwm, GPIO_FUNC_PWM); + pwm_set_wrap(slice, count_max); + pwm_set_chan_level(slice, channel, 0); + pwm_set_enabled(slice, true); +} + +int main() +{ + const uint LED_PIN = PICO_DEFAULT_LED_PIN; + gpio_init(LED_PIN); + gpio_set_dir(LED_PIN, GPIO_OUT); + + stdio_init_all(); + + // Initialize I2C at 100kHz + i2c_init(I2C_PORT, I2C_BAUDRATE); + gpio_set_function(I2C_SDA_PIN, GPIO_FUNC_I2C); + gpio_set_function(I2C_SCL_PIN, GPIO_FUNC_I2C); + + init_buff(&buffer, &tmp); + + // Set I2C address for Pico + i2c_slave_init(I2C_PORT, I2C_PICO_ADDR, &i2c_handler); + + // Set up the pins for the motors + setup_pins(wheel_1a, wheel_1b, wheel_1_pwm, wheel_1_slice, wheel_1_channel); + setup_pins(wheel_2a, wheel_2b, wheel_2_pwm, wheel_2_slice, wheel_2_channel); + setup_pins(wheel_3a, wheel_3b, wheel_3_pwm, wheel_3_slice, wheel_3_channel); + + setup_pins(turn_1a, turn_1b, turn_1_pwm, turn_1_slice, turn_1_channel); + setup_pins(turn_2a, turn_2b, turn_2_pwm, turn_2_slice, turn_2_channel); + setup_pins(turn_3a, turn_3b, turn_3_pwm, turn_3_slice, turn_3_channel); + + sleep_ms(1000); // wait for the i2c to be ready + + absolute_time_t start_time = get_absolute_time(); + + // turns 180 degrees (approximately) - use 11660 to help scale the degrees needed to turn to the # of timesteps necessary to complete that turn at full speed + // for (int timetest = 0; timetest < 11660; timetest++) + while (1) + { + gpio_put(LED_PIN, 1); + if (input_status == 1) + { + input_status = 0; + // use https://www.h-schmidt.net/FloatConverter/IEEE754.html to convert between float and binary/hex + uint32_t tmp_float[num_floats]; + + for (int i = 0; i < num_floats; i++) + { + tmp_float[i] = 0; + for (int j = 0; j < 4; j++) + { + tmp_float[i] |= buffer->buff[buffer->head]->data[(i * 4) + j + 1] << (8 * j); + // printf("%d:%d ", (i * 4) + j + 1, buffer->buff[buffer->head]->data[(i * 4) + j + 1]); + } + // printf("\n %x\n", tmp_float[i]); + } + v1 = *(((float *)&tmp_float[0])); + v2 = *(((float *)&tmp_float[2])); + v3 = *(((float *)&tmp_float[4])); + + printf("i2c %u ", to_ms_since_boot(get_absolute_time())); + + // tmp_float = 0; // clear float + buffer->head = (buffer->head + 1) % I2C_DATA_LENGTH; + buffer->full = 0; + } + gpio_put(LED_PIN, 0); + + // get delta for wheel 1 + delt1 = v1 - rot1; + if (abs(delt1) > 90) + { + flip1 *= -1; + delt1 = (90) * (delt1 < 0 ? -1 : 1) - delt1; + } + + // get delta for wheel 2 + delt2 = v2 - rot2; + if (abs(delt2) > 90) + { + flip2 *= -1; + delt2 = (90) * (delt2 < 0 ? -1 : 1) - delt2; + } + + // get delta for wheel 3 + delt3 = v3 - rot3; + if (abs(delt3) > 90) + { + flip3 *= -1; + delt3 = (90) * (delt3 < 0 ? -1 : 1) - delt3; + } + + if (v1 == 0) + { + // in1 and in2 are high + gpio_put(wheel_1a, 1); + gpio_put(wheel_1b, 1); + } + else if (v1 * flip1 < 0) + { + // in1 is high and in2 is low + gpio_put(wheel_1a, 1); + gpio_put(wheel_1b, 0); + rot1 += 0.01543739279 + } + else + { + // in1 is low and in2 is high + gpio_put(wheel_1b, 1); + gpio_put(wheel_1a, 0); + rot1 -= 0.01543739279 + } + + if (v2 == 0) + { + // in1 and in2 are high + gpio_put(wheel_2a, 1); + gpio_put(wheel_2b, 1); + } + else if (v2 * flip2 < 0) + { + // in1 is high and in2 is low + gpio_put(wheel_2a, 1); + gpio_put(wheel_2b, 0); + rot2 += 0.01543739279 + } + else + { + // in1 is low and in2 is high + gpio_put(wheel_2b, 1); + gpio_put(wheel_2a, 0); + rot2 -= 0.01543739279 + } + + if (v3 == 0) + { + // in1 and in2 are high + gpio_put(wheel_3a, 1); + gpio_put(wheel_3b, 1); + } + else if (v3 * flip3 < 0) + { + // in1 is high and in2 is low + gpio_put(wheel_3a, 1); + gpio_put(wheel_3b, 0); + rot3 += 0.01543739279 + } + else + { + // in1 is low and in2 is high + gpio_put(wheel_3b, 1); + gpio_put(wheel_3a, 0); + rot3 -= 0.01543739279 + } + pwm_set_chan_level(wheel_1_slice, wheel_1_channel, abs((int)(v1 * count_max))); + + pwm_set_chan_level(wheel_2_slice, wheel_2_channel, abs((int)(v2 * count_max))); + + pwm_set_chan_level(wheel_3_slice, wheel_3_channel, abs((int)(v3 * count_max))); + + // sleep_ms(20); // used for debugging - reduces the number of loops w/ no new i2c data + printf("%u %u\n", to_ms_since_boot(get_absolute_time()), to_ms_since_boot(start_time)); + } + + printf("%u\n", to_ms_since_boot(get_absolute_time())); + + while (1) + { + gpio_put(LED_PIN, 1); + sleep_ms(500); + gpio_put(LED_PIN, 0); + sleep_ms(500); + } + + return 0; +} \ No newline at end of file diff --git a/dev/basic_swerve/joystick_sim.py b/dev/basic_swerve/joystick_sim.py new file mode 100644 index 0000000..4aa9f93 --- /dev/null +++ b/dev/basic_swerve/joystick_sim.py @@ -0,0 +1,195 @@ +import matplotlib.pyplot as plt +import numpy as np +from Controller import (Gem_Xbox_Controller, Nintendo_Pro_Controller, + PS4_Controller) + + +# return the vector perpendicular to the given vector +def perpendicular(vec): + return np.array([-vec[1], vec[0]]) + + +# NOTE: make sure to account for max motor speed when programming real motors, and normalize +# for example, if the swerve math commands one motor to spin higher than it's max speed, +# then it will only spin at the max speed, thus making the ratio of motor powers wrong and the robot will move wrong + + +if __name__ == "__main__": + # joy = Gem_Xbox_Controller() + # joy = Nintendo_Pro_Controller() + joy = PS4_Controller() + + rumble = type(joy) == Nintendo_Pro_Controller + + # robot radius + R = 5 + # dt, the delta time of the "animation" + DT = 0.001 + + # initial robot state + center_pos = np.array([0.0, 0.0]) # center position + module_dirs = ( + np.array([3.0, 7.0, 11.0]) / 6.0 * np.pi + ) # directions of each module, relative to screen + module_pos = np.array( + [ + [R * np.cos(a) + center_pos[0], R * np.sin(a) + center_pos[1]] + for a in module_dirs + ] + ) # absolute positions of each module (as a point) + freeze_pos = ( + center_pos.copy() + ) # position to rotate about when right bumper is pressed + + while True: + try: + # get inputs + joy_input = joy.read_self() + if joy_input.Back: # exit if back button is pressed + print("Exiting") + break + + # TODO: should replace this by standardizing inverts in the Controller.py class + inverts = [False, False, False, False] # Nintendo Pro Controller + # inverts = [False, True, True] # Gem Xbox Controller + + # use joystick inputs to calculate "strafe" movement + left_x = (-1.0 if inverts[0] else 1.0) * round(joy_input.LeftJoystickX, 3) + left_y = (-1.0 if inverts[1] else 1.0) * round(joy_input.LeftJoystickY, 3) + triggers = joy_input.LeftTrigger - joy_input.RightTrigger + + right_x = (-1.0 if inverts[2] else 1.0) * round(joy_input.RightJoystickX, 3) + right_y = (-1.0 if inverts[3] else 1.0) * round(joy_input.RightJoystickY, 3) + + ## LOGIC (begin) + + # get distance between freeze_pos and center_pos + dist = np.hypot( + freeze_pos[0] - center_pos[0], freeze_pos[1] - center_pos[1] + ) + + # if right bumper is not pressed, move robot in direction of joystick & rotate relative to center pos + if not joy_input.RightBumper: + move = np.array([left_x, left_y]) * 1.0 + rotate = 0.1 * triggers + + # if right bumper is pressed and freeze pos is not "inside" robot, rotate robot around freeze pos + elif dist > R: + # calculate vector from freeze to center pos + x = (freeze_pos[0] - center_pos[0]) / dist + y = (freeze_pos[1] - center_pos[1]) / dist + + # calculate new center position, moving robot around freeze pos + # x' = x*cos(theta) - y*sin(theta) + # y' = x*sin(theta) + y*cos(theta) + # where theta is the rotation angle, but we can use left_x and left_y as sin(theta) and cos(theta) + # https://academo.org/demos/rotation-about-point/ + move = np.array( + [-1.0 * y * left_x + x * left_y, x * left_x + y * left_y] + ) + # rotate robot so direction of modules is the same relative to freeze pos, plus some rotation from triggers + rotate = (-1.0 if left_x > 0 else 1.0) * np.hypot( + move[0], move[1] + ) / dist + 0.1 * triggers + + # if left bumper is pressed, make freeze pos the same as center pos + if joy_input.LeftBumper: + freeze_pos = center_pos.copy() + else: # if left bumper is not pressed, move freeze pos in direction of right joystick + freeze_pos += np.array([right_x, right_y]) * 1.0 + + # if right bumper is not pressed, move freeze pos in direction of right joystick (relative to center pos) + if not joy_input.RightBumper: + freeze_pos += move * 1.0 + np.array([right_x, right_y]) * 1.0 + + # update center position + center_pos += move + + # update module directions + module_dirs += rotate + + ## LOGIC (end) + + # update module positions using module directions and center position + module_pos = np.array( + [ + [R * np.cos(a) + center_pos[0], R * np.sin(a) + center_pos[1]] + for a in module_dirs + ] + ) + + # set box size and aspect ratio for matplotlib plot window + box_scale = 10 + plt.xlim(-box_scale * R, box_scale * R) + plt.ylim(-box_scale * R, box_scale * R) + plt.gca().set_aspect("equal", adjustable="box") + + # array to store module controls (direction & speed of each module) + module_controls = [] + + # plot robot + for i, module in enumerate(module_pos): + # plot line from center to module + plt.plot( + [center_pos[0], module[0]], [center_pos[1], module[1]], "black" + ) + + # calculate module direction vector using robot movement vector & rotation + dir_vec = ( + move + + np.array([-np.sin(module_dirs[i]), np.cos(module_dirs[i])]) + * rotate + * 10 + ) + + # add module direction vector to module_controls as degrees & speed + module_controls.append( + ( + round(np.rad2deg(np.arctan2(dir_vec[1], dir_vec[0])), 3), + round(np.hypot(dir_vec[0], dir_vec[1]), 3), + ) + ) + + # plot module direction vectors + plt.quiver( + module[0], + module[1], + dir_vec[0], + dir_vec[1], + color="red", + angles="xy", + scale_units="xy", + scale=0.5, + ) + + # print(module_controls) + + # plot center direction vector + plt.quiver( + center_pos[0], + center_pos[1], + move[0], + move[1], + color="green", + angles="xy", + scale_units="xy", + scale=0.5, + ) + + # plot line from center to freeze pos + plt.plot( + [center_pos[0], freeze_pos[0]], [center_pos[1], freeze_pos[1]], "b" + ) + + # rumble if robot is outside of box + if rumble and ( + abs(center_pos[0]) > box_scale * R or abs(center_pos[1]) > box_scale * R + ): + joy.controller.send_rumble(False, True, 1) + + # pause for DT seconds and clear plot + plt.pause(DT) + plt.clf() + + except Exception as e: + print(e) diff --git a/dev/basic_swerve/pico_sdk_import.cmake b/dev/basic_swerve/pico_sdk_import.cmake new file mode 100644 index 0000000..65f8a6f --- /dev/null +++ b/dev/basic_swerve/pico_sdk_import.cmake @@ -0,0 +1,73 @@ +# This is a copy of /external/pico_sdk_import.cmake + +# This can be dropped into an external project to help locate this SDK +# It should be include()ed prior to project() + +if (DEFINED ENV{PICO_SDK_PATH} AND (NOT PICO_SDK_PATH)) + set(PICO_SDK_PATH $ENV{PICO_SDK_PATH}) + message("Using PICO_SDK_PATH from environment ('${PICO_SDK_PATH}')") +endif () + +if (DEFINED ENV{PICO_SDK_FETCH_FROM_GIT} AND (NOT PICO_SDK_FETCH_FROM_GIT)) + set(PICO_SDK_FETCH_FROM_GIT $ENV{PICO_SDK_FETCH_FROM_GIT}) + message("Using PICO_SDK_FETCH_FROM_GIT from environment ('${PICO_SDK_FETCH_FROM_GIT}')") +endif () + +if (DEFINED ENV{PICO_SDK_FETCH_FROM_GIT_PATH} AND (NOT PICO_SDK_FETCH_FROM_GIT_PATH)) + set(PICO_SDK_FETCH_FROM_GIT_PATH $ENV{PICO_SDK_FETCH_FROM_GIT_PATH}) + message("Using PICO_SDK_FETCH_FROM_GIT_PATH from environment ('${PICO_SDK_FETCH_FROM_GIT_PATH}')") +endif () + +set(PICO_SDK_PATH "${PICO_SDK_PATH}" CACHE PATH "Path to the Raspberry Pi Pico SDK") +set(PICO_SDK_FETCH_FROM_GIT "${PICO_SDK_FETCH_FROM_GIT}" CACHE BOOL "Set to ON to fetch copy of SDK from git if not otherwise locatable") +set(PICO_SDK_FETCH_FROM_GIT_PATH "${PICO_SDK_FETCH_FROM_GIT_PATH}" CACHE FILEPATH "location to download SDK") + +if (NOT PICO_SDK_PATH) + if (PICO_SDK_FETCH_FROM_GIT) + include(FetchContent) + set(FETCHCONTENT_BASE_DIR_SAVE ${FETCHCONTENT_BASE_DIR}) + if (PICO_SDK_FETCH_FROM_GIT_PATH) + get_filename_component(FETCHCONTENT_BASE_DIR "${PICO_SDK_FETCH_FROM_GIT_PATH}" REALPATH BASE_DIR "${CMAKE_SOURCE_DIR}") + endif () + # GIT_SUBMODULES_RECURSE was added in 3.17 + if (${CMAKE_VERSION} VERSION_GREATER_EQUAL "3.17.0") + FetchContent_Declare( + pico_sdk + GIT_REPOSITORY https://github.com/raspberrypi/pico-sdk + GIT_TAG master + GIT_SUBMODULES_RECURSE FALSE + ) + else () + FetchContent_Declare( + pico_sdk + GIT_REPOSITORY https://github.com/raspberrypi/pico-sdk + GIT_TAG master + ) + endif () + + if (NOT pico_sdk) + message("Downloading Raspberry Pi Pico SDK") + FetchContent_Populate(pico_sdk) + set(PICO_SDK_PATH ${pico_sdk_SOURCE_DIR}) + endif () + set(FETCHCONTENT_BASE_DIR ${FETCHCONTENT_BASE_DIR_SAVE}) + else () + message(FATAL_ERROR + "SDK location was not specified. Please set PICO_SDK_PATH or set PICO_SDK_FETCH_FROM_GIT to on to fetch from git." + ) + endif () +endif () + +get_filename_component(PICO_SDK_PATH "${PICO_SDK_PATH}" REALPATH BASE_DIR "${CMAKE_BINARY_DIR}") +if (NOT EXISTS ${PICO_SDK_PATH}) + message(FATAL_ERROR "Directory '${PICO_SDK_PATH}' not found") +endif () + +set(PICO_SDK_INIT_CMAKE_FILE ${PICO_SDK_PATH}/pico_sdk_init.cmake) +if (NOT EXISTS ${PICO_SDK_INIT_CMAKE_FILE}) + message(FATAL_ERROR "Directory '${PICO_SDK_PATH}' does not appear to contain the Raspberry Pi Pico SDK") +endif () + +set(PICO_SDK_PATH ${PICO_SDK_PATH} CACHE PATH "Path to the Raspberry Pi Pico SDK" FORCE) + +include(${PICO_SDK_INIT_CMAKE_FILE}) diff --git a/dev/basic_swerve/procon.py b/dev/basic_swerve/procon.py new file mode 100644 index 0000000..8e04765 --- /dev/null +++ b/dev/basic_swerve/procon.py @@ -0,0 +1,331 @@ +#!/usr/bin/env python3 + +import math +import time + +import hid # pip install hidapi + + +def to_int16(uint16): + return -((uint16 ^ 0xFFFF) + 1) if uint16 & 0x8000 else uint16 + + +class ProCon: + VENDOR_ID = 0x057E + PRODUCT_ID = 0x2009 + PACKET_SIZE = 64 + CALIBRATION_OFFSET = 0x603D + CALIBRATION_LENGTH = 0x12 + COMMAND_RETRIES = 10 + RUMBLE_NEUTRAL = (0x00, 0x01, 0x40, 0x40) + RUMBLE = (0x74, 0xBE, 0xBD, 0x6F) + DEFAULT_IMU_SENSITIVITY = (0x03, 0x00, 0x00, 0x01) + + class OutputReportID: + RUMBLE_SUBCOMMAND = 0x01 + RUMBLE = 0x10 + COMMAND = 0x80 + + class InputReportID: + SUBCOMMAND_REPLY = 0x21 + CONTROLLER_STATE = 0x30 + COMMAND_ACK = 0x81 + + class CommandID: + HANDSHAKE = 0x02 + HIGH_SPEED = 0x03 + FORCE_USB = 0x04 + + class SubcommandID: + SET_INPUT_REPORT_MODE = 0x03 + SPI_FLASH_READ = 0x10 + SET_PLAYER_LIGHTS = 0x30 + SET_HOME_LIGHT = 0x38 + ENABLE_IMU = 0x40 + SET_IMU_SENSITIVITY = 0x41 + ENABLE_VIBRATION = 0x48 + + class Button: + A = "A" + B = "B" + X = "X" + Y = "Y" + UP = "Up" + DOWN = "Down" + LEFT = "Left" + RIGHT = "Right" + MINUS = "-" + PLUS = "+" + SCREENSHOT = "Screenshot" + HOME = "Home" + L = "L" + ZL = "ZL" + R = "R" + ZR = "ZR" + LS = "LS" + RS = "RS" + + def __init__(self): + self.subcommand_counter = 0 + self.dev = hid.device() + self.dev.open(ProCon.VENDOR_ID, ProCon.PRODUCT_ID) + self.handshake() + self.high_speed() + self.handshake() + self.rumble_low = self.rumble_high = ProCon.RUMBLE_NEUTRAL + self.rumble_expire = 0 + self.load_stick_calibration() + self.enable_vibration(True) + self.set_input_report_mode(ProCon.InputReportID.CONTROLLER_STATE) + self.force_usb() + self.set_player_lights(True, False, False, False) + self.enable_imu(True) + self.set_imu_sensitivity(ProCon.DEFAULT_IMU_SENSITIVITY) + + def start(self, callback): + while True: + state = self.recv() + if state[0] != ProCon.InputReportID.CONTROLLER_STATE: + continue + buttons = { + ProCon.Button.A: state[3] & 0x08 > 0, + ProCon.Button.B: state[3] & 0x04 > 0, + ProCon.Button.X: state[3] & 0x02 > 0, + ProCon.Button.Y: state[3] & 0x01 > 0, + ProCon.Button.UP: state[5] & 0x02 > 0, + ProCon.Button.DOWN: state[5] & 0x01 > 0, + ProCon.Button.LEFT: state[5] & 0x08 > 0, + ProCon.Button.RIGHT: state[5] & 0x04 > 0, + ProCon.Button.MINUS: state[4] & 0x01 > 0, + ProCon.Button.PLUS: state[4] & 0x02 > 0, + ProCon.Button.SCREENSHOT: state[4] & 0x20 > 0, + ProCon.Button.HOME: state[4] & 0x10 > 0, + ProCon.Button.L: state[5] & 0x40 > 0, + ProCon.Button.ZL: state[5] & 0x80 > 0, + ProCon.Button.R: state[3] & 0x40 > 0, + ProCon.Button.ZR: state[3] & 0x80 > 0, + ProCon.Button.LS: state[4] & 0x08 > 0, + ProCon.Button.RS: state[4] & 0x04 > 0, + } + l_x = state[6] | ((state[7] & 0xF) << 8) + l_y = (state[7] >> 4) | (state[8] << 4) + r_x = state[9] | ((state[10] & 0xF) << 8) + r_y = (state[10] >> 4) | (state[11] << 4) + l_x = self.apply_stick_calibration(l_x, 0, 0) + l_y = self.apply_stick_calibration(l_y, 0, 1) + r_x = self.apply_stick_calibration(r_x, 1, 0) + r_y = self.apply_stick_calibration(r_y, 1, 1) + l_stick = (l_x, l_y) + r_stick = (r_x, r_y) + accel = ( + state[13] | state[14] << 8, + state[15] | state[16] << 8, + state[17] | state[18] << 8, + ) + gyro = ( + state[19] | state[20] << 8, + state[21] | state[22] << 8, + state[23] | state[24] << 8, + ) + accel = tuple(map(to_int16, accel)) + gyro = tuple(map(to_int16, gyro)) + battery = (state[2] & 0xF0) >> 4 + callback(buttons, l_stick, r_stick, accel, gyro, battery) + if self.rumble_expire and int(time.time() * 1000) >= self.rumble_expire: + self.send_rumble(False, False, 0) + + def load_stick_calibration(self): + ok, reply = self.spi_flash_read( + ProCon.CALIBRATION_OFFSET, ProCon.CALIBRATION_LENGTH + ) + if not ok: + raise RuntimeError("cannot load stick calibration") + self.stick_calibration = [ + [ + [ + ((reply[27] & 0xF) << 8) | reply[26], + ((reply[24] & 0xF) << 8) | reply[23], + ((reply[21] & 0xF) << 8) | reply[20], + ], + [ + (reply[28] << 4) | (reply[27] >> 4), + (reply[25] << 4) | (reply[24] >> 4), + (reply[22] << 4) | (reply[21] >> 4), + ], + ], + [ + [ + ((reply[33] & 0xF) << 8) | reply[32], + ((reply[30] & 0xF) << 8) | reply[29], + ((reply[36] & 0xF) << 8) | reply[35], + ], + [ + (reply[34] << 4) | (reply[33] >> 4), + (reply[31] << 4) | (reply[30] >> 4), + (reply[37] << 4) | (reply[36] >> 4), + ], + ], + ] + for i in range(len(self.stick_calibration)): + for j in range(len(self.stick_calibration[i])): + for k in range(len(self.stick_calibration[i][j])): + if self.stick_calibration[i][j][k] == 0xFFF: + self.stick_calibration[i][j][k] = 0 + self.stick_extends = [ + [ + [ + -int(self.stick_calibration[0][0][0] * 0.7), + int(self.stick_calibration[0][0][2] * 0.7), + ], + [ + -int(self.stick_calibration[0][1][0] * 0.7), + int(self.stick_calibration[0][1][2] * 0.7), + ], + ], + [ + [ + -int(self.stick_calibration[1][0][0] * 0.7), + int(self.stick_calibration[1][0][2] * 0.7), + ], + [ + -int(self.stick_calibration[1][1][0] * 0.7), + int(self.stick_calibration[1][1][2] * 0.7), + ], + ], + ] + + def apply_stick_calibration(self, value, stick, axis): + value -= self.stick_calibration[stick][axis][1] + if value < self.stick_extends[stick][axis][0]: + self.stick_extends[stick][axis][0] = value + if value > self.stick_extends[stick][axis][1]: + self.stick_extends[stick][axis][1] = value + if value > 0: + return int(value * 0x7FFF / self.stick_extends[stick][axis][1]) + return int(value * -0x7FFF / self.stick_extends[stick][axis][0]) + + def send(self, data): + return self.dev.write(data) == len(data) + + def recv(self): + return self.dev.read(ProCon.PACKET_SIZE) + + def send_command(self, id, wait_for_reply=True): + data = (ProCon.OutputReportID.COMMAND, id) + for _ in range(ProCon.COMMAND_RETRIES): + if not self.send(data): + continue + if not wait_for_reply: + return True + reply = self.recv() + if reply[0] == ProCon.InputReportID.COMMAND_ACK and reply[1] == id: + return True + return False + + def send_subcommand(self, id, param, wait_for_reply=True): + data = ( + (ProCon.OutputReportID.RUMBLE_SUBCOMMAND, self.subcommand_counter) + + self.rumble_low + + self.rumble_high + + (id,) + + param + ) + self.subcommand_counter = (self.subcommand_counter + 1) & 0xFF + for _ in range(ProCon.COMMAND_RETRIES): + if not self.send(data): + continue + if not wait_for_reply: + return True, [] + reply = self.recv() + if reply[0] == ProCon.InputReportID.SUBCOMMAND_REPLY and reply[14] == id: + return True, reply + return False, [] + + def send_rumble(self, low, high, duration): + self.rumble_low = ProCon.RUMBLE if low else ProCon.RUMBLE_NEUTRAL + self.rumble_high = ProCon.RUMBLE if high else ProCon.RUMBLE_NEUTRAL + self.rumble_expire = ( + int(time.time() * 1000) + duration if (low or high) and duration else 0 + ) + data = ( + (ProCon.OutputReportID.RUMBLE, self.subcommand_counter) + + self.rumble_low + + self.rumble_high + ) + self.subcommand_counter = (self.subcommand_counter + 1) & 0xFF + for _ in range(ProCon.COMMAND_RETRIES): + if self.send(data): + return True + return False + + def handshake(self): + return self.send_command(ProCon.CommandID.HANDSHAKE) + + def high_speed(self): + return self.send_command(ProCon.CommandID.HIGH_SPEED) + + def force_usb(self): + return self.send_command(ProCon.CommandID.FORCE_USB, False) + + def set_input_report_mode(self, mode): + return self.send_subcommand(ProCon.SubcommandID.SET_INPUT_REPORT_MODE, (mode,)) + + def spi_flash_read(self, addr, l): + param = ( + addr & 0x000000FF, + (addr & 0x0000FF00) >> 8, + (addr & 0x00FF0000) >> 16, + (addr & 0xFF000000) >> 24, + l, + ) + return self.send_subcommand(ProCon.SubcommandID.SPI_FLASH_READ, param) + + def set_player_lights(self, one, two, three, four): + param = (one << 0) | (two << 1) | (three << 2) | (four << 3) + return self.send_subcommand(ProCon.SubcommandID.SET_PLAYER_LIGHTS, (param,)) + + def set_home_light(self, brightness): + intensity = 0 + if brightness > 0: + if brightness < 65: + intensity = (brightness + 5) // 10 + else: + intensity = math.ceil(0xF * ((brightness / 100) ** 2.13)) + intensity = (intensity & 0xF) << 4 + param = (0x01, intensity, intensity, 0x00) + return self.send_subcommand(ProCon.SubcommandID.SET_HOME_LIGHT, param) + + def enable_imu(self, enable): + return self.send_subcommand(ProCon.SubcommandID.ENABLE_IMU, (int(enable),)) + + def set_imu_sensitivity(self, sensitivity): + return self.send_subcommand( + ProCon.SubcommandID.SET_IMU_SENSITIVITY, sensitivity + ) + + def enable_vibration(self, enable): + return self.send_subcommand( + ProCon.SubcommandID.ENABLE_VIBRATION, (int(enable),) + ) + + +def print_state(buttons, l_stick, r_stick, accel, gyro, battery): + print("\33[2JButtons:") + for k, v in buttons.items(): + if v: + print("[{}]".format(k), end=" ") + else: + print(" {} ".format(k), end=" ") + print() + print("L Stick: ({:6}, {:6})".format(l_stick[0], l_stick[1])) + print("R Stick: ({:6}, {:6})".format(r_stick[0], r_stick[1])) + print("Accelerometer: ({:6}, {:6}, {:6})".format(accel[0], accel[1], accel[2])) + print("Gyroscope: ({:6}, {:6}, {:6})".format(gyro[0], gyro[1], gyro[2])) + print("Battery: {}/9".format(battery)) + + +if __name__ == "__main__": + try: + ProCon().start(print_state) + except KeyboardInterrupt: + print("\rGoodbye!") diff --git a/dev/basic_swerve/swerve_test.py b/dev/basic_swerve/swerve_test.py new file mode 100644 index 0000000..236f265 --- /dev/null +++ b/dev/basic_swerve/swerve_test.py @@ -0,0 +1,80 @@ +import math + +# radius of swerve drive base in meters +radius = 1 + +# vectors of swerve drive, using [speed (m/s), orientation (deg)] in global reference frame +swerve = [0,0] + +# vectors of motors in global frame +m1 = [0,0] +m2 = [0,0] +m3 = [0,0] + +# add two vectors +def add_two_vec(vec1, vec2): + print(vec1,vec2) + # get difference between two vectors, treating one vector as perpendicular to x axis + theta_diff = vec2[1] - vec1[1] + theta_diff = math.radians(theta_diff) + print(theta_diff) + + # since vec1 is x axis, vector 1 contributes in only the x axis + # when breaking into components, just add vec2 in x to vec1 magnitude to get x + # vec2 in y to get y + # sqrt(a^2+b^2) to get magnitude, and arctan(y/x) to get angle relative to x (add to vec1 orientation) + x_comp = vec1[0] + math.cos(theta_diff) * vec2[0] + print(x_comp) + y_comp = math.sin(theta_diff) * vec2[0] + print(y_comp) + f_mag = math.sqrt(x_comp**2 + y_comp**2) + print(f_mag) + f_angle = math.atan2(y_comp, x_comp) + math.radians(vec1[1]) + print(f_angle) + f_angle = math.degrees(f_angle) + print(f_angle) + if f_angle < -180: + f_angle += 360 + elif f_angle > 180: + f_angle -= 360 + return [f_mag, f_angle] + + +# input velocity [speed (m/s), orientation (deg)] in local reference frame +# rotation (rad/s) +def convert(velocity, rotation): + + # the vector for each motor in global reference frame is given by adding + # 1) velocity (relative to global reference frame) + # 2) rotation vector (vector perpendicular to each motor relative to the body reference frame) + # because the frame is circular, magnitude of vector is calculated by V = rw (formula for tangential speed relative to radius and rad/s) + + # setting all vectors to velocity relative to local reference frame + m1 = [velocity[0], velocity[1]] + m2 = [velocity[0], velocity[1]-120] + m3 = [velocity[0], velocity[1]-240] + + # rotation vector relative to global reference + # three motors, with motor 1 having an offset of 0 relative to frame + # because rotation vectors must be perpendicular, add 90 to each + # to convert from local to global, add orientation of frame in global reference frame + # rot_ang_m1 = 0+90+swerve[1] + # rot_ang_m2 = 120+90+swerve[1] + # rot_ang_m3 = 240+90+swerve[1] + + # create magnitude for each vector + rot_mag = rotation * radius + dir = 1 + + if rot_mag < 0: + dir *= -1 + rot_mag *= -1 + print(rot_mag) + # add two vectors (in local frame) based on direction and magnitude + m1 = add_two_vec(m1, [rot_mag, 90*dir]) + m2 = add_two_vec(m2, [rot_mag, 90*dir]) + m3 = add_two_vec(m3, [rot_mag, 90*dir]) + + print(m1, m2, m3) + +convert([10,0],0) \ No newline at end of file diff --git a/dev/encoder/CMakeLists.txt b/dev/encoder/CMakeLists.txt new file mode 100644 index 0000000..fc8ee89 --- /dev/null +++ b/dev/encoder/CMakeLists.txt @@ -0,0 +1,26 @@ +cmake_minimum_required(VERSION 3.12) +include(pico_sdk_import.cmake) +project(quadrature_encoder VERSION 1.0.0) + +add_executable(quadrature_encoder quadrature_encoder.cpp) + +set(PICO_CXX_ENABLE_EXCEPTIONS 1) + +pico_sdk_init() + +target_sources(quadrature_encoder PRIVATE quadrature_encoder.cpp) + +target_link_libraries(quadrature_encoder PRIVATE + pico_stdlib + hardware_pio + ) + +pico_generate_pio_header(quadrature_encoder ${CMAKE_CURRENT_LIST_DIR}/quadrature_encoder.pio) + +pico_add_extra_outputs(quadrature_encoder) + +# enable usb output, disable uart output +pico_enable_stdio_usb(quadrature_encoder 1) +pico_enable_stdio_uart(quadrature_encoder 0) + + diff --git a/dev/encoder/pico_sdk_import.cmake b/dev/encoder/pico_sdk_import.cmake new file mode 100644 index 0000000..28efe9e --- /dev/null +++ b/dev/encoder/pico_sdk_import.cmake @@ -0,0 +1,62 @@ +# This is a copy of /external/pico_sdk_import.cmake + +# This can be dropped into an external project to help locate this SDK +# It should be include()ed prior to project() + +if (DEFINED ENV{PICO_SDK_PATH} AND (NOT PICO_SDK_PATH)) + set(PICO_SDK_PATH $ENV{PICO_SDK_PATH}) + message("Using PICO_SDK_PATH from environment ('${PICO_SDK_PATH}')") +endif () + +if (DEFINED ENV{PICO_SDK_FETCH_FROM_GIT} AND (NOT PICO_SDK_FETCH_FROM_GIT)) + set(PICO_SDK_FETCH_FROM_GIT $ENV{PICO_SDK_FETCH_FROM_GIT}) + message("Using PICO_SDK_FETCH_FROM_GIT from environment ('${PICO_SDK_FETCH_FROM_GIT}')") +endif () + +if (DEFINED ENV{PICO_SDK_FETCH_FROM_GIT_PATH} AND (NOT PICO_SDK_FETCH_FROM_GIT_PATH)) + set(PICO_SDK_FETCH_FROM_GIT_PATH $ENV{PICO_SDK_FETCH_FROM_GIT_PATH}) + message("Using PICO_SDK_FETCH_FROM_GIT_PATH from environment ('${PICO_SDK_FETCH_FROM_GIT_PATH}')") +endif () + +set(PICO_SDK_PATH "${PICO_SDK_PATH}" CACHE PATH "Path to the Raspberry Pi Pico SDK") +set(PICO_SDK_FETCH_FROM_GIT "${PICO_SDK_FETCH_FROM_GIT}" CACHE BOOL "Set to ON to fetch copy of SDK from git if not otherwise locatable") +set(PICO_SDK_FETCH_FROM_GIT_PATH "${PICO_SDK_FETCH_FROM_GIT_PATH}" CACHE FILEPATH "location to download SDK") + +if (NOT PICO_SDK_PATH) + if (PICO_SDK_FETCH_FROM_GIT) + include(FetchContent) + set(FETCHCONTENT_BASE_DIR_SAVE ${FETCHCONTENT_BASE_DIR}) + if (PICO_SDK_FETCH_FROM_GIT_PATH) + get_filename_component(FETCHCONTENT_BASE_DIR "${PICO_SDK_FETCH_FROM_GIT_PATH}" REALPATH BASE_DIR "${CMAKE_SOURCE_DIR}") + endif () + FetchContent_Declare( + pico_sdk + GIT_REPOSITORY https://github.com/raspberrypi/pico-sdk + GIT_TAG master + ) + if (NOT pico_sdk) + message("Downloading Raspberry Pi Pico SDK") + FetchContent_Populate(pico_sdk) + set(PICO_SDK_PATH ${pico_sdk_SOURCE_DIR}) + endif () + set(FETCHCONTENT_BASE_DIR ${FETCHCONTENT_BASE_DIR_SAVE}) + else () + message(FATAL_ERROR + "SDK location was not specified. Please set PICO_SDK_PATH or set PICO_SDK_FETCH_FROM_GIT to on to fetch from git." + ) + endif () +endif () + +get_filename_component(PICO_SDK_PATH "${PICO_SDK_PATH}" REALPATH BASE_DIR "${CMAKE_BINARY_DIR}") +if (NOT EXISTS ${PICO_SDK_PATH}) + message(FATAL_ERROR "Directory '${PICO_SDK_PATH}' not found") +endif () + +set(PICO_SDK_INIT_CMAKE_FILE ${PICO_SDK_PATH}/pico_sdk_init.cmake) +if (NOT EXISTS ${PICO_SDK_INIT_CMAKE_FILE}) + message(FATAL_ERROR "Directory '${PICO_SDK_PATH}' does not appear to contain the Raspberry Pi Pico SDK") +endif () + +set(PICO_SDK_PATH ${PICO_SDK_PATH} CACHE PATH "Path to the Raspberry Pi Pico SDK" FORCE) + +include(${PICO_SDK_INIT_CMAKE_FILE}) diff --git a/dev/encoder/quadrature_encoder.cpp b/dev/encoder/quadrature_encoder.cpp new file mode 100644 index 0000000..a1e3079 --- /dev/null +++ b/dev/encoder/quadrature_encoder.cpp @@ -0,0 +1,145 @@ +/** + * Copyright (c) 2023 Raspberry Pi (Trading) Ltd. + * + * SPDX-License-Identifier: BSD-3-Clause + */ + +#include +#include +#include "pico/stdlib.h" +#include "hardware/pio.h" +#include "hardware/timer.h" + +#include "quadrature_encoder.pio.h" + +// +// ---- quadrature encoder interface example +// +// the PIO program reads phase A/B of a quadrature encoder and increments or +// decrements an internal counter to keep the current absolute step count +// updated. At any point, the main code can query the current count by using +// the quadrature_encoder_*_count functions. The counter is kept in a full +// 32 bit register that just wraps around. Two's complement arithmetic means +// that it can be interpreted as a 32-bit signed or unsigned value, and it will +// work anyway. +// +// As an example, a two wheel robot being controlled at 100Hz, can use two +// state machines to read the two encoders and in the main control loop it can +// simply ask for the current encoder counts to get the absolute step count. It +// can also subtract the values from the last sample to check how many steps +// each wheel as done since the last sample period. +// +// One advantage of this approach is that it requires zero CPU time to keep the +// encoder count updated and because of that it supports very high step rates. +// + +// 374 pulses per revolution (from the description at https://www.dfrobot.com/product-1462.html) +// 4x because the encoder has 2 sensors each w/ rising & falling edges, so each pulse results in +// 4 counts received (https://deltamotion.com/support/webhelp/rmctools/Controller_Features/Transducer_Basics/Quadrature_Fundamentals.htm) +const float ROT_PER_TICK = 1.0 / (4 * 374.0); +const float PULLEY_RATIO = 0.3185 / 1.528; +const float DEG_PER_ROT = 360.0; + +class Encoder +{ +public: + // Create an encoder. Reccommended NOT to use this class, use EncoderFactory::createEncoder() + // @param pinA the pin that encoder A channel is connected to, the B channel should connect to the next pin + // @param sm the state machine to keep track of the encoder, 0-3 + // @param which pio + // @param ratio the ratio by which to multiply encoder ticks + Encoder(uint pinA, uint sm, PIO pio, float ratio = 1.0, bool addProgram = true) + { + this->pio = pio; + this->sm = sm; + this->ratio = ratio; + + uint offset = 0; + + // we don't really need to keep the offset, as this program must be loaded + // at offset 0 + if (addProgram) + uint offset = pio_add_program(pio, &quadrature_encoder_program); + + quadrature_encoder_program_init(pio, sm, offset, pinA, 0); + } + + // updates the pos and velocity, call periodically. + // @param delta_time the time, in miliseconds, since last calling update + void update(int delta_time) + { + pos = quadrature_encoder_get_count(pio, sm) * ratio * DEG_PER_ROT; + velocity = ((prev_pos - pos) / delta_time) * 1000; + prev_pos = pos; + } + + // get position of wheel in ticks, multiplied by any provided ratio. resets on init. + // update() must be called periodically for this to be accurate + float get_pos() + { + return pos; + } + + // get velocity of wheel in ticks per second, multiplied by any provided ratio. + // update() must be called periodically for this to be accurate + float get_velocity() + { + return velocity; + } + +private: + float prev_pos, pos; + float velocity; + float ratio; + PIO pio; + uint sm; +}; + +class EncoderFactory +{ +public: + // Create an encoder, automatically configuring the state machine and pio. + // @param pinA the A encoder channel, the B channel should be connected to the next pin + // @param ratio the ratio by which to multiply encoder outputs. ratio of 1 results in tick / sec + static Encoder createEncoder(uint pinA, float ratio = 1.0) + { + if (encoder_count > 7) + { + throw std::out_of_range("reached encoder limit of 8"); + } + + uint sm = encoder_count % 4; + PIO pio = encoder_count < 4 ? pio0 : pio1; + + encoder_count++; + return Encoder(pinA, sm, pio, ratio, sm == 0); + } + +private: + static uint encoder_count; +}; + +uint EncoderFactory::encoder_count = 0; + +int main() +{ + stdio_init_all(); + + // Base pin to connect the A phase of the encoder (yellow wire). + // The B phase must be connected to the next pin (green wire) + const uint PIN_STEER = 14; + const uint PIN_DRIVE = 16; + + Encoder steer = EncoderFactory::createEncoder(PIN_STEER, ROT_PER_TICK * DEG_PER_ROT * PULLEY_RATIO); + Encoder drive = EncoderFactory::createEncoder(PIN_DRIVE, ROT_PER_TICK * DEG_PER_ROT); + + while (1) + { + steer.update(20); + drive.update(20); + + printf("steer position %8f, velocity %6f\n", steer.get_pos(), steer.get_velocity()); + printf("drive position %8f, velocity %6f\n", drive.get_pos(), drive.get_velocity()); + sleep_ms(20); + } +} diff --git a/dev/encoder/quadrature_encoder.pio b/dev/encoder/quadrature_encoder.pio new file mode 100644 index 0000000..8b1e618 --- /dev/null +++ b/dev/encoder/quadrature_encoder.pio @@ -0,0 +1,141 @@ +; +; Copyright (c) 2023 Raspberry Pi (Trading) Ltd. +; +; SPDX-License-Identifier: BSD-3-Clause +; + +.program quadrature_encoder + +; the code must be loaded at address 0, because it uses computed jumps +.origin 0 + + +; the code works by running a loop that continuously shifts the 2 phase pins into +; ISR and looks at the lower 4 bits to do a computed jump to an instruction that +; does the proper "do nothing" | "increment" | "decrement" action for that pin +; state change (or no change) + +; ISR holds the last state of the 2 pins during most of the code. The Y register +; keeps the current encoder count and is incremented / decremented according to +; the steps sampled + +; the program keeps trying to write the current count to the RX FIFO without +; blocking. To read the current count, the user code must drain the FIFO first +; and wait for a fresh sample (takes ~4 SM cycles on average). The worst case +; sampling loop takes 10 cycles, so this program is able to read step rates up +; to sysclk / 10 (e.g., sysclk 125MHz, max step rate = 12.5 Msteps/sec) + +; 00 state + JMP update ; read 00 + JMP decrement ; read 01 + JMP increment ; read 10 + JMP update ; read 11 + +; 01 state + JMP increment ; read 00 + JMP update ; read 01 + JMP update ; read 10 + JMP decrement ; read 11 + +; 10 state + JMP decrement ; read 00 + JMP update ; read 01 + JMP update ; read 10 + JMP increment ; read 11 + +; to reduce code size, the last 2 states are implemented in place and become the +; target for the other jumps + +; 11 state + JMP update ; read 00 + JMP increment ; read 01 +decrement: + ; note: the target of this instruction must be the next address, so that + ; the effect of the instruction does not depend on the value of Y. The + ; same is true for the "JMP X--" below. Basically "JMP Y--, " + ; is just a pure "decrement Y" instruction, with no other side effects + JMP Y--, update ; read 10 + + ; this is where the main loop starts +.wrap_target +update: + MOV ISR, Y ; read 11 + PUSH noblock + +sample_pins: + ; we shift into ISR the last state of the 2 input pins (now in OSR) and + ; the new state of the 2 pins, thus producing the 4 bit target for the + ; computed jump into the correct action for this state. Both the PUSH + ; above and the OUT below zero out the other bits in ISR + OUT ISR, 2 + IN PINS, 2 + + ; save the state in the OSR, so that we can use ISR for other purposes + MOV OSR, ISR + ; jump to the correct state machine action + MOV PC, ISR + + ; the PIO does not have a increment instruction, so to do that we do a + ; negate, decrement, negate sequence +increment: + MOV Y, ~Y + JMP Y--, increment_cont +increment_cont: + MOV Y, ~Y +.wrap ; the .wrap here avoids one jump instruction and saves a cycle too + + + +% c-sdk { + +#include "hardware/clocks.h" +#include "hardware/gpio.h" + +// max_step_rate is used to lower the clock of the state machine to save power +// if the application doesn't require a very high sampling rate. Passing zero +// will set the clock to the maximum + +static inline void quadrature_encoder_program_init(PIO pio, uint sm, uint offset, uint pin, int max_step_rate) +{ + pio_sm_set_consecutive_pindirs(pio, sm, pin, 2, false); + gpio_pull_up(pin); + gpio_pull_up(pin + 1); + + pio_sm_config c = quadrature_encoder_program_get_default_config(offset); + + sm_config_set_in_pins(&c, pin); // for WAIT, IN + sm_config_set_jmp_pin(&c, pin); // for JMP + // shift to left, autopull disabled + sm_config_set_in_shift(&c, false, false, 32); + // don't join FIFO's + sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_NONE); + + // passing "0" as the sample frequency, + if (max_step_rate == 0) { + sm_config_set_clkdiv(&c, 1.0); + } else { + // one state machine loop takes at most 10 cycles + float div = (float)clock_get_hz(clk_sys) / (10 * max_step_rate); + sm_config_set_clkdiv(&c, div); + } + + pio_sm_init(pio, sm, offset, &c); + pio_sm_set_enabled(pio, sm, true); +} + +static inline int32_t quadrature_encoder_get_count(PIO pio, uint sm) +{ + uint ret; + int n; + + // if the FIFO has N entries, we fetch them to drain the FIFO, + // plus one entry which will be guaranteed to not be stale + n = pio_sm_get_rx_fifo_level(pio, sm) + 1; + while (n > 0) { + ret = pio_sm_get_blocking(pio, sm); + n--; + } + return ret; +} + +%} diff --git a/dev/i2c-comms/CMakeLists.txt b/dev/i2c-comms/CMakeLists.txt index b961952..2fd69ae 100644 --- a/dev/i2c-comms/CMakeLists.txt +++ b/dev/i2c-comms/CMakeLists.txt @@ -34,7 +34,10 @@ pico_enable_stdio_usb(pico_i2c 1) # Add the standard library to the build target_link_libraries(pico_i2c - pico_stdlib) + pico_stdlib + pico_i2c_slave + hardware_i2c + ) # Add the standard include files to the build target_include_directories(pico_i2c PRIVATE diff --git a/dev/i2c-comms/pi_i2c.py b/dev/i2c-comms/pi_i2c.py index a4b07c7..e1a1291 100644 --- a/dev/i2c-comms/pi_i2c.py +++ b/dev/i2c-comms/pi_i2c.py @@ -2,9 +2,10 @@ import os import time + I2C_PRIM = 0x0703 -# open i2c devices +# open i2c devices (sudo apt install i2c-tools) i2c_fd = os.open("/dev/i2c-1", os.O_RDWR) # set the i2c address of pico @@ -13,6 +14,17 @@ # send data to pico data = [0x01, 0x02, 0x03] # example data -while (True): - os.write(i2c_fd, bytes(data)) - time.sleep(1) +while True: + try: + os.write(i2c_fd, bytes(data)) + time.sleep(0.02) + print("Sent data to Pico: ", list(data)) + except OSError: + print("Remote I/O Error") + # read data from pico + try: + incoming_data = os.read(i2c_fd, 3) # read 3 bytes + time.sleep(0.02) + print("Received data from Pico: ", list(incoming_data)) + except TimeoutError: + print("Timeout Error") diff --git a/dev/i2c-comms/pi_spi.c b/dev/i2c-comms/pi_spi.c new file mode 100644 index 0000000..b98a690 --- /dev/null +++ b/dev/i2c-comms/pi_spi.c @@ -0,0 +1,63 @@ +#include +#include +#include +#include +#include +#include + +#define SPI_DEVICE "/dev/spidev0.0" +#define SPI_SPEED 1000000 // 1MHz + +int main() +{ + int spi_fd; + unsigned char tx_data[] = {0xAA, 0xBB, 0xCC, 0xDD}; + unsigned char rx_data[sizeof(tx_data)]; + + spi_fd = open(SPI_DEVICE, O_RDWR); + if (spi_fd < 0) + { + perror("Error opening SPI device"); + return -1; + } + + int mode = SPI_MODE_0; + if (ioctl(spi_fd, SPI_IOC_WR_MODE, &mode) == -1) + { + perror("Error setting SPI mode"); + return -1; + } + + if (ioctl(spi_fd, SPI_IOC_WR_MAX_SPEED_HZ, &SPI_SPEED) == -1) + { + perror("Error setting SPI speed"); + return -1; + } + + struct spi_ioc_transfer transfer = { + .tx_buf = (unsigned long)tx_data, + .rx_buf = (unsigned long)rx_data, + .len = sizeof(tx_data), + .speed_hz = SPI_SPEED, + .bits_per_word = 8, + }; + + while (1) + { + if (ioctl(spi_fd, SPI_IOC_MESSAGE(1), &transfer) == -1) + { + perror("Error during SPI message transfer"); + return -1; + } + + printf("Received data: "); + for (int i = 0; i < sizeof(rx_data); i++) + { + printf(" %02X", rx_data[i]); + } + printf("\n"); + sleep(1); + } + close(spi_fd); + return 0; +} \ No newline at end of file diff --git a/dev/i2c-comms/pico_i2c.c b/dev/i2c-comms/pico_i2c.c index 948dc18..7bde92b 100644 --- a/dev/i2c-comms/pico_i2c.c +++ b/dev/i2c-comms/pico_i2c.c @@ -1,32 +1,78 @@ #include -#include "pico/stdlib.h" -#include "hardware/i2c.h" +#include +#include +#include -int main() { +#ifndef PICO_DEFAULT_LED_PIN +#warning blink requires a board with a regular LED +#else +const uint LED_PIN = PICO_DEFAULT_LED_PIN; +#endif + +uint8_t outgoing_data[4] = {0x11, 0x12, 0x13, 0x14}; // example data +uint8_t incoming_data[3]; +int data_index = 0; + +static void i2c_handler(i2c_inst_t *i2c, i2c_slave_event_t event) +{ + switch (event) + { + case I2C_SLAVE_RECEIVE: // master has written some data + for (int i = 0; i < 3; i++) + { + if (incoming_data[i] == 0x00) + { + incoming_data[i] = i2c_read_byte_raw(i2c); + printf("Received data %d: 0x%02X\n ", i, incoming_data[i]); + gpio_put(LED_PIN, 1); + break; + } + } + break; + case I2C_SLAVE_REQUEST: // master is requesting data + i2c_write_byte_raw(i2c, outgoing_data[data_index]); + printf("Sent data %d: 0x%02X\n ", data_index, outgoing_data[data_index]); + gpio_put(LED_PIN, 0); + data_index++; + if (data_index > 3) + { + data_index = 0; + } + break; + case I2C_SLAVE_FINISH: // master has signalled Stop / Restart + data_index = 0; + for (int i = 0; i < 3; i++) + { + incoming_data[i] = 0x00; + } + printf("reset\n"); + break; + default: + break; + } +} + +int main() +{ stdio_init_all(); +#ifndef PICO_DEFAULT_LED_PIN +#warning blink requires a board with a regular LED +#else + gpio_init(LED_PIN); + gpio_set_dir(LED_PIN, GPIO_OUT); +#endif + // init i2c at 100kHz i2c_init(i2c0, 100 * 1000); gpio_set_function(0, GPIO_FUNC_I2C); gpio_set_function(1, GPIO_FUNC_I2C); - // i2c_pullup_en(i2c0, true); // set i2c address for pico - i2c_set_slave_mode(i2c0, true, 0x08); - // i2c_set_slave_address(i2c0, 0x08); // address should match pi code - - uint8_t data[3]; - - while(1) { - // read data from i2c_pi.py - i2c_read_blocking(i2c0, 0x08, data, 3, true); - - // process data - for (int i = 0; i < 3; i++) { - printf("Received data %d: 0x%02X\n ", i, data[i]); - } - } + i2c_slave_init(i2c0, 0x08, &i2c_handler); - return 0; + while (1) + ; + return 0; } \ No newline at end of file diff --git a/dev/i2c-comms/pico_spi.c b/dev/i2c-comms/pico_spi.c new file mode 100644 index 0000000..a3d8b18 --- /dev/null +++ b/dev/i2c-comms/pico_spi.c @@ -0,0 +1,35 @@ +#include +#include "pico/stdlib.h" +#include "hardware/spi.h" + +#define SPI_PORT spi0 +#define PIN_MISO 16 +#define PIN_CS 17 + +int main() +{ + stdio_init_all(); + + spi_init(SPI_PORT, 1000 * 1000); // init at 1MHz + gpio_set_function(PIN_MISO, GPIO_FUNC_SPI); // set pin to SPI mode + + gpi_init(PIN_CS); + gpio_set_dir(PIN_CS, GPIO_OUT); + + while (1) + { + gpio_put(PIN_CS, 1); // set CS high to indiciate start of communication + uint8_t rx_data[4]; + spi_read_blocking(SPI_PORT, 0, rx_data, sizeof(rx_data)); // read data from pi + gpio_put(PIN_CS, 0); // set CS low to indicate end of communication + + printf("Received: "); + for (int i = 0; i < sizeof(rx_data); i++) + { + printf(" %02X", rx_data[i]); + } + printf("\n"); + sleep_ms(1000); + } + return 0; +} \ No newline at end of file diff --git a/dev/i2c_drive/CMakeLists.txt b/dev/i2c_drive/CMakeLists.txt new file mode 100644 index 0000000..914fc65 --- /dev/null +++ b/dev/i2c_drive/CMakeLists.txt @@ -0,0 +1,55 @@ +# Generated Cmake Pico project file + +cmake_minimum_required(VERSION 3.5) + +set(CMAKE_C_STANDARD 11) +set(CMAKE_CXX_STANDARD 17) + +# Initialise pico_sdk from installed location +# (note this can come from environment, CMake cache etc) + +set(PICO_BOARD pico CACHE STRING "Board type") + +# Pull in Raspberry Pi Pico SDK (must be before project) +include(pico_sdk_import.cmake) + +if (PICO_SDK_VERSION_STRING VERSION_LESS "1.4.0") + message(FATAL_ERROR "Raspberry Pi Pico SDK version 1.4.0 (or later) required. Your version is ${PICO_SDK_VERSION_STRING}") +endif() + +project(i2c_drive_pico C CXX ASM) + +# Initialise the Raspberry Pi Pico SDK +pico_sdk_init() + +# Add executable. Default name is the project name, version 0.1 + +add_executable(i2c_drive_pico i2c_drive_pico.c ) + +pico_set_program_name(i2c_drive_pico "i2c_drive_pico") +pico_set_program_version(i2c_drive_pico "0.1") + +pico_enable_stdio_uart(i2c_drive_pico 0) +pico_enable_stdio_usb(i2c_drive_pico 1) + +# Add the standard library to the build +target_link_libraries(i2c_drive_pico + pico_stdlib + pico_i2c_slave + hardware_i2c + hardware_pwm + ) + +# Add the standard include files to the build +target_include_directories(i2c_drive_pico PRIVATE + ${CMAKE_CURRENT_LIST_DIR} + ${CMAKE_CURRENT_LIST_DIR}/.. # for our common lwipopts or any other standard includes, if required +) + +# Add any user requested libraries +target_link_libraries(i2c_drive_pico + hardware_i2c + ) + +pico_add_extra_outputs(i2c_drive_pico) + diff --git a/dev/i2c_drive/Controller.py b/dev/i2c_drive/Controller.py new file mode 100644 index 0000000..a7e9ace --- /dev/null +++ b/dev/i2c_drive/Controller.py @@ -0,0 +1,204 @@ +import math +import threading + +from inputs import \ + get_gamepad # Import the get_gamepad function from the inputs module +from procon import ProCon # Import the ProCon class from the procon module + + +# This class represents a PS4 Controller +class PS4_Controller(object): + def __init__(self): + self.MAX_TRIG_VAL = math.pow(2, 8) # Maximum value for trigger input + self.MAX_JOY_VAL = math.pow(2, 7) # Maximum value for joystick input + self.THRESHOLD = 0.03 # Threshold for joystick deadzone + self.reset_vars() # Reset all controller variables to their initial state + self.start_thread(()) # Start a new thread to monitor the controller + + # This method resets all controller variables to their initial state + def reset_vars(self): + # Initialize all controller variables to 0 + self.LeftJoystickY = 0 + self.LeftJoystickX = 0 + self.RightJoystickY = 0 + self.RightJoystickX = 0 + self.LeftTrigger = 0 + self.RightTrigger = 0 + self.LeftBumper = 0 + self.RightBumper = 0 + self.A = 0 + self.X = 0 + self.Y = 0 + self.B = 0 + self.LeftThumb = 0 + self.RightThumb = 0 + self.Back = 0 + self.Start = 0 + self.LeftDPad = 0 + self.RightDPad = 0 + self.UpDPad = 0 + self.DownDPad = 0 + + # This method starts a new thread to monitor the controller + def start_thread(self, thread_args=()): + self._monitor_thread = threading.Thread( + target=self._monitor_controller, args=thread_args + ) + self._monitor_thread.daemon = ( + True # Set the thread as a daemon so it will end when the main program ends + ) + self._monitor_thread.start() # Start the thread + + # This method returns the current state of all buttons/triggers + def read(self): + return [ + self.LeftJoystickY, + self.LeftJoystickX, + self.RightJoystickY, + self.RightJoystickX, + self.LeftTrigger, + self.RightTrigger, + self.LeftBumper, + self.RightBumper, + self.A, + self.B, + self.X, + self.Y, + self.LeftThumb, + self.RightThumb, + self.Back, + self.Start, + self.LeftDPad, + self.RightDPad, + self.UpDPad, + self.DownDPad, + ] + + # This method returns the controller object itself + def read_self(self): + return self + + # This method applies a threshold to a value + def threshold(self, val): + return val - 1.0 if abs(val - 1.0) > self.THRESHOLD else 0 + + def _monitor_controller(self): + while True: + events = get_gamepad() + for event in events: + if event.code == "ABS_Y": + self.LeftJoystickY = self.threshold( + event.state / self.MAX_JOY_VAL + ) # normalize between -1 and 1 + elif event.code == "ABS_X": + self.LeftJoystickX = self.threshold( + event.state / self.MAX_JOY_VAL + ) # normalize between -1 and 1 + elif event.code == "ABS_RY": + self.RightJoystickY = self.threshold( + event.state / self.MAX_JOY_VAL + ) # normalize between -1 and 1 + elif event.code == "ABS_RX": + self.RightJoystickX = self.threshold( + event.state / self.MAX_JOY_VAL + ) # normalize between -1 and 1 + elif event.code == "ABS_Z": + self.LeftTrigger = self.threshold( + event.state / self.MAX_TRIG_VAL + ) # normalize between 0 and 1 + elif event.code == "ABS_RZ": + self.RightTrigger = self.threshold( + event.state / self.MAX_TRIG_VAL + ) # normalize between 0 and 1 + elif event.code == "BTN_TL": + self.LeftBumper = event.state + elif event.code == "BTN_TR": + self.RightBumper = event.state + elif event.code == "BTN_SOUTH": + self.A = event.state + elif event.code == "BTN_NORTH": + self.Y = event.state # previously switched with X + elif event.code == "BTN_WEST": + self.X = event.state # previously switched with Y + elif event.code == "BTN_EAST": + self.B = event.state + elif event.code == "BTN_THUMBL": + self.LeftThumb = event.state + elif event.code == "BTN_THUMBR": + self.RightThumb = event.state + elif event.code == "BTN_SELECT": + self.Back = event.state + elif event.code == "BTN_START": + self.Start = event.state + elif event.code == "BTN_TRIGGER_HAPPY1": + self.LeftDPad = event.state + elif event.code == "BTN_TRIGGER_HAPPY2": + self.RightDPad = event.state + elif event.code == "BTN_TRIGGER_HAPPY3": + self.UpDPad = event.state + elif event.code == "BTN_TRIGGER_HAPPY4": + self.DownDPad = event.state + + +# This class represents the Xbox Controller in WRP used for the CPSRC GEM +class Gem_Xbox_Controller(PS4_Controller): + def __init__(self): + self.MAX_TRIG_VAL = math.pow(2, 8) # Maximum value for trigger input + self.MAX_JOY_VAL = math.pow(2, 15) # Maximum value for joystick input + self.THRESHOLD = 0.03 # Threshold for joystick deadzone + + self.reset_vars() # Reset all controller variables to their initial state + self.start_thread(()) # Start a new thread to monitor the controller + + +# This class represents the Nintendo Pro Controller +class Nintendo_Pro_Controller(PS4_Controller): + def __init__(self): + self.MAX_TRIG_VAL = math.pow(2, 8) # Maximum value for trigger input + self.MAX_JOY_VAL = math.pow(2, 15) # Maximum value for joystick input + self.THRESHOLD = 0.1 # Threshold for joystick deadzone + self.controller = ProCon() # Initialize the ProCon controller + + self.reset_vars() # Reset all controller variables to their initial state + self.start_thread( + self.procon_callback_func + ) # Start a new thread to monitor the controller + + # This method is called when the ProCon controller state changes + def procon_callback_func(self, buttons, l_stick, r_stick, *_): + # Update the controller variables based on the new state + # The joystick values are normalized between -1 and 1 + # The threshold method is used to apply a deadband to the joystick values + # The button values are either 0 or 1 + self.LeftJoystickX = self.threshold(l_stick[0] / self.MAX_JOY_VAL) + self.LeftJoystickY = self.threshold(l_stick[1] / self.MAX_JOY_VAL) + self.RightJoystickX = self.threshold(r_stick[0] / self.MAX_JOY_VAL) + self.RightJoystickY = self.threshold(r_stick[1] / self.MAX_JOY_VAL) + self.LeftTrigger = self.threshold(buttons[ProCon.Button.ZL]) + self.RightTrigger = self.threshold(buttons[ProCon.Button.ZR]) + self.LeftBumper = buttons[ProCon.Button.L] + self.RightBumper = buttons[ProCon.Button.R] + self.A = buttons[ProCon.Button.A] + self.B = buttons[ProCon.Button.B] + self.X = buttons[ProCon.Button.X] + self.Y = buttons[ProCon.Button.Y] + self.LeftThumb = buttons[ProCon.Button.LS] + self.RightThumb = buttons[ProCon.Button.RS] + self.Back = buttons[ProCon.Button.MINUS] + self.Start = buttons[ProCon.Button.PLUS] + self.LeftDPad = buttons[ProCon.Button.LEFT] + self.RightDPad = buttons[ProCon.Button.RIGHT] + self.UpDPad = buttons[ProCon.Button.UP] + self.DownDPad = buttons[ProCon.Button.DOWN] + + +if __name__ == "__main__": + joy = PS4_Controller() # Initialize a PS4 controller + # joy = Gem_Xbox_Controller() # Initialize a Gem Xbox controller + # joy = Nintendo_Pro_Controller() # Initialize a Nintendo Pro controller + while True: + try: + print(joy.read()) # Print the current state of the controller + except Exception as e: + print("error!", e) # Print any errors that occur + break # Exit the loop if an error occurs diff --git a/dev/i2c_drive/i2c_drive_pi.py b/dev/i2c_drive/i2c_drive_pi.py new file mode 100644 index 0000000..230f98e --- /dev/null +++ b/dev/i2c_drive/i2c_drive_pi.py @@ -0,0 +1,59 @@ +import fcntl +import os +import time +import struct + +from Controller import PS4_Controller + +I2C_PRIM = 0x0703 # I2C primary address + +# Open i2c devices (sudo apt install i2c-tools) +i2c_fd = os.open("/dev/i2c-1", os.O_RDWR) # File descriptor for the i2c device + +# Set the i2c address of pico +pico_address = 0x08 # Address of the pico on the i2c bus +fcntl.ioctl( + i2c_fd, I2C_PRIM, pico_address +) # Set the address of the i2c device to communicate with + +# Send data to pico +joy = PS4_Controller() # Initialize the controller +delay = 0.05 # Delay between each read/write operation + +while True: # Infinite loop + status = joy.read_self() # Read the status of the controller + x = status.LeftJoystickX # Get the X position of the left joystick + y = status.RightJoystickY # Get the Y position of the right joystick + joystickswitch = x > 0 # Check if the joystick is moved to the right + + x_b = struct.pack('f', x) + y_b = struct.pack('f', y) + + data = bytes([0xFA]) + \ + x_b + \ + y_b + \ + bytes([0xFB, 0x00]) # Prepare the data to be sent + # data = [0xFA, int(joystickswitch), int(joystickswitch), 0xFB, 0x00] + + print(len(data)) + print(bytes(data)) + + try: + os.write(i2c_fd, data) # Write the data to the i2c device + # os.write(i2c_fd, bytes(data)) # Write the data to the i2c device + time.sleep(delay) # Wait for a while + print("Sent data to Pico: ", list(data)) # Print the data that was sent + except OSError: + print( + "Remote I/O Error" + ) # Print an error message if there was a problem with the write operation + + # Read data from pico + try: + incoming_data = os.read(i2c_fd, 1) # Read 1 byte from the i2c device + time.sleep(delay) # Wait for a while + print( + "Received data from Pico: ", list(incoming_data) + ) # Print the data that was received + except TimeoutError: + print("Timeout Error") # Print an error message if there was a timeout error diff --git a/dev/i2c_drive/i2c_drive_pico.c b/dev/i2c_drive/i2c_drive_pico.c new file mode 100644 index 0000000..1b89574 --- /dev/null +++ b/dev/i2c_drive/i2c_drive_pico.c @@ -0,0 +1,220 @@ +#include +#include +#include +#include +#include +#include + +// Define constants for I2C communication +#define I2C_PICO_ADDR 0x08 +#define I2C_SDA_PIN 0 +#define I2C_SCL_PIN 1 +#define I2C_PORT i2c0 +#define I2C_BAUDRATE 100 * 1000 + +// Define the length of the data packet +#define I2C_DATA_LENGTH 10 + +#define MESSAGE_START 0xFA +#define MESSAGE_STOP 0xFB + + +// digital low on in# pins indicates direction, both high is no signal +#define turn_in1_pin 4 // 1A, forward direction +#define turn_in2_pin 5 // 1B, backward direction + +// #define motor_pwm_pin 9 // 2A, 2B take up by motor speed +#define turn_pwm_pin 9 // 2A, turn motor speed +#define wheel_pwm_pin 8 // 2B, wheel motor speed +#define pwm_slice 4 +#define turn_channel PWM_CHAN_B +#define wheel_channel PWM_CHAN_A + +#define wheel_in1_pin 6 // 3A, forward direction +#define wheel_in2_pin 7 // 3B, backard direction + +// #define freq 500 // note: use clock management frequencies to set frequency +// #define duty_cycle 1 +#define count_max 65535 + + +// Buffer for incoming data +uint8_t incoming_data[I2C_DATA_LENGTH]; + +// Status of the input data +uint8_t input_status = 0; + +// Last event that occurred +int last_event = 0; + +// Index of the current data byte +int data_index = 0; + +// Buffer for the input data +uint8_t input[I2C_DATA_LENGTH - 2]; + +// Handler for I2C events +static void i2c_handler(i2c_inst_t *i2c, i2c_slave_event_t event) +{ + switch (event) + { + case I2C_SLAVE_RECEIVE:{ + // Read the data + uint8_t tmp = i2c_read_byte_raw(i2c); + // Check if the data is valid + // TODO: probably revert this back to the original, we don't really need the MESSAGE_START stuff + if ((incoming_data[0] == 0x00 && tmp != MESSAGE_START) || data_index >= I2C_DATA_LENGTH) + { + printf("Invalid data %x\n", tmp); + break; + } + // Store the data + incoming_data[data_index] = tmp; + // printf("Data: %d\n", incoming_data[data_index]); + data_index++; + // set the event status to received + last_event = 1; + break; + } + + + case I2C_SLAVE_REQUEST: // Pi is requesting data + // Write the data into the void + i2c_write_byte_raw(i2c, (uint8_t)input_status); + // set the event status to sent + last_event = 2; + break; + + case I2C_SLAVE_FINISH: // Pi has signalled Stop / Restart + // if the last event was a receive event and the data is valid + if (last_event == 1) + if (incoming_data[0] == MESSAGE_START && incoming_data[I2C_DATA_LENGTH - 1] == MESSAGE_STOP) + { + // move the data into the input array + for (int i = 0; i < I2C_DATA_LENGTH - 2; i++) + { + input[i] = (int)incoming_data[i + 1]; + } + // set the input status to ready + input_status = 1; + + // Reset incoming_data + for (int i = 0; i < I2C_DATA_LENGTH; i++) + { + incoming_data[i] = 0x00; + } + } + data_index = 0; + break; + default: + break; + } +} + + +bool read = false; + +float joyX = 0.0f; +float joyY = 0.0f; + +void setup() +{ // setup pins for pwm functions + stdio_init_all(); + gpio_init(turn_in1_pin); + gpio_init(turn_in2_pin); + gpio_init(wheel_in1_pin); + gpio_init(wheel_in2_pin); + + // check if default output signal is 0, for now put this in + gpio_put(turn_in1_pin, 0); + gpio_put(turn_in2_pin, 0); + gpio_put(wheel_in1_pin, 0); + gpio_put(wheel_in2_pin, 0); + + gpio_set_dir(turn_in1_pin, GPIO_OUT); + gpio_set_dir(turn_in2_pin, GPIO_OUT); + gpio_set_dir(wheel_in1_pin, GPIO_OUT); + gpio_set_dir(wheel_in2_pin, GPIO_OUT); + + gpio_set_function(turn_pwm_pin, GPIO_FUNC_PWM); + gpio_set_function(wheel_pwm_pin, GPIO_FUNC_PWM); + pwm_set_wrap(pwm_slice, count_max); + + pwm_set_chan_level(pwm_slice, turn_channel, 0); + pwm_set_chan_level(pwm_slice, wheel_channel, 0); + + pwm_set_enabled(pwm_slice, true); +} + +int main() +{ + + stdio_init_all(); + + // Initialize I2C at 100kHz + i2c_init(I2C_PORT, I2C_BAUDRATE); + gpio_set_function(I2C_SDA_PIN, GPIO_FUNC_I2C); + gpio_set_function(I2C_SCL_PIN, GPIO_FUNC_I2C); + + // Set I2C address for Pico + i2c_slave_init(I2C_PORT, I2C_PICO_ADDR, &i2c_handler); + + setup();//setup for pwm + + while (1) + { + if (input_status ==1){ + uint64_t tmp_float = 0; //use https://www.h-schmidt.net/FloatConverter/IEEE754.html to convert between float and binary/hex + for (int i = 7; i >= 0; i--) //bytes are stored in int8 array (64 bits), pico reads backwards + { + tmp_float |= input[i]; //write byte at a time to tmp_float and shift + if(i!=0) + tmp_float<<=8; //preserves order of bits in 64bit int + } + joyX = *(((float*)&tmp_float)); //convert to interprit bits as float (32 bits) + joyY = *(((float*)&tmp_float)+1); + printf("%f ", joyX); + printf("%f\n", joyY); //printing floats in console + tmp_float = 0; //clear float + } + + if (joyX == 0) + { // in1 and in2 are high + // pwm_set_gpio_level (3, count_max); this method would work, but is iffy, as setting the count value for individual pins update next cycle + gpio_put(turn_in1_pin, 1); + gpio_put(turn_in2_pin, 1); + } + else if (joyX < 0) + { // in1 is high and in2 is low + gpio_put(turn_in1_pin, 1); + gpio_put(turn_in2_pin, 0); + } + else + { // in1 is low and in2 is high + gpio_put(turn_in2_pin, 1); + gpio_put(turn_in1_pin, 0); + } + + // wheel motor + if (joyY == 0) + { // in1 and in2 are high + gpio_put(wheel_in1_pin, 1); + gpio_put(wheel_in2_pin, 1); + } + else if (joyY < 0) + { // in1 is high and in2 is low + gpio_put(wheel_in1_pin, 1); + gpio_put(wheel_in2_pin, 0); + } + else + { // in1 is low and in2 is high + gpio_put(wheel_in1_pin, 0); + gpio_put(wheel_in2_pin, 1); + } + + pwm_set_chan_level(pwm_slice, turn_channel, abs((int)(joyX* count_max))); + pwm_set_chan_level(pwm_slice, wheel_channel, abs((int)(joyY * count_max))); + } + + return 0; +} \ No newline at end of file diff --git a/dev/i2c_drive/pico_sdk_import.cmake b/dev/i2c_drive/pico_sdk_import.cmake new file mode 100644 index 0000000..65f8a6f --- /dev/null +++ b/dev/i2c_drive/pico_sdk_import.cmake @@ -0,0 +1,73 @@ +# This is a copy of /external/pico_sdk_import.cmake + +# This can be dropped into an external project to help locate this SDK +# It should be include()ed prior to project() + +if (DEFINED ENV{PICO_SDK_PATH} AND (NOT PICO_SDK_PATH)) + set(PICO_SDK_PATH $ENV{PICO_SDK_PATH}) + message("Using PICO_SDK_PATH from environment ('${PICO_SDK_PATH}')") +endif () + +if (DEFINED ENV{PICO_SDK_FETCH_FROM_GIT} AND (NOT PICO_SDK_FETCH_FROM_GIT)) + set(PICO_SDK_FETCH_FROM_GIT $ENV{PICO_SDK_FETCH_FROM_GIT}) + message("Using PICO_SDK_FETCH_FROM_GIT from environment ('${PICO_SDK_FETCH_FROM_GIT}')") +endif () + +if (DEFINED ENV{PICO_SDK_FETCH_FROM_GIT_PATH} AND (NOT PICO_SDK_FETCH_FROM_GIT_PATH)) + set(PICO_SDK_FETCH_FROM_GIT_PATH $ENV{PICO_SDK_FETCH_FROM_GIT_PATH}) + message("Using PICO_SDK_FETCH_FROM_GIT_PATH from environment ('${PICO_SDK_FETCH_FROM_GIT_PATH}')") +endif () + +set(PICO_SDK_PATH "${PICO_SDK_PATH}" CACHE PATH "Path to the Raspberry Pi Pico SDK") +set(PICO_SDK_FETCH_FROM_GIT "${PICO_SDK_FETCH_FROM_GIT}" CACHE BOOL "Set to ON to fetch copy of SDK from git if not otherwise locatable") +set(PICO_SDK_FETCH_FROM_GIT_PATH "${PICO_SDK_FETCH_FROM_GIT_PATH}" CACHE FILEPATH "location to download SDK") + +if (NOT PICO_SDK_PATH) + if (PICO_SDK_FETCH_FROM_GIT) + include(FetchContent) + set(FETCHCONTENT_BASE_DIR_SAVE ${FETCHCONTENT_BASE_DIR}) + if (PICO_SDK_FETCH_FROM_GIT_PATH) + get_filename_component(FETCHCONTENT_BASE_DIR "${PICO_SDK_FETCH_FROM_GIT_PATH}" REALPATH BASE_DIR "${CMAKE_SOURCE_DIR}") + endif () + # GIT_SUBMODULES_RECURSE was added in 3.17 + if (${CMAKE_VERSION} VERSION_GREATER_EQUAL "3.17.0") + FetchContent_Declare( + pico_sdk + GIT_REPOSITORY https://github.com/raspberrypi/pico-sdk + GIT_TAG master + GIT_SUBMODULES_RECURSE FALSE + ) + else () + FetchContent_Declare( + pico_sdk + GIT_REPOSITORY https://github.com/raspberrypi/pico-sdk + GIT_TAG master + ) + endif () + + if (NOT pico_sdk) + message("Downloading Raspberry Pi Pico SDK") + FetchContent_Populate(pico_sdk) + set(PICO_SDK_PATH ${pico_sdk_SOURCE_DIR}) + endif () + set(FETCHCONTENT_BASE_DIR ${FETCHCONTENT_BASE_DIR_SAVE}) + else () + message(FATAL_ERROR + "SDK location was not specified. Please set PICO_SDK_PATH or set PICO_SDK_FETCH_FROM_GIT to on to fetch from git." + ) + endif () +endif () + +get_filename_component(PICO_SDK_PATH "${PICO_SDK_PATH}" REALPATH BASE_DIR "${CMAKE_BINARY_DIR}") +if (NOT EXISTS ${PICO_SDK_PATH}) + message(FATAL_ERROR "Directory '${PICO_SDK_PATH}' not found") +endif () + +set(PICO_SDK_INIT_CMAKE_FILE ${PICO_SDK_PATH}/pico_sdk_init.cmake) +if (NOT EXISTS ${PICO_SDK_INIT_CMAKE_FILE}) + message(FATAL_ERROR "Directory '${PICO_SDK_PATH}' does not appear to contain the Raspberry Pi Pico SDK") +endif () + +set(PICO_SDK_PATH ${PICO_SDK_PATH} CACHE PATH "Path to the Raspberry Pi Pico SDK" FORCE) + +include(${PICO_SDK_INIT_CMAKE_FILE}) diff --git a/dev/i2c_drive/procon.py b/dev/i2c_drive/procon.py new file mode 100644 index 0000000..8e04765 --- /dev/null +++ b/dev/i2c_drive/procon.py @@ -0,0 +1,331 @@ +#!/usr/bin/env python3 + +import math +import time + +import hid # pip install hidapi + + +def to_int16(uint16): + return -((uint16 ^ 0xFFFF) + 1) if uint16 & 0x8000 else uint16 + + +class ProCon: + VENDOR_ID = 0x057E + PRODUCT_ID = 0x2009 + PACKET_SIZE = 64 + CALIBRATION_OFFSET = 0x603D + CALIBRATION_LENGTH = 0x12 + COMMAND_RETRIES = 10 + RUMBLE_NEUTRAL = (0x00, 0x01, 0x40, 0x40) + RUMBLE = (0x74, 0xBE, 0xBD, 0x6F) + DEFAULT_IMU_SENSITIVITY = (0x03, 0x00, 0x00, 0x01) + + class OutputReportID: + RUMBLE_SUBCOMMAND = 0x01 + RUMBLE = 0x10 + COMMAND = 0x80 + + class InputReportID: + SUBCOMMAND_REPLY = 0x21 + CONTROLLER_STATE = 0x30 + COMMAND_ACK = 0x81 + + class CommandID: + HANDSHAKE = 0x02 + HIGH_SPEED = 0x03 + FORCE_USB = 0x04 + + class SubcommandID: + SET_INPUT_REPORT_MODE = 0x03 + SPI_FLASH_READ = 0x10 + SET_PLAYER_LIGHTS = 0x30 + SET_HOME_LIGHT = 0x38 + ENABLE_IMU = 0x40 + SET_IMU_SENSITIVITY = 0x41 + ENABLE_VIBRATION = 0x48 + + class Button: + A = "A" + B = "B" + X = "X" + Y = "Y" + UP = "Up" + DOWN = "Down" + LEFT = "Left" + RIGHT = "Right" + MINUS = "-" + PLUS = "+" + SCREENSHOT = "Screenshot" + HOME = "Home" + L = "L" + ZL = "ZL" + R = "R" + ZR = "ZR" + LS = "LS" + RS = "RS" + + def __init__(self): + self.subcommand_counter = 0 + self.dev = hid.device() + self.dev.open(ProCon.VENDOR_ID, ProCon.PRODUCT_ID) + self.handshake() + self.high_speed() + self.handshake() + self.rumble_low = self.rumble_high = ProCon.RUMBLE_NEUTRAL + self.rumble_expire = 0 + self.load_stick_calibration() + self.enable_vibration(True) + self.set_input_report_mode(ProCon.InputReportID.CONTROLLER_STATE) + self.force_usb() + self.set_player_lights(True, False, False, False) + self.enable_imu(True) + self.set_imu_sensitivity(ProCon.DEFAULT_IMU_SENSITIVITY) + + def start(self, callback): + while True: + state = self.recv() + if state[0] != ProCon.InputReportID.CONTROLLER_STATE: + continue + buttons = { + ProCon.Button.A: state[3] & 0x08 > 0, + ProCon.Button.B: state[3] & 0x04 > 0, + ProCon.Button.X: state[3] & 0x02 > 0, + ProCon.Button.Y: state[3] & 0x01 > 0, + ProCon.Button.UP: state[5] & 0x02 > 0, + ProCon.Button.DOWN: state[5] & 0x01 > 0, + ProCon.Button.LEFT: state[5] & 0x08 > 0, + ProCon.Button.RIGHT: state[5] & 0x04 > 0, + ProCon.Button.MINUS: state[4] & 0x01 > 0, + ProCon.Button.PLUS: state[4] & 0x02 > 0, + ProCon.Button.SCREENSHOT: state[4] & 0x20 > 0, + ProCon.Button.HOME: state[4] & 0x10 > 0, + ProCon.Button.L: state[5] & 0x40 > 0, + ProCon.Button.ZL: state[5] & 0x80 > 0, + ProCon.Button.R: state[3] & 0x40 > 0, + ProCon.Button.ZR: state[3] & 0x80 > 0, + ProCon.Button.LS: state[4] & 0x08 > 0, + ProCon.Button.RS: state[4] & 0x04 > 0, + } + l_x = state[6] | ((state[7] & 0xF) << 8) + l_y = (state[7] >> 4) | (state[8] << 4) + r_x = state[9] | ((state[10] & 0xF) << 8) + r_y = (state[10] >> 4) | (state[11] << 4) + l_x = self.apply_stick_calibration(l_x, 0, 0) + l_y = self.apply_stick_calibration(l_y, 0, 1) + r_x = self.apply_stick_calibration(r_x, 1, 0) + r_y = self.apply_stick_calibration(r_y, 1, 1) + l_stick = (l_x, l_y) + r_stick = (r_x, r_y) + accel = ( + state[13] | state[14] << 8, + state[15] | state[16] << 8, + state[17] | state[18] << 8, + ) + gyro = ( + state[19] | state[20] << 8, + state[21] | state[22] << 8, + state[23] | state[24] << 8, + ) + accel = tuple(map(to_int16, accel)) + gyro = tuple(map(to_int16, gyro)) + battery = (state[2] & 0xF0) >> 4 + callback(buttons, l_stick, r_stick, accel, gyro, battery) + if self.rumble_expire and int(time.time() * 1000) >= self.rumble_expire: + self.send_rumble(False, False, 0) + + def load_stick_calibration(self): + ok, reply = self.spi_flash_read( + ProCon.CALIBRATION_OFFSET, ProCon.CALIBRATION_LENGTH + ) + if not ok: + raise RuntimeError("cannot load stick calibration") + self.stick_calibration = [ + [ + [ + ((reply[27] & 0xF) << 8) | reply[26], + ((reply[24] & 0xF) << 8) | reply[23], + ((reply[21] & 0xF) << 8) | reply[20], + ], + [ + (reply[28] << 4) | (reply[27] >> 4), + (reply[25] << 4) | (reply[24] >> 4), + (reply[22] << 4) | (reply[21] >> 4), + ], + ], + [ + [ + ((reply[33] & 0xF) << 8) | reply[32], + ((reply[30] & 0xF) << 8) | reply[29], + ((reply[36] & 0xF) << 8) | reply[35], + ], + [ + (reply[34] << 4) | (reply[33] >> 4), + (reply[31] << 4) | (reply[30] >> 4), + (reply[37] << 4) | (reply[36] >> 4), + ], + ], + ] + for i in range(len(self.stick_calibration)): + for j in range(len(self.stick_calibration[i])): + for k in range(len(self.stick_calibration[i][j])): + if self.stick_calibration[i][j][k] == 0xFFF: + self.stick_calibration[i][j][k] = 0 + self.stick_extends = [ + [ + [ + -int(self.stick_calibration[0][0][0] * 0.7), + int(self.stick_calibration[0][0][2] * 0.7), + ], + [ + -int(self.stick_calibration[0][1][0] * 0.7), + int(self.stick_calibration[0][1][2] * 0.7), + ], + ], + [ + [ + -int(self.stick_calibration[1][0][0] * 0.7), + int(self.stick_calibration[1][0][2] * 0.7), + ], + [ + -int(self.stick_calibration[1][1][0] * 0.7), + int(self.stick_calibration[1][1][2] * 0.7), + ], + ], + ] + + def apply_stick_calibration(self, value, stick, axis): + value -= self.stick_calibration[stick][axis][1] + if value < self.stick_extends[stick][axis][0]: + self.stick_extends[stick][axis][0] = value + if value > self.stick_extends[stick][axis][1]: + self.stick_extends[stick][axis][1] = value + if value > 0: + return int(value * 0x7FFF / self.stick_extends[stick][axis][1]) + return int(value * -0x7FFF / self.stick_extends[stick][axis][0]) + + def send(self, data): + return self.dev.write(data) == len(data) + + def recv(self): + return self.dev.read(ProCon.PACKET_SIZE) + + def send_command(self, id, wait_for_reply=True): + data = (ProCon.OutputReportID.COMMAND, id) + for _ in range(ProCon.COMMAND_RETRIES): + if not self.send(data): + continue + if not wait_for_reply: + return True + reply = self.recv() + if reply[0] == ProCon.InputReportID.COMMAND_ACK and reply[1] == id: + return True + return False + + def send_subcommand(self, id, param, wait_for_reply=True): + data = ( + (ProCon.OutputReportID.RUMBLE_SUBCOMMAND, self.subcommand_counter) + + self.rumble_low + + self.rumble_high + + (id,) + + param + ) + self.subcommand_counter = (self.subcommand_counter + 1) & 0xFF + for _ in range(ProCon.COMMAND_RETRIES): + if not self.send(data): + continue + if not wait_for_reply: + return True, [] + reply = self.recv() + if reply[0] == ProCon.InputReportID.SUBCOMMAND_REPLY and reply[14] == id: + return True, reply + return False, [] + + def send_rumble(self, low, high, duration): + self.rumble_low = ProCon.RUMBLE if low else ProCon.RUMBLE_NEUTRAL + self.rumble_high = ProCon.RUMBLE if high else ProCon.RUMBLE_NEUTRAL + self.rumble_expire = ( + int(time.time() * 1000) + duration if (low or high) and duration else 0 + ) + data = ( + (ProCon.OutputReportID.RUMBLE, self.subcommand_counter) + + self.rumble_low + + self.rumble_high + ) + self.subcommand_counter = (self.subcommand_counter + 1) & 0xFF + for _ in range(ProCon.COMMAND_RETRIES): + if self.send(data): + return True + return False + + def handshake(self): + return self.send_command(ProCon.CommandID.HANDSHAKE) + + def high_speed(self): + return self.send_command(ProCon.CommandID.HIGH_SPEED) + + def force_usb(self): + return self.send_command(ProCon.CommandID.FORCE_USB, False) + + def set_input_report_mode(self, mode): + return self.send_subcommand(ProCon.SubcommandID.SET_INPUT_REPORT_MODE, (mode,)) + + def spi_flash_read(self, addr, l): + param = ( + addr & 0x000000FF, + (addr & 0x0000FF00) >> 8, + (addr & 0x00FF0000) >> 16, + (addr & 0xFF000000) >> 24, + l, + ) + return self.send_subcommand(ProCon.SubcommandID.SPI_FLASH_READ, param) + + def set_player_lights(self, one, two, three, four): + param = (one << 0) | (two << 1) | (three << 2) | (four << 3) + return self.send_subcommand(ProCon.SubcommandID.SET_PLAYER_LIGHTS, (param,)) + + def set_home_light(self, brightness): + intensity = 0 + if brightness > 0: + if brightness < 65: + intensity = (brightness + 5) // 10 + else: + intensity = math.ceil(0xF * ((brightness / 100) ** 2.13)) + intensity = (intensity & 0xF) << 4 + param = (0x01, intensity, intensity, 0x00) + return self.send_subcommand(ProCon.SubcommandID.SET_HOME_LIGHT, param) + + def enable_imu(self, enable): + return self.send_subcommand(ProCon.SubcommandID.ENABLE_IMU, (int(enable),)) + + def set_imu_sensitivity(self, sensitivity): + return self.send_subcommand( + ProCon.SubcommandID.SET_IMU_SENSITIVITY, sensitivity + ) + + def enable_vibration(self, enable): + return self.send_subcommand( + ProCon.SubcommandID.ENABLE_VIBRATION, (int(enable),) + ) + + +def print_state(buttons, l_stick, r_stick, accel, gyro, battery): + print("\33[2JButtons:") + for k, v in buttons.items(): + if v: + print("[{}]".format(k), end=" ") + else: + print(" {} ".format(k), end=" ") + print() + print("L Stick: ({:6}, {:6})".format(l_stick[0], l_stick[1])) + print("R Stick: ({:6}, {:6})".format(r_stick[0], r_stick[1])) + print("Accelerometer: ({:6}, {:6}, {:6})".format(accel[0], accel[1], accel[2])) + print("Gyroscope: ({:6}, {:6}, {:6})".format(gyro[0], gyro[1], gyro[2])) + print("Battery: {}/9".format(battery)) + + +if __name__ == "__main__": + try: + ProCon().start(print_state) + except KeyboardInterrupt: + print("\rGoodbye!") diff --git a/dev/messaging-library/compute_module_id.py b/dev/messaging-library/compute_module_id.py new file mode 100644 index 0000000..104db85 --- /dev/null +++ b/dev/messaging-library/compute_module_id.py @@ -0,0 +1,28 @@ +"""Defined list of valid message ids and helper function to +check and process messages into actions""" + +from enum import Enum + + +class MessageProcessResult (Enum): + """state of robot after message has been fully processed""" + PAYLOAD_ID_NOT_FOUND = 0 + + +result_array = {} + + +def messaging_process_payload(payload, id) -> MessageProcessResult: + """ + process_payload: reads parameter of specified length + and performs actions based on the id + + @param payload string + @param id id of payload + + @returns success/failure to process payload, + next state of main state machine + """ + print(payload) + print(id) + return MessageProcessResult.PAYLOAD_ID_NOT_FOUND diff --git a/dev/messaging-library/messaging-library.h b/dev/messaging-library/messaging-library.h new file mode 100644 index 0000000..0cc3e05 --- /dev/null +++ b/dev/messaging-library/messaging-library.h @@ -0,0 +1,178 @@ +#include "swerve-module-id.h" + +#define PAYLOAD_MAX_LEN 127 +#define HEAD_ID 0xCC +#define TAIL_ID 0xB9 + +enum messageParseResult { + MESSAGE_BUFFER_OVERFLOW = -2, + MESSAGE_PARSE_FAILURE, + MESSAGE_PARSE_NONE, + MESSAGE_PARSE_SUCCESS +}; + +enum messageReadState { + MESSAGE_ERROR = -1, + MESSAGE_HEAD, + MESSAGE_LENGTH, + MESSAGE_PAYLOAD, + MESSAGE_TAIL, + MESSAGE_CHECKSUM, + MESSAGE_END +}; + +/** + * BSDChecksum: Calculates checksum for messaging, using BSDChecksum algorithm. The max length is 128 bytes. The first byte is the message ID, the next 127 bytes is the payload + * + * @param payloadString string of payload and id, 128 bytes max + * @param payloadLen length of payload with id + * + * @return calculated checksum + */ +char MessagingBSDChecksum(char* payloadString, unsigned char payloadLen) { + unsigned char checksum = 0; + for(short i = 0; i < payloadLen; i++) { + checksum = (checksum >> 1) | (checksum << 7); + checksum += payloadString[i]; + } + return checksum; +} + +/** + * ReadBuffer: Uses state machine to take the buffer parameter and processes the message into the result string; + * Returns true/false based on if conversion was successful. Changes value of robotState if message is valid. + * + * \nPacket Structure: + * HEAD LENGTH PAYLOAD TAIL CHECKSUM END + * 1 1 (1)<128 1 1 \r\n + * + * Head: 1 byte 0xCC + * Length: 1 byte, length of payload in bytes + * Payload: Variable length < 128 bytes, always proceeded by a 1 byte ID + * Tail: 1 byte 0xB9 + * Checksum: 1 byte checksum calculated over payload using BSDChecksum algorithm + * End: 2 bytes \r\n + * + * @param bufferString string that will be processed + * @param resultString string that the result is stored in + * @param bufferLength length of buffer function parses through + * @param robotState pointer to variable containing state of robot that is modified + * when a message is fully processed + * + * @return true or false based on success of function + */ +char MessagingReadBuffer(char* bufferString, char* resultString, int bufferLength, char* robotState) { // turn result to struct later + short idx = 0; + char state = MESSAGE_HEAD; + char currentChar = bufferString[idx]; + char payloadLen = 0; + char payloadIdx = 0; + char messageID = 0; + char calculatedChecksum = 0; + char messageParseResult = MESSAGE_PARSE_NONE; + char robotResultState = 0; + + while (idx < bufferLength) { + if(state == MESSAGE_HEAD) { + if(currentChar == (char)HEAD_ID) { + state = MESSAGE_LENGTH; + } + }else if(state == MESSAGE_LENGTH) { + payloadLen = currentChar; + + // expected payload range check + if(payloadLen < 2) { + state = MESSAGE_ERROR; + } + + payloadIdx = 0; + char* payload = bufferString + idx + 1; + calculatedChecksum = MessagingBSDChecksum(payload, payloadLen); + state = MESSAGE_PAYLOAD; + + }else if(state == MESSAGE_PAYLOAD) { + + // assign/store id + if(payloadIdx == 0) { + messageID = currentChar; + } + + // actual payload length check + if(payloadIdx < payloadLen - 1) { + if(currentChar == (char)TAIL_ID) { + state = MESSAGE_ERROR; + }else { + payloadIdx++; + } + }else { + if(currentChar == (char)TAIL_ID) { + state = MESSAGE_ERROR; + }else { + robotResultState = MessagingProcessPayload(bufferString + idx - payloadLen + 2, messageID, payloadLen - 1); + state = MESSAGE_TAIL; + } + } + + }else if(state == MESSAGE_TAIL) { + if(currentChar == (char)TAIL_ID) { + state = MESSAGE_CHECKSUM; + }else{ + state = MESSAGE_ERROR; + } + + }else if(state == MESSAGE_CHECKSUM) { + if(calculatedChecksum != currentChar) { + state = MESSAGE_ERROR; + }else { + state = MESSAGE_END; + } + + }else if(state == MESSAGE_END){ + char nextChar = bufferString[idx + 1]; + if(currentChar == '\r' && nextChar == '\n') { + messageParseResult = MESSAGE_PARSE_SUCCESS; + state = MESSAGE_HEAD; + *robotState = robotResultState; + break; + }else { + state = MESSAGE_ERROR; + } + + }else { + messageParseResult = MESSAGE_PARSE_FAILURE; + break; + } + + idx++; + currentChar = bufferString[idx]; + } + + if(state != MESSAGE_HEAD && state != MESSAGE_ERROR) { + messageParseResult = MESSAGE_BUFFER_OVERFLOW; + } + + return messageParseResult; +} + +/** + * WriteMessage: converts parameter payload into a valid message string and writes it to parameter message. + * Payload corresponds to PAYLOAD in the packet structure: [HEAD, LENGTH, PAYLOAD, TAIL, CHECKSUM, END] + * + * @param payload string that will be converted to a valid message format + * @param message string that the message will be written to + * + * @return None + */ +void MessagingWriteMessage(unsigned char payload[], char message[]){ + char payloadLen = 0; + for(;payload[payloadLen] != '\0'; payloadLen++){ + message[payloadLen + 2] = payload[payloadLen]; + } + message[0] = (char)HEAD_ID; + message[1] = payloadLen; + message[payloadLen + 2] = (char)TAIL_ID; + message[payloadLen + 3] = MessagingBSDChecksum(payload, payloadLen); + message[payloadLen + 4] = '\r'; + message[payloadLen + 5] = '\n'; + message[payloadLen + 6] = '\0'; +} diff --git a/dev/messaging-library/messaging-test.c b/dev/messaging-library/messaging-test.c new file mode 100644 index 0000000..f6cd6a9 --- /dev/null +++ b/dev/messaging-library/messaging-test.c @@ -0,0 +1,174 @@ +#include +#include + +#include "messaging-library.h" + +#define MIN 0x00 +#define MAX 0x7F + +int main() +{ + // printf("\nTesting BSDChecksum\n"); + // char payload[] = {0x80,0x40,0x13,0x33,0x33,0x40,0x13,0x33,0x33,0x40,0x13,0x33,0x33,0x40,0x13,0x33,0x33,0x40,0x13,0x33,0x33,0x40,0x13,0x33,0x33}; + // char c = MessagingBSDChecksum(payload, 25); + // printf("%i\n", c); + char *r = ""; + char state = 0; + // // correct message + // printf("Testing correct message\n"); + // char mes[] = {0xCC, 0x02, 0x01, 0x01, 0xB9, 0x81, 0x0D, 0x0A}; + // printf("%i %s %i\n", MessagingReadBuffer(mes, r, 8, &state), r, state); + + // // incorrect given payload length + // printf("Testing too short given payload length\n"); + // char mes20[] = {0xCC, MIN, 0x01, 0x01, 0xB9, 0x81, 0x0D, 0x0A}; + // printf("%i %s %i\n", MessagingReadBuffer(mes20, r, 8, &state), r, state); + + // printf("Testing too long given payload length\n"); + // char mes2[] = {0xCC, MAX, 0x01, 0x01, 0xB9, 0x81, 0x0D, 0x0A}; + // printf("%i %s %i\n", MessagingReadBuffer(mes2, r, 8, &state), r, state); + + // printf("Testing too short actual payload length\n"); + // char mes3[] = {0xCC, 0x02, 0x01, 0xB9, 0x01, 0x0D, 0x0A}; + // printf("%i %s %i\n", MessagingReadBuffer(mes3, r, 8, &state), r, state); + + // printf("Testing too long actual payload length\n"); + // char mes4[] = {0xCC, 0x02, 0x01, 0x01, 0x01, 0xB9, 0xC1, 0x0D, 0x0A}; + // printf("%i %s %i\n", MessagingReadBuffer(mes4, r, 8, &state), r, state); + + // printf("Testing limit payload length\n"); + // char mes5[] = {0xCC, 0x7F, + // 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, + // 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, + // 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, + // 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, + // 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, + // 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, + // 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, + // 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, + // 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, + // 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, + // 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, + // 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, + // 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, + // 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, + // 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, + // 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, + // 0xB9, 0x01, 0x0D, 0x0A}; + // printf("%i %s %i\n", MessagingReadBuffer(mes5, r, 133, &state), r, state); + + // printf("Testing exceed limit payload length\n"); + // char mes14[] = {0xCC, 0x80, + // 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, + // 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, + // 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, + // 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, + // 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, + // 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, + // 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, + // 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, + // 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, + // 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, + // 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, + // 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, + // 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, + // 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, + // 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, + // 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, + // 0xB9, 0x81, 0x0D, 0x0A}; + // printf("%i %s %i\n", MessagingReadBuffer(mes14, r, 133, &state), r, state); + + // // incorrect checksum + // printf("Testing incorrect checksum\n"); + // char mes6[] = {0xCC, 0x02, 0x01, 0x01, 0xB9, MIN, 0x0D, 0x0A}; + // printf("%i %s %i\n", MessagingReadBuffer(mes6, r, 8, &state), r, state); + + // // fixed character test (HEAD, TAIL, DELIMITERS) + // printf("Testing incorrect head\n"); + // char mes7[] = {MIN, 0x02, 0x01, 0x01, 0xB9, 0x81, 0x0D, 0x0A}; + // printf("%i %s %i\n", MessagingReadBuffer(mes7, r, 8, &state), r, state); + + // printf("Testing incorrect tail\n"); + // char mes8[] = {0xCC, 0x02, 0x01, 0x01, MIN, 0x81, 0x0D, 0x0A}; + // printf("%i %s %i\n", MessagingReadBuffer(mes8, r, 8, &state), r, state); + + // printf("Testing incorrect delimiter 1\n"); + // char mes9[] = {0xCC, 0x02, 0x01, 0x01, 0xB9, 0x81, MIN, 0x0A}; + // printf("%i %s %i\n", MessagingReadBuffer(mes9, r, 8, &state), r, state); + + // printf("Testing incorrect delimiter 2\n"); + // char mes10[] = {0xCC, 0x02, 0x01, 0x01, 0xB9, 0x81, 0x0D, MIN}; + // printf("%i %s %i\n", MessagingReadBuffer(mes10, r, 8, &state), r, state); + + // // message structure test + // printf("Testing incorrect structure\n"); + // char mes11[] = {0xCC, MIN, 0x02, 0x01, 0x01, 0xB9, 0x81, 0x0D, 0x0A}; + // printf("%i %s %i\n", MessagingReadBuffer(mes11, r, 8, &state), r, state); + + // char mes12[] = {0xCC, 0x02, MIN, 0x01, 0x01, 0xB9, 0x81, 0x0D, 0x0A}; + // printf("%i %s %i\n", MessagingReadBuffer(mes12, r, 8, &state), r, state); + + // char mes13[] = {0xCC, 0x02, 0x01, 0x01, MIN, 0xB9, 0x81, 0x0D, 0x0A}; + // printf("%i %s %i\n", MessagingReadBuffer(mes13, r, 8, &state), r, state); + + // char mes15[] = {0xCC, 0x02, 0x01, 0x01, 0xB9, MIN, 0x81, 0x0D, 0x0A}; + // printf("%i %s %i\n", MessagingReadBuffer(mes15, r, 8, &state), r, state); + + // char mes16[] = {0xCC, 0x02, 0x01, 0x01, 0xB9, 0x81, MIN, 0x0D, 0x0A}; + // printf("%i %s %i\n", MessagingReadBuffer(mes16, r, 8, &state), r, state); + + // char mes17[] = {0xCC, 0x02, 0x01, 0x01, 0xB9, 0x81, 0x0D, MIN, 0x0A}; + // printf("%i %s %i\n", MessagingReadBuffer(mes17, r, 8, &state), r, state); + + // printf("Testing characters before/after message\n"); + // char mes18[] = {MIN, 0xCC, 0x02, 0x01, 0x01, 0xB9, 0x81, 0x0D, 0x0A}; + // printf("%i %s %i\n", MessagingReadBuffer(mes18, r, 8, &state), r, state); + + // char mes19[] = {0xCC, 0x02, 0x01, 0x01, 0xB9, 0x81, 0x0D, 0x0A, MIN}; + // printf("%i %s %i\n", MessagingReadBuffer(mes19, r, 8, &state), r, state); + + // printf("Testing buffer overflow\n"); + // char mes22[] = {0xCC, 0x02, 0x01, 0x01, 0xB9, 0x81, 0x0D, 0x0A}; + // printf("%i %s %i\n", MessagingReadBuffer(mes22, r, 2, &state), r, state); + + // printf("Testing WriteMessage\n"); + // // put c encoded message here + // char mes23[] = "TEST_TEST_TEST,1,2,34545edrdfgCCV_+{}|\\\\\\\'\b\b67879$^&*^#$54rtyfghvnghn65876878nyghvgnh658766$--+P]"; + // char messLen = strlen(mes23); + // char message[100] = {'\0'}; + // MessagingWriteMessage(mes23,message); + // printf("C output: "); + // for(char i = 0; i < (messLen + 7); i ++) { + // if(i < 2 || i > messLen + 1) { + // printf("0x%02hhX ", message[i]); + // }else { + // printf("%c", message[i]); + // if(i == messLen + 1) { + // printf(" "); + // } + // } + // } + // printf("\n\n"); + + // printf("Python Version: "); + // for(char i = 0; i < (messLen + 7); i ++) { + // printf("\\x%02hhX", message[i]); + // } + // printf("\n\n"); + + // put python test harness output here + char message2[] = {0xcc,0x19,0x80, + 0x40,0x13,0x33,0x33, + 0x40,0x13,0x33,0x33, + 0x40,0x13,0x33,0x33, + 0x40,0x13,0x33,0x33, + 0x40,0x13,0x33,0x33, + 0x40,0x13,0x33,0x33, + 0xb9,0x57,0xd,0xa}; + printf("Testing encode/decode between libraries \n"); + printf("%i ", MessagingReadBuffer(message2, r, 31, &state)); + printf("%i\n",state); + for(int i=0;i<6;i++) + printf(" %f",resultArray[i]); + return 0; +} \ No newline at end of file diff --git a/dev/messaging-library/messaging_library.py b/dev/messaging-library/messaging_library.py new file mode 100644 index 0000000..b1a23db --- /dev/null +++ b/dev/messaging-library/messaging_library.py @@ -0,0 +1,199 @@ +""" +messaging_library encodes and decodes messages that are sent over i2c between +raspberry pis and raspberry picos for modbot +""" + +from enum import Enum +import compute_module_id + +HEAD = 0xCC +TAIL = 0xB9 + +# using pep 8 and flake 8 linter for code styling +# DOES NOT have buffer overflow error due to how python handles strings + + +class MessageReadState (Enum): + """enums that describe the state of the state machine + in messaging_readbuffer""" + MESSAGE_ERROR = -1 + MESSAGE_HEAD = 0 + MESSAGE_LENGTH = 1 + MESSAGE_PAYLOAD = 2 + MESSAGE_TAIL = 3 + MESSAGE_CHECKSUM = 4 + MESSAGE_END = 5 + + +class MessageParseResult (Enum): + """enums that describe the success state of messaging_readbuffer""" + MESSAGE_PARSE_FAILURE = -1 + MESSAGE_PARSE_NONE = 0 + MESSAGE_PARSE_SUCCESS = 1 + + +def messaging_bsd_checksum(payload_string: bytes) -> int: + """ + + messaging_checksum: Calculates checksum for messaging, + using BSDChecksum algorithm. The max length is 128 bytes. + The first byte is the message ID, the next 127 bytes is the payload + + Args: + payloadString (bytes): string of payload and id, 128 bytes max + + Returns: + int: calculated checksum + + """ + checksum = 0 + for idx in payload_string: + checksum = (checksum >> 1) | (checksum << 7) + # 8-bit rotation + checksum = checksum & 0b11111111 + # mask int to only include lower 8 bits + checksum = int(bin(idx), 2) + int(bin(checksum), 2) + # add values bitwise + checksum = checksum & 0b11111111 + # mask int to only include lower 8 bits + return checksum + + +def messaging_readbuffer(buffer_string: bytes, result_string: bytes, + robot_state: Enum) -> bool: + """ + + messaging_readbuffer: Uses state machine to take the buffer parameter + and processes the message into the result string; + Returns true/false based on if conversion was successful. + + Packet Structure: + HEAD LENGTH PAYLOAD TAIL CHECKSUM END + 1 1 (1)<128 1 1 \r\n + + Head: 1 byte 0xCC + Length: 1 byte, length of payload in bytes + Payload: Variable length < 128 bytes, always proceeded by a 1 byte ID + Tail: 1 byte 0xB9 + Checksum: 1 byte checksum calculated over payload + using BSDChecksum algorithm + End: 2 bytes \r\n + + Args: + buffer_string (bytes): string that will be processed + result_string (bytes): string that the result is stored in + robot_state (enum): variable containing state of robot that is modified + when a message is fully processed + + Returns: + enum: true false or none based on success of function + or if buffer contains no message + + DOES NOT HAVE BUFFER OVERFLOW ERROR + """ + state = MessageReadState.MESSAGE_HEAD + payload_idx = 0 + payload_len = 0 + calculated_checksum = 0 + message_id = 0 + message_parse_result = MessageParseResult.MESSAGE_PARSE_NONE + robot_result_state = 0 + + # note: since I am writing both the library in c and python, + # I wanted to make the logic the same between both, so it could be easier + # to update/change the whole library in the future + for idx, current_char in enumerate(buffer_string): + if state == MessageReadState.MESSAGE_HEAD: + if current_char == HEAD: + state = MessageReadState.MESSAGE_LENGTH + elif state == MessageReadState.MESSAGE_LENGTH: + payload_len = int(current_char) + + # expected payload range check + if payload_len < 2: + state = MessageReadState.MESSAGE_ERROR + + payload_idx = 0 + payload_string = buffer_string[(idx + 1):(idx + 1 + payload_len)] + calculated_checksum = messaging_bsd_checksum(payload_string) + # calculated_checksum = hex(calculated_checksum).encode() + state = MessageReadState.MESSAGE_PAYLOAD + + elif state == MessageReadState.MESSAGE_PAYLOAD: + + # assign/store id + if payload_idx == 0: + message_id = current_char + + if result_string: + pass + # actual payload range check + # (this is implemented slightly differently + # as python for loops work differently) + if payload_idx < payload_len - 1: + if current_char == TAIL: + state = MessageReadState.MESSAGE_ERROR + else: + payload_idx += 1 + else: + if current_char == TAIL: + state = MessageReadState.MESSAGE_ERROR + else: + robot_result_state = compute_module_id.\ + messaging_process_payload(payload_string[1:], + message_id) + state = MessageReadState.MESSAGE_TAIL + + elif state == MessageReadState.MESSAGE_TAIL: + if current_char == TAIL: + state = MessageReadState.MESSAGE_CHECKSUM + else: + state = MessageReadState.MESSAGE_ERROR + + elif state == MessageReadState.MESSAGE_CHECKSUM: + if calculated_checksum != current_char: + state = MessageReadState.MESSAGE_ERROR + else: + state = MessageReadState.MESSAGE_END + + elif state == MessageReadState.MESSAGE_END: + next_char = buffer_string[idx + 1] + if current_char == 0x0D and next_char == 0x0A: + message_parse_result = MessageParseResult.MESSAGE_PARSE_SUCCESS + state = MessageReadState.MESSAGE_HEAD + robot_state = robot_result_state + print(str(robot_state)[0:0]) # silence error + break + else: + state = MessageReadState.MESSAGE_ERROR + + else: + message_parse_result = MessageParseResult.MESSAGE_PARSE_FAILURE + break + + return message_parse_result + + +def messaging_write_message(payload: bytes) -> bytes: + """ + + WriteMessage: + converts parameter payload into a valid message + string and writes to parameter message. + Payload corresponds to PAYLOAD in the packet structure: + [HEAD, LENGTH, PAYLOAD, TAIL, CHECKSUM, END] + + Args: + payload (str): string that will be converted to a valid message format + + Returns: + message (str): string that the message will be written to + """ + payload_len = len(payload) + payload_checksum = messaging_bsd_checksum(payload) + converted_payload = [] + for char in payload: + converted_payload.append(char) + bytes_return = bytes([HEAD, payload_len] + converted_payload + + [TAIL, payload_checksum, 13, 10]) + return bytes_return diff --git a/dev/messaging-library/messaging_test.py b/dev/messaging-library/messaging_test.py new file mode 100644 index 0000000..45b2c5f --- /dev/null +++ b/dev/messaging-library/messaging_test.py @@ -0,0 +1,168 @@ +"""testing file for messaging library in python""" +import messaging_library + +MIN = b"\x00" +MAX = b"\x7F" + +# print("Testing bsd checksum") +# PAYLOAD = b"\x80\x40\x13\x33\x33\x40\x13\x33\x33\x40\x13\x33\x33\x40\x13"+\ +# b"\x33\x33\x40\x13\x33\x33\x40\x13\x33\x33" +# c = messaging_library.messaging_bsd_checksum(PAYLOAD) +# print(c) +R = "" +robot_state = 0 +# # correct message +# print("Testing correct message") +# MES = b"\xCC\x02\x01\x01\xB9\x81\x0D\x0A" +# print(messaging_library.messaging_readbuffer(MES, R, robot_state)) +# print(R) + +# # incorrect given payload length +# print("Testing too short given payload length") +# MES = b"\xCC\x02" + MIN + b"\x01\xB9\x81\x0D\x0A" +# print(messaging_library.messaging_readbuffer(MES, R, robot_state)) +# print(R) + +# print("Testing too long given payload length") +# MES = b"\xCC\x02" + MAX + b"\x01\xB9\x81\x0D\x0A" +# print(messaging_library.messaging_readbuffer(MES, R, robot_state)) +# print(R) + +# print("Testing too short actual payload length") +# MES = b"\xCC\x02\x01\xB9\x81\x0D\x0A" +# print(messaging_library.messaging_readbuffer(MES, R, robot_state)) +# print(R) + +# print("Testing too long actual payload length") +# MES = b"\xCC\x02\x01\x01\x01\xB9\x81\x0D\x0A" +# print(messaging_library.messaging_readbuffer(MES, R, robot_state)) +# print(R) + +# # newlines are included in multi-line strings, must concat strings +# print("Testing limit payload length") +# MES = b"\xCC\x7F" + \ +# b"\x01\x01\x01\x01\x01\x01\x01\x01" + \ +# b"\x01\x01\x01\x01\x01\x01\x01\x01" + \ +# b"\x01\x01\x01\x01\x01\x01\x01\x01" + \ +# b"\x01\x01\x01\x01\x01\x01\x01\x01" + \ +# b"\x01\x01\x01\x01\x01\x01\x01\x01" + \ +# b"\x01\x01\x01\x01\x01\x01\x01\x01" + \ +# b"\x01\x01\x01\x01\x01\x01\x01\x01" + \ +# b"\x01\x01\x01\x01\x01\x01\x01\x01" + \ +# b"\x01\x01\x01\x01\x01\x01\x01\x01" + \ +# b"\x01\x01\x01\x01\x01\x01\x01\x01" + \ +# b"\x01\x01\x01\x01\x01\x01\x01\x01" + \ +# b"\x01\x01\x01\x01\x01\x01\x01\x01" + \ +# b"\x01\x01\x01\x01\x01\x01\x01\x01" + \ +# b"\x01\x01\x01\x01\x01\x01\x01\x01" + \ +# b"\x01\x01\x01\x01\x01\x01\x01\x01" + \ +# b"\x01\x01\x01\x01\x01\x01\x01" + \ +# b"\xB9\x01\x0D\x0A" +# print(messaging_library.messaging_readbuffer(MES, R, robot_state)) +# print(R) + +# print("Testing exceed limit payload length") +# MES = b"\xCC\x80" + \ +# b"\x01\x01\x01\x01\x01\x01\x01\x01" + \ +# b"\x01\x01\x01\x01\x01\x01\x01\x01" + \ +# b"\x01\x01\x01\x01\x01\x01\x01\x01" + \ +# b"\x01\x01\x01\x01\x01\x01\x01\x01" + \ +# b"\x01\x01\x01\x01\x01\x01\x01\x01" + \ +# b"\x01\x01\x01\x01\x01\x01\x01\x01" + \ +# b"\x01\x01\x01\x01\x01\x01\x01\x01" + \ +# b"\x01\x01\x01\x01\x01\x01\x01\x01" + \ +# b"\x01\x01\x01\x01\x01\x01\x01\x01" + \ +# b"\x01\x01\x01\x01\x01\x01\x01\x01" + \ +# b"\x01\x01\x01\x01\x01\x01\x01\x01" + \ +# b"\x01\x01\x01\x01\x01\x01\x01\x01" + \ +# b"\x01\x01\x01\x01\x01\x01\x01\x01" + \ +# b"\x01\x01\x01\x01\x01\x01\x01\x01" + \ +# b"\x01\x01\x01\x01\x01\x01\x01\x01" + \ +# b"\x01\x01\x01\x01\x01\x01\x01\x01" + \ +# b"\xB9\x81\x0D\x0A" +# print(messaging_library.messaging_readbuffer(MES, R, robot_state)) +# print(R) + +# # incorrect checksum +# print("Testing incorrect checksum") +# MES = b"\xCC\x02\x01\x01\x01\xB9" + MIN + b"\x0D\x0A" +# print(messaging_library.messaging_readbuffer(MES, R, robot_state)) +# print(R) + +# # fixed character test (HEAD, TAIL, DELIMITERS) +# print("Testing incorrect head") +# MES = MIN + b"\x02\x01\x01\xB9\x81\x0D\x0A" +# print(messaging_library.messaging_readbuffer(MES, R, robot_state)) +# print(R) + +# print("Testing incorrect tail") +# MES = b"\xCC\x02\x01\x01" + MIN + b"\x81\x0D\x0A" +# print(messaging_library.messaging_readbuffer(MES, R, robot_state)) +# print(R) + +# print("Testing incorrect delimiter 1\n") +# MES = b"\xCC\x02\x01\x01\xB9\x81" + MIN + b"\x0A" +# print(messaging_library.messaging_readbuffer(MES, R, robot_state)) +# print(R) + +# print("Testing incorrect delimiter 2\n") +# MES = b"\xCC\x02\x01\x01\xB9\x81\x0D" + MIN +# print(messaging_library.messaging_readbuffer(MES, R, robot_state)) +# print(R) + +# # message structure test +# print("Testing incorrect structure") +# MES = b"\xCC" + MIN + b"\x02\x01\x01\xB9\x81\x0D\x0A" +# print(messaging_library.messaging_readbuffer(MES, R, robot_state)) +# print(R) + +# MES = b"\xCC\x02" + MIN + b"\x01\x01\xB9\x81\x0D\x0A" +# print(messaging_library.messaging_readbuffer(MES, R, robot_state)) +# print(R) + +# MES = b"\xCC\x02\x01\x01" + MIN + b"\xB9\x81\x0D\x0A" +# print(messaging_library.messaging_readbuffer(MES, R, robot_state)) +# print(R) + +# MES = b"\xCC\x02\x01\x01\xB9" + MIN + b"\x81\x0D\x0A" +# print(messaging_library.messaging_readbuffer(MES, R, robot_state)) +# print(R) + +# MES = b"\xCC\x02\x01\x01\xB9\x81" + MIN + b"\x0D\x0A" +# print(messaging_library.messaging_readbuffer(MES, R, robot_state)) +# print(R) + +# MES = b"\xCC\x02\x01\x01\xB9\x81\x0D" + MIN + b"\x0A" +# print(messaging_library.messaging_readbuffer(MES, R, robot_state)) +# print(R) + +# print("Testing characters before/after message") +# MES = MIN + b"\xCC\x02\x01\x01\xB9\x81\x0D\x0A" +# print(messaging_library.messaging_readbuffer(MES, R, robot_state)) +# print(R) + +# MES = b"\xCC\x02\x01\x01\xB9\x81\x0D\x0A" + MIN +# print(messaging_library.messaging_readbuffer(MES, R, robot_state)) +# print(R) + +# print("Testing WriteMessage") +# # put python encoded message here +# MES = b"\x80\x40\x13\x33\x33\x40\x13\x33\x33\x40\x13\x33\x33\x40\x13\x33"+\ +# b"\x33\x40\x13\x33\x33\x40\x13\x33\x33" +# R = messaging_library.messaging_write_message(MES) +# print("Python output: ", end="") +# print(R) +# print("C version: ", end="") +# print("{", end="") +# for char in list(R): +# if char == 10: +# print(hex(char), end="") +# else: +# print(hex(char), end=",") +# print("};", end="\n\n") + +print("Testing encode/decode between libraries") +# put c test harness output message here +MES = b"\xCC\x1B\x54\x45\x53\x54\x5F\x54\x45\x53\x54\x5F\x54\x45\x53\x54" +\ + b"\x2C\x31\x2C\x32\x2C\x33\x24\x5E\x26\x2A\x5E\x23\x24\xB9\xAD\x0D\x0A\x00" +print(messaging_library.messaging_readbuffer(MES, R, robot_state)) diff --git a/dev/messaging-library/swerve-module-id.h b/dev/messaging-library/swerve-module-id.h new file mode 100644 index 0000000..e9537df --- /dev/null +++ b/dev/messaging-library/swerve-module-id.h @@ -0,0 +1,45 @@ +/** + * Defined list of valid message ids and helper function to check and process messages into actions + */ +#ifndef _MESSAGING_IDS + #define _MESSAGING_IDS + + #define MOVE_IN_DIRECTION 0x80 + + #define resultArraySize 6 + + enum messageProcessResult { + PAYLOAD_ID_NOT_FOUND, + STATE_READ_RESULT_ARRAY + }; + + float resultArray[resultArraySize] = {0}; + char flippedBinaryArray[sizeof(float) * resultArraySize] = {0}; + /** + * ProcessPayload: reads parameter of specified length and performs actions based on the id + * + * @param payload pointer to beginning of payload + * @param id id of payload + * @param len length of payload + * + * @returns success/failure to process payload, next state of main state machine + */ + char MessagingProcessPayload(char* payload, unsigned char id, unsigned char len){ + switch(id){ + case MOVE_IN_DIRECTION: + // use https://www.h-schmidt.net/FloatConverter/IEEE754.html to convert between float and binary/hex + // store buffer array of uint8 (or char) to n floats + // bytes are stored in the wrong direction, flip and rewrite + + for(char i=0;i +#include +#include +#include +#include + +// digital low on in# pins indicates direction, both high is no signal +#define steer_in1_pin 4 // 1A, forward direction +#define steer_in2_pin 5 // 1B, backward direction + +#define drive_in1_pin 6 // 3A, forward direction +#define drive_in2_pin 7 // 3B, backard direction + +// #define motor_pwm_pin 9 // 2A, 2B take up by motor speed +#define steer_pwm_pin 9 // 2A, steer motor speed +#define drive_pwm_pin 8 // 2B, drive motor speed +#define pwm_slice 4 +#define steer_channel PWM_CHAN_B +#define drive_channel PWM_CHAN_A + +#define count_max 65535 // number of counts in a cycle --> 1/count_max = freq + +class Motor +{ +public: + Motor(uint pin1, uint pin2, uint pwm_pin, uint slice_num, pwm_chan channel, bool slice = true) + { + this->pwm_pin = pwm_pin; + this->pin1 = pin1; + this->pin2 = pin2; + this->slice_num = slice_num; + this->channel = channel; + + // setup pins for pwm functions + gpio_init(pin1); + gpio_init(pin2); + + // set non-pwm pins to output + gpio_set_dir(pin1, GPIO_OUT); + gpio_set_dir(pin2, GPIO_OUT); + + // setup pwm + gpio_set_function(pwm_pin, GPIO_FUNC_PWM); + + // set pwm slice and channel + if (slice) + pwm_set_wrap(slice_num, count_max); + + pwm_set_chan_level(slice_num, channel, 0); + + if (slice) + pwm_set_enabled(slice_num, true); + } + + void set(float power) + { + power = power > 1 ? 1 : power; + power = power < -1 ? -1 : power; + printf("input power: %f\n", power); + + if (power == 0) + { // in1 and in2 are high + gpio_put(this->pin1, 1); + gpio_put(this->pin2, 1); + } + else if (power < 0) + { // in1 is high and in2 is low + gpio_put(this->pin1, 1); + gpio_put(this->pin2, 0); + } + else + { // in1 is low and in2 is high + gpio_put(this->pin1, 0); + gpio_put(this->pin2, 1); + } + pwm_set_chan_level(this->slice_num, this->channel, abs((int)(power * count_max))); + } + +private: + uint pwm_pin; + uint pin1; + uint pin2; + uint slice_num; + pwm_chan channel; +}; + +int main() +{ + // setup stdio for printing + stdio_init_all(); + + Motor steer = Motor(steer_in1_pin, steer_in2_pin, steer_pwm_pin, pwm_slice, steer_channel, true); + Motor drive = Motor(drive_in1_pin, drive_in2_pin, drive_pwm_pin, pwm_slice, drive_channel, false); + + // step size for oscillation + float step = 0.01; + int a = 0; + + while (1) + { + float power = sin(a * step); + + steer.set(power); + drive.set(power); + + a++; + if (a * step >= 6.28) + a = 0; + + sleep_ms(20); + } + return 0; +} diff --git a/dev/pico_motor_test/cooler_motor_test.cpp b/dev/pico_motor_test/cooler_motor_test.cpp new file mode 100644 index 0000000..3061a73 --- /dev/null +++ b/dev/pico_motor_test/cooler_motor_test.cpp @@ -0,0 +1,129 @@ +#include +#include +#include +#include + +// digital low on in# pins indicates direction, both high is no signal +#define turn_in1_pin 4 // 1A, forward direction +#define turn_in2_pin 5 // 1B, backward direction + +// #define motor_pwm_pin 9 // 2A, 2B take up by motor speed +#define turn_pwm_pin 9 // 2A, turn motor speed +#define wheel_pwm_pin 8 // 2B, wheel motor speed +#define pwm_slice 4 +#define turn_channel PWM_CHAN_B +#define wheel_channel PWM_CHAN_A + +#define wheel_in1_pin 6 // 3A, forward direction +#define wheel_in2_pin 7 // 3B, backard direction + +// #define freq 500 // note: use clock management frequencies to set frequency +// #define duty_cycle 1 +#define count_max 65535 + +void setup() +{ // setup pins for pwm functions + stdio_init_all(); + gpio_init(turn_in1_pin); + gpio_init(turn_in2_pin); + gpio_init(wheel_in1_pin); + gpio_init(wheel_in2_pin); + + // check if default output signal is 0, for now put this in + gpio_put(turn_in1_pin, 0); + gpio_put(turn_in2_pin, 0); + gpio_put(wheel_in1_pin, 0); + gpio_put(wheel_in2_pin, 0); + + gpio_set_dir(turn_in1_pin, GPIO_OUT); + gpio_set_dir(turn_in2_pin, GPIO_OUT); + gpio_set_dir(wheel_in1_pin, GPIO_OUT); + gpio_set_dir(wheel_in2_pin, GPIO_OUT); + + gpio_set_function(turn_pwm_pin, GPIO_FUNC_PWM); + gpio_set_function(wheel_pwm_pin, GPIO_FUNC_PWM); + pwm_set_wrap(pwm_slice, count_max); + + pwm_set_chan_level(pwm_slice, turn_channel, 0); + pwm_set_chan_level(pwm_slice, wheel_channel, 0); + + pwm_set_enabled(pwm_slice, true); +} + +int main() +{ + setup(); + double x = 0; + double y = 0.5; + + int xflip = 0; + int yflip = 0; + + float step = 0.001; + + while (1) + { + // // turn motor + if (x == 0) + { // in1 and in2 are high + // pwm_set_gpio_level (3, count_max); this method would work, but is iffy, as setting the count value for individual pins update next cycle + gpio_put(turn_in1_pin, 1); + gpio_put(turn_in2_pin, 1); + } + else if (x < 0) + { // in1 is high and in2 is low + gpio_put(turn_in1_pin, 1); + gpio_put(turn_in2_pin, 0); + } + else + { // in1 is low and in2 is high + gpio_put(turn_in2_pin, 1); + gpio_put(turn_in1_pin, 0); + } + + // wheel motor + if (y == 0) + { // in1 and in2 are high + gpio_put(wheel_in1_pin, 1); + gpio_put(wheel_in2_pin, 1); + } + else if (y < 0) + { // in1 is high and in2 is low + gpio_put(wheel_in1_pin, 1); + gpio_put(wheel_in2_pin, 0); + } + else + { // in1 is low and in2 is high + gpio_put(wheel_in1_pin, 0); + gpio_put(wheel_in2_pin, 1); + } + + pwm_set_chan_level(pwm_slice, turn_channel, abs((int)(x * count_max))); + pwm_set_chan_level(pwm_slice, wheel_channel, abs((int)(y * count_max))); + // printf("hello world\n"); + if (xflip) + { + x -= step; + } + else + { + x += step; + } + if (yflip) + { + y -= step; + } + else + { + y += step; + } + if (x >= 1 || x <= -1) + xflip = !(xflip); + if (y >= 1 || y <= -1) + yflip = !(yflip); + + printf("x: %f, y: %f\n", x, y); + sleep_ms(20); + } + return 0; +} \ No newline at end of file diff --git a/dev/pico_motor_test/pico_sdk_import.cmake b/dev/pico_motor_test/pico_sdk_import.cmake new file mode 100644 index 0000000..65f8a6f --- /dev/null +++ b/dev/pico_motor_test/pico_sdk_import.cmake @@ -0,0 +1,73 @@ +# This is a copy of /external/pico_sdk_import.cmake + +# This can be dropped into an external project to help locate this SDK +# It should be include()ed prior to project() + +if (DEFINED ENV{PICO_SDK_PATH} AND (NOT PICO_SDK_PATH)) + set(PICO_SDK_PATH $ENV{PICO_SDK_PATH}) + message("Using PICO_SDK_PATH from environment ('${PICO_SDK_PATH}')") +endif () + +if (DEFINED ENV{PICO_SDK_FETCH_FROM_GIT} AND (NOT PICO_SDK_FETCH_FROM_GIT)) + set(PICO_SDK_FETCH_FROM_GIT $ENV{PICO_SDK_FETCH_FROM_GIT}) + message("Using PICO_SDK_FETCH_FROM_GIT from environment ('${PICO_SDK_FETCH_FROM_GIT}')") +endif () + +if (DEFINED ENV{PICO_SDK_FETCH_FROM_GIT_PATH} AND (NOT PICO_SDK_FETCH_FROM_GIT_PATH)) + set(PICO_SDK_FETCH_FROM_GIT_PATH $ENV{PICO_SDK_FETCH_FROM_GIT_PATH}) + message("Using PICO_SDK_FETCH_FROM_GIT_PATH from environment ('${PICO_SDK_FETCH_FROM_GIT_PATH}')") +endif () + +set(PICO_SDK_PATH "${PICO_SDK_PATH}" CACHE PATH "Path to the Raspberry Pi Pico SDK") +set(PICO_SDK_FETCH_FROM_GIT "${PICO_SDK_FETCH_FROM_GIT}" CACHE BOOL "Set to ON to fetch copy of SDK from git if not otherwise locatable") +set(PICO_SDK_FETCH_FROM_GIT_PATH "${PICO_SDK_FETCH_FROM_GIT_PATH}" CACHE FILEPATH "location to download SDK") + +if (NOT PICO_SDK_PATH) + if (PICO_SDK_FETCH_FROM_GIT) + include(FetchContent) + set(FETCHCONTENT_BASE_DIR_SAVE ${FETCHCONTENT_BASE_DIR}) + if (PICO_SDK_FETCH_FROM_GIT_PATH) + get_filename_component(FETCHCONTENT_BASE_DIR "${PICO_SDK_FETCH_FROM_GIT_PATH}" REALPATH BASE_DIR "${CMAKE_SOURCE_DIR}") + endif () + # GIT_SUBMODULES_RECURSE was added in 3.17 + if (${CMAKE_VERSION} VERSION_GREATER_EQUAL "3.17.0") + FetchContent_Declare( + pico_sdk + GIT_REPOSITORY https://github.com/raspberrypi/pico-sdk + GIT_TAG master + GIT_SUBMODULES_RECURSE FALSE + ) + else () + FetchContent_Declare( + pico_sdk + GIT_REPOSITORY https://github.com/raspberrypi/pico-sdk + GIT_TAG master + ) + endif () + + if (NOT pico_sdk) + message("Downloading Raspberry Pi Pico SDK") + FetchContent_Populate(pico_sdk) + set(PICO_SDK_PATH ${pico_sdk_SOURCE_DIR}) + endif () + set(FETCHCONTENT_BASE_DIR ${FETCHCONTENT_BASE_DIR_SAVE}) + else () + message(FATAL_ERROR + "SDK location was not specified. Please set PICO_SDK_PATH or set PICO_SDK_FETCH_FROM_GIT to on to fetch from git." + ) + endif () +endif () + +get_filename_component(PICO_SDK_PATH "${PICO_SDK_PATH}" REALPATH BASE_DIR "${CMAKE_BINARY_DIR}") +if (NOT EXISTS ${PICO_SDK_PATH}) + message(FATAL_ERROR "Directory '${PICO_SDK_PATH}' not found") +endif () + +set(PICO_SDK_INIT_CMAKE_FILE ${PICO_SDK_PATH}/pico_sdk_init.cmake) +if (NOT EXISTS ${PICO_SDK_INIT_CMAKE_FILE}) + message(FATAL_ERROR "Directory '${PICO_SDK_PATH}' does not appear to contain the Raspberry Pi Pico SDK") +endif () + +set(PICO_SDK_PATH ${PICO_SDK_PATH} CACHE PATH "Path to the Raspberry Pi Pico SDK" FORCE) + +include(${PICO_SDK_INIT_CMAKE_FILE}) diff --git a/dev/swerve/motor_test.py b/dev/swerve/motor_test.py new file mode 100644 index 0000000..c5e6860 --- /dev/null +++ b/dev/swerve/motor_test.py @@ -0,0 +1,88 @@ +import lgpio # sudo apt install python3-lgpio +from Controller import Controller + + +turn_in1_pin = 17 +turn_in2_pin = 27 +turn_pwm_pin = 22 + +wheel_in1_pin = 11 +wheel_in2_pin = 9 +wheel_pwm_pin = 10 + +freq = 500 +disable = True +joy = Controller() + +h = lgpio.gpiochip_open(0) + +lgpio.gpio_claim_output(h, turn_in1_pin) +lgpio.gpio_claim_output(h, turn_in2_pin) +lgpio.gpio_claim_output(h, wheel_in1_pin) +lgpio.gpio_claim_output(h, wheel_in2_pin) + +exit_count = 0 + +try: + while True: + status = joy.read_self() + + if status.X: + if not disable: + print("disabling!") + disable = True + exit_count += 1 + elif status.B: + if disable: + print("enabling!") + disable = False + exit_count = 0 + + if disable: + lgpio.gpio_write(h, turn_in1_pin, 1) + lgpio.gpio_write(h, turn_in2_pin, 1) + lgpio.gpio_write(h, wheel_in1_pin, 1) + lgpio.gpio_write(h, wheel_in2_pin, 1) + + if exit_count > 50000: + break + + continue + + x = status.LeftJoystickX + y = status.RightJoystickY + + if x == 0: + lgpio.gpio_write(h, turn_in1_pin, 1) + lgpio.gpio_write(h, turn_in2_pin, 1) + elif x < 0: + lgpio.gpio_write(h, turn_in1_pin, 1) + lgpio.gpio_write(h, turn_in2_pin, 0) + else: + lgpio.gpio_write(h, turn_in1_pin, 0) + lgpio.gpio_write(h, turn_in2_pin, 1) + + if y == 0: + lgpio.gpio_write(h, wheel_in1_pin, 1) + lgpio.gpio_write(h, wheel_in2_pin, 1) + elif y < 0: + lgpio.gpio_write(h, wheel_in1_pin, 1) + lgpio.gpio_write(h, wheel_in2_pin, 0) + else: + lgpio.gpio_write(h, wheel_in1_pin, 0) + lgpio.gpio_write(h, wheel_in2_pin, 1) + + lgpio.tx_pwm(h, turn_pwm_pin, freq, round(abs(x) * 100.0, 2)) + lgpio.tx_pwm(h, wheel_pwm_pin, freq, round(abs(y) * 100.0, 2)) +except KeyboardInterrupt: + pass + +lgpio.tx_pwm(h, turn_pwm_pin, freq, 0) +lgpio.gpio_write(h, turn_in1_pin, 1) +lgpio.gpio_write(h, turn_in2_pin, 1) + +lgpio.tx_pwm(h, wheel_pwm_pin, freq, 0) +lgpio.gpio_write(h, wheel_in1_pin, 1) +lgpio.gpio_write(h, wheel_in2_pin, 1) + +lgpio.gpiochip_close(h) diff --git a/dev/swerve_sim_joystick/joystick_sim.py b/dev/swerve_sim_joystick/joystick_sim.py index 619913b..88eb379 100644 --- a/dev/swerve_sim_joystick/joystick_sim.py +++ b/dev/swerve_sim_joystick/joystick_sim.py @@ -1,9 +1,7 @@ import matplotlib.pyplot as plt import numpy as np - -# from Controller import Gem_Xbox_Controller -from Controller import Nintendo_Pro_Controller -from Controller import PS4_Controller +from Controller import (Gem_Xbox_Controller, Nintendo_Pro_Controller, + PS4_Controller) # return the vector perpendicular to the given vector @@ -12,7 +10,7 @@ def perpendicular(vec): # NOTE: make sure to account for max motor speed when programming real motors, and normalize -# for example, if the swerve math commands one motor to spin higher than it's max speed, +# for example, if the swerve math commands one motor to spin higher than it's max speed, # then it will only spin at the max speed, thus making the ratio of motor powers wrong and the robot will move wrong diff --git a/dev/swerve_sim_keyboard/src/main.py b/dev/swerve_sim_keyboard/src/main.py old mode 100755 new mode 100644 diff --git a/dev/swerve_sim_keyboard/test/keyboard_test.py b/dev/swerve_sim_keyboard/test/keyboard_test.py old mode 100755 new mode 100644 diff --git a/swerve_pico/CMakeLists.txt b/swerve_pico/CMakeLists.txt new file mode 100644 index 0000000..7fa9b64 --- /dev/null +++ b/swerve_pico/CMakeLists.txt @@ -0,0 +1,62 @@ +# Generated Cmake Pico project file + +cmake_minimum_required(VERSION 3.13) + +set(CMAKE_C_STANDARD 11) +set(CMAKE_CXX_STANDARD 17) + +# Initialise pico_sdk from installed location +# (note this can come from environment, CMake cache etc) + +set(PICO_BOARD pico CACHE STRING "Board type") + +# Pull in Raspberry Pi Pico SDK (must be before project) +include(pico_sdk_import.cmake) + +if (PICO_SDK_VERSION_STRING VERSION_LESS "1.4.0") + message(FATAL_ERROR "Raspberry Pi Pico SDK version 1.4.0 (or later) required. Your version is ${PICO_SDK_VERSION_STRING}") +endif() + +project(swerve-pico C CXX ASM) + +# enable error handling +set(PICO_CXX_ENABLE_EXCEPTIONS 1) + +# Initialise the Raspberry Pi Pico SDK +pico_sdk_init() + +# Add executable. Default name is the project name, version 0.1 +add_executable(swerve-pico swerve-pico.cpp +) + + +pico_set_program_name(swerve-pico "swerve-pico") +pico_set_program_version(swerve-pico "0.1") + +pico_enable_stdio_uart(swerve-pico 0) +pico_enable_stdio_usb(swerve-pico 1) + +# Add the standard library to the build +target_link_libraries(swerve-pico + pico_stdlib + pico_i2c_slave + hardware_i2c + hardware_pio +) + +# Add the standard include files to the build +target_include_directories(swerve-pico PRIVATE + ${CMAKE_CURRENT_LIST_DIR} + ${CMAKE_CURRENT_LIST_DIR}/.. # for our common lwipopts or any other standard includes, if required +) + +# Add any user requested libraries +target_link_libraries(swerve-pico + hardware_pwm + hardware_pio +) + +# link pio +pico_generate_pio_header(swerve-pico ${CMAKE_CURRENT_LIST_DIR}/motor-library/quadrature_encoder.pio) + +pico_add_extra_outputs(swerve-pico) diff --git a/swerve_pico/_OLD/CMakeLists.txt b/swerve_pico/_OLD/CMakeLists.txt new file mode 100644 index 0000000..914fc65 --- /dev/null +++ b/swerve_pico/_OLD/CMakeLists.txt @@ -0,0 +1,55 @@ +# Generated Cmake Pico project file + +cmake_minimum_required(VERSION 3.5) + +set(CMAKE_C_STANDARD 11) +set(CMAKE_CXX_STANDARD 17) + +# Initialise pico_sdk from installed location +# (note this can come from environment, CMake cache etc) + +set(PICO_BOARD pico CACHE STRING "Board type") + +# Pull in Raspberry Pi Pico SDK (must be before project) +include(pico_sdk_import.cmake) + +if (PICO_SDK_VERSION_STRING VERSION_LESS "1.4.0") + message(FATAL_ERROR "Raspberry Pi Pico SDK version 1.4.0 (or later) required. Your version is ${PICO_SDK_VERSION_STRING}") +endif() + +project(i2c_drive_pico C CXX ASM) + +# Initialise the Raspberry Pi Pico SDK +pico_sdk_init() + +# Add executable. Default name is the project name, version 0.1 + +add_executable(i2c_drive_pico i2c_drive_pico.c ) + +pico_set_program_name(i2c_drive_pico "i2c_drive_pico") +pico_set_program_version(i2c_drive_pico "0.1") + +pico_enable_stdio_uart(i2c_drive_pico 0) +pico_enable_stdio_usb(i2c_drive_pico 1) + +# Add the standard library to the build +target_link_libraries(i2c_drive_pico + pico_stdlib + pico_i2c_slave + hardware_i2c + hardware_pwm + ) + +# Add the standard include files to the build +target_include_directories(i2c_drive_pico PRIVATE + ${CMAKE_CURRENT_LIST_DIR} + ${CMAKE_CURRENT_LIST_DIR}/.. # for our common lwipopts or any other standard includes, if required +) + +# Add any user requested libraries +target_link_libraries(i2c_drive_pico + hardware_i2c + ) + +pico_add_extra_outputs(i2c_drive_pico) + diff --git a/swerve_pico/_OLD/basic_swerve_pico.c b/swerve_pico/_OLD/basic_swerve_pico.c new file mode 100644 index 0000000..9f4bf53 --- /dev/null +++ b/swerve_pico/_OLD/basic_swerve_pico.c @@ -0,0 +1,393 @@ +#include +#include +#include +#include +#include +#include +#include + +// Define constants for I2C communication +#define I2C_PICO_ADDR 0x08 +#define I2C_SDA_PIN 0 +#define I2C_SCL_PIN 1 +#define I2C_PORT i2c0 +#define I2C_BAUDRATE 100 * 1000 + +// Define the length of the data packet +#define I2C_DATA_LENGTH 128 + +#define MESSAGE_START 0xFA +#define MESSAGE_STOP 0xFB + +// digital low on a/b pins indicates direction, both high is no signal (stop) +// pwm signal on pwm pin controls speed + +#define wheel_1_pwm 2 +#define wheel_1_slice 1 +#define wheel_1_channel PWM_CHAN_A +#define wheel_1a 3 +#define wheel_1b 4 + +#define turn_1a 5 +#define turn_1b 6 +#define turn_1_pwm 7 +#define turn_1_slice 3 +#define turn_1_channel PWM_CHAN_B + +#define wheel_2_pwm 8 +#define wheel_2_slice 4 +#define wheel_2_channel PWM_CHAN_A +#define wheel_2a 9 +#define wheel_2b 10 + +#define turn_2a 11 +#define turn_2b 12 +#define turn_2_pwm 13 +#define turn_2_slice 6 +#define turn_2_channel PWM_CHAN_B + +#define turn_3_pwm 16 +#define turn_3_slice 0 +#define turn_3_channel PWM_CHAN_A +#define turn_3b 17 +#define turn_3a 18 + +#define wheel_3b 19 +#define wheel_3a 20 +#define wheel_3_pwm 21 +#define wheel_3_slice 2 +#define wheel_3_channel PWM_CHAN_B + +// #define freq 500 // note: use clock management frequencies to set frequency +#define count_max 65535 + +// Status of the input data +int input_status = 0; + +// Last event that occurred +int last_event = 0; + +// Index of the current data byte +int data_index = 0; + +int count = 0; + +// Buffer for the input data +typedef struct dataT +{ + uint8_t data[I2C_DATA_LENGTH]; + uint8_t len; +} *data; + +typedef struct buffT +{ + data buff[I2C_DATA_LENGTH]; + uint8_t head; + uint8_t tail; + uint8_t full; +} *buff; + +buff buffer; +data tmp; + +// Initialize the circular buffer +void init_buff(buff *b, data *d) +{ + *b = malloc(sizeof(struct buffT)); + (*b)->head = 0; + (*b)->tail = 0; + (*b)->full = 0; + for (int i = 0; i < I2C_DATA_LENGTH; i++) + { + (*b)->buff[i] = malloc(sizeof(struct dataT)); + (*b)->buff[i]->len = 0; + for (int j = 0; j < I2C_DATA_LENGTH; j++) + { + (*b)->buff[i]->data[j] = 0; + } + } + + *d = malloc(sizeof(struct dataT)); + (*d)->len = 0; + for (int i = 0; i < I2C_DATA_LENGTH; i++) + { + (*d)->data[i] = 0; + } +} + +// Handler for I2C events +static void i2c_handler(i2c_inst_t *i2c, i2c_slave_event_t event) +{ + switch (event) + { + case I2C_SLAVE_RECEIVE: + { + // Read the data + uint8_t tmp_byte = i2c_read_byte_raw(i2c); + // Check if the data is valid + if (tmp->len >= I2C_DATA_LENGTH) + { + printf("Invalid data %x, len %x\n", tmp_byte, tmp->len); + break; + } + // Store the data + tmp->data[tmp->len] = tmp_byte; + tmp->len++; + // set the event status to received + last_event = 1; + + break; + } + + case I2C_SLAVE_REQUEST: // Pi is requesting data + // Write the data into the void + i2c_write_byte_raw(i2c, (uint8_t)input_status); + // set the event status to sent + last_event = 2; + break; + + case I2C_SLAVE_FINISH: // Pi has signalled Stop / Restart + // if the last event was a receive event and the data is valid + if (last_event == 1 && !buffer->full) + { + if (tmp->data[0] == MESSAGE_START && tmp->data[tmp->len - 1] == MESSAGE_STOP) + { + for (int i = 0; i < tmp->len; i++) + { + buffer->buff[buffer->tail]->data[i] = tmp->data[i]; + } + buffer->buff[buffer->tail]->len = tmp->len; + + // set the input status to ready + input_status = 1; + + // move the tail of the buffer + buffer->tail = (buffer->tail + 1) % I2C_DATA_LENGTH; + + // check if the buffer is full + if (buffer->tail == buffer->head) + { + buffer->full = 1; + } + tmp->len = 0; + } + } + else + { + // printf("Buffer full, attempted to put this into buffer:\n"); + // for (int i = 0; i < tmp->len; i++) + // { + // printf("%d ", tmp->data[i]); + // } + // printf("\n"); + tmp->len = 0; + } + break; + default: + break; + } +} + +// linear & angular velocity for each wheel +float v1 = 0.0f; +float v2 = 0.0f; +float v3 = 0.0f; + +float w1 = 0.0f; +float w2 = 0.0f; +float w3 = 0.0f; + +const int num_floats = 6; + +float rot1, rot2, rot3; // current working rotation to calc delta +float delt1, delt2, delt3; // change in rotation after subtracting omega +float u1, u2, u3; // interim omega values + +int flip1 = 1, flip2 = 1, flip3 = 1; +// const float scale = + +void setup_pins(int a, int b, int pwm, int slice, int channel) +{ + gpio_init(a); + gpio_init(b); + gpio_init(pwm); + gpio_set_dir(a, GPIO_OUT); + gpio_set_dir(b, GPIO_OUT); + gpio_set_function(pwm, GPIO_FUNC_PWM); + pwm_set_wrap(slice, count_max); + pwm_set_chan_level(slice, channel, 0); + pwm_set_enabled(slice, true); +} + +int main() +{ + const uint LED_PIN = PICO_DEFAULT_LED_PIN; + gpio_init(LED_PIN); + gpio_set_dir(LED_PIN, GPIO_OUT); + + stdio_init_all(); + + // Initialize I2C at 100kHz + i2c_init(I2C_PORT, I2C_BAUDRATE); + gpio_set_function(I2C_SDA_PIN, GPIO_FUNC_I2C); + gpio_set_function(I2C_SCL_PIN, GPIO_FUNC_I2C); + + init_buff(&buffer, &tmp); + + // Set I2C address for Pico + i2c_slave_init(I2C_PORT, I2C_PICO_ADDR, &i2c_handler); + + // Set up the pins for the motors + setup_pins(wheel_1a, wheel_1b, wheel_1_pwm, wheel_1_slice, wheel_1_channel); + setup_pins(wheel_2a, wheel_2b, wheel_2_pwm, wheel_2_slice, wheel_2_channel); + setup_pins(wheel_3a, wheel_3b, wheel_3_pwm, wheel_3_slice, wheel_3_channel); + + setup_pins(turn_1a, turn_1b, turn_1_pwm, turn_1_slice, turn_1_channel); + setup_pins(turn_2a, turn_2b, turn_2_pwm, turn_2_slice, turn_2_channel); + setup_pins(turn_3a, turn_3b, turn_3_pwm, turn_3_slice, turn_3_channel); + + sleep_ms(1000); // wait for the i2c to be ready + + absolute_time_t start_time = get_absolute_time(); + + // turns 180 degrees (approximately) - use 11660 to help scale the degrees needed to turn to the # of timesteps necessary to complete that turn at full speed + // for (int timetest = 0; timetest < 11660; timetest++) + while (1) + { + gpio_put(LED_PIN, 1); + if (input_status == 1) + { + input_status = 0; + // use https://www.h-schmidt.net/FloatConverter/IEEE754.html to convert between float and binary/hex + uint32_t tmp_float[num_floats]; + + for (int i = 0; i < num_floats; i++) + { + tmp_float[i] = 0; + for (int j = 0; j < 4; j++) + { + tmp_float[i] |= buffer->buff[buffer->head]->data[(i * 4) + j + 1] << (8 * j); + // printf("%d:%d ", (i * 4) + j + 1, buffer->buff[buffer->head]->data[(i * 4) + j + 1]); + } + // printf("\n %x\n", tmp_float[i]); + } + v1 = *(((float *)&tmp_float[0])); + v2 = *(((float *)&tmp_float[2])); + v3 = *(((float *)&tmp_float[4])); + + printf("i2c %u ", to_ms_since_boot(get_absolute_time())); + + // tmp_float = 0; // clear float + buffer->head = (buffer->head + 1) % I2C_DATA_LENGTH; + buffer->full = 0; + } + gpio_put(LED_PIN, 0); + + // get delta for wheel 1 + delt1 = v1 - rot1; + if (abs(delt1) > 90) + { + flip1 *= -1; + delt1 = (90) * (delt1 < 0 ? -1 : 1) - delt1; + } + + // get delta for wheel 2 + delt2 = v2 - rot2; + if (abs(delt2) > 90) + { + flip2 *= -1; + delt2 = (90) * (delt2 < 0 ? -1 : 1) - delt2; + } + + // get delta for wheel 3 + delt3 = v3 - rot3; + if (abs(delt3) > 90) + { + flip3 *= -1; + delt3 = (90) * (delt3 < 0 ? -1 : 1) - delt3; + } + + if (v1 == 0) + { + // in1 and in2 are high + gpio_put(wheel_1a, 1); + gpio_put(wheel_1b, 1); + } + else if (v1 * flip1 < 0) + { + // in1 is high and in2 is low + gpio_put(wheel_1a, 1); + gpio_put(wheel_1b, 0); + rot1 += 0.01543739279 + } + else + { + // in1 is low and in2 is high + gpio_put(wheel_1b, 1); + gpio_put(wheel_1a, 0); + rot1 -= 0.01543739279 + } + + if (v2 == 0) + { + // in1 and in2 are high + gpio_put(wheel_2a, 1); + gpio_put(wheel_2b, 1); + } + else if (v2 * flip2 < 0) + { + // in1 is high and in2 is low + gpio_put(wheel_2a, 1); + gpio_put(wheel_2b, 0); + rot2 += 0.01543739279 + } + else + { + // in1 is low and in2 is high + gpio_put(wheel_2b, 1); + gpio_put(wheel_2a, 0); + rot2 -= 0.01543739279 + } + + if (v3 == 0) + { + // in1 and in2 are high + gpio_put(wheel_3a, 1); + gpio_put(wheel_3b, 1); + } + else if (v3 * flip3 < 0) + { + // in1 is high and in2 is low + gpio_put(wheel_3a, 1); + gpio_put(wheel_3b, 0); + rot3 += 0.01543739279 + } + else + { + // in1 is low and in2 is high + gpio_put(wheel_3b, 1); + gpio_put(wheel_3a, 0); + rot3 -= 0.01543739279 + } + pwm_set_chan_level(wheel_1_slice, wheel_1_channel, abs((int)(v1 * count_max))); + + pwm_set_chan_level(wheel_2_slice, wheel_2_channel, abs((int)(v2 * count_max))); + + pwm_set_chan_level(wheel_3_slice, wheel_3_channel, abs((int)(v3 * count_max))); + + // sleep_ms(20); // used for debugging - reduces the number of loops w/ no new i2c data + printf("%u %u\n", to_ms_since_boot(get_absolute_time()), to_ms_since_boot(start_time)); + } + + printf("%u\n", to_ms_since_boot(get_absolute_time())); + + while (1) + { + gpio_put(LED_PIN, 1); + sleep_ms(500); + gpio_put(LED_PIN, 0); + sleep_ms(500); + } + + return 0; +} \ No newline at end of file diff --git a/swerve_pico/_OLD/i2c_drive_pico.c b/swerve_pico/_OLD/i2c_drive_pico.c new file mode 100644 index 0000000..1b89574 --- /dev/null +++ b/swerve_pico/_OLD/i2c_drive_pico.c @@ -0,0 +1,220 @@ +#include +#include +#include +#include +#include +#include + +// Define constants for I2C communication +#define I2C_PICO_ADDR 0x08 +#define I2C_SDA_PIN 0 +#define I2C_SCL_PIN 1 +#define I2C_PORT i2c0 +#define I2C_BAUDRATE 100 * 1000 + +// Define the length of the data packet +#define I2C_DATA_LENGTH 10 + +#define MESSAGE_START 0xFA +#define MESSAGE_STOP 0xFB + + +// digital low on in# pins indicates direction, both high is no signal +#define turn_in1_pin 4 // 1A, forward direction +#define turn_in2_pin 5 // 1B, backward direction + +// #define motor_pwm_pin 9 // 2A, 2B take up by motor speed +#define turn_pwm_pin 9 // 2A, turn motor speed +#define wheel_pwm_pin 8 // 2B, wheel motor speed +#define pwm_slice 4 +#define turn_channel PWM_CHAN_B +#define wheel_channel PWM_CHAN_A + +#define wheel_in1_pin 6 // 3A, forward direction +#define wheel_in2_pin 7 // 3B, backard direction + +// #define freq 500 // note: use clock management frequencies to set frequency +// #define duty_cycle 1 +#define count_max 65535 + + +// Buffer for incoming data +uint8_t incoming_data[I2C_DATA_LENGTH]; + +// Status of the input data +uint8_t input_status = 0; + +// Last event that occurred +int last_event = 0; + +// Index of the current data byte +int data_index = 0; + +// Buffer for the input data +uint8_t input[I2C_DATA_LENGTH - 2]; + +// Handler for I2C events +static void i2c_handler(i2c_inst_t *i2c, i2c_slave_event_t event) +{ + switch (event) + { + case I2C_SLAVE_RECEIVE:{ + // Read the data + uint8_t tmp = i2c_read_byte_raw(i2c); + // Check if the data is valid + // TODO: probably revert this back to the original, we don't really need the MESSAGE_START stuff + if ((incoming_data[0] == 0x00 && tmp != MESSAGE_START) || data_index >= I2C_DATA_LENGTH) + { + printf("Invalid data %x\n", tmp); + break; + } + // Store the data + incoming_data[data_index] = tmp; + // printf("Data: %d\n", incoming_data[data_index]); + data_index++; + // set the event status to received + last_event = 1; + break; + } + + + case I2C_SLAVE_REQUEST: // Pi is requesting data + // Write the data into the void + i2c_write_byte_raw(i2c, (uint8_t)input_status); + // set the event status to sent + last_event = 2; + break; + + case I2C_SLAVE_FINISH: // Pi has signalled Stop / Restart + // if the last event was a receive event and the data is valid + if (last_event == 1) + if (incoming_data[0] == MESSAGE_START && incoming_data[I2C_DATA_LENGTH - 1] == MESSAGE_STOP) + { + // move the data into the input array + for (int i = 0; i < I2C_DATA_LENGTH - 2; i++) + { + input[i] = (int)incoming_data[i + 1]; + } + // set the input status to ready + input_status = 1; + + // Reset incoming_data + for (int i = 0; i < I2C_DATA_LENGTH; i++) + { + incoming_data[i] = 0x00; + } + } + data_index = 0; + break; + default: + break; + } +} + + +bool read = false; + +float joyX = 0.0f; +float joyY = 0.0f; + +void setup() +{ // setup pins for pwm functions + stdio_init_all(); + gpio_init(turn_in1_pin); + gpio_init(turn_in2_pin); + gpio_init(wheel_in1_pin); + gpio_init(wheel_in2_pin); + + // check if default output signal is 0, for now put this in + gpio_put(turn_in1_pin, 0); + gpio_put(turn_in2_pin, 0); + gpio_put(wheel_in1_pin, 0); + gpio_put(wheel_in2_pin, 0); + + gpio_set_dir(turn_in1_pin, GPIO_OUT); + gpio_set_dir(turn_in2_pin, GPIO_OUT); + gpio_set_dir(wheel_in1_pin, GPIO_OUT); + gpio_set_dir(wheel_in2_pin, GPIO_OUT); + + gpio_set_function(turn_pwm_pin, GPIO_FUNC_PWM); + gpio_set_function(wheel_pwm_pin, GPIO_FUNC_PWM); + pwm_set_wrap(pwm_slice, count_max); + + pwm_set_chan_level(pwm_slice, turn_channel, 0); + pwm_set_chan_level(pwm_slice, wheel_channel, 0); + + pwm_set_enabled(pwm_slice, true); +} + +int main() +{ + + stdio_init_all(); + + // Initialize I2C at 100kHz + i2c_init(I2C_PORT, I2C_BAUDRATE); + gpio_set_function(I2C_SDA_PIN, GPIO_FUNC_I2C); + gpio_set_function(I2C_SCL_PIN, GPIO_FUNC_I2C); + + // Set I2C address for Pico + i2c_slave_init(I2C_PORT, I2C_PICO_ADDR, &i2c_handler); + + setup();//setup for pwm + + while (1) + { + if (input_status ==1){ + uint64_t tmp_float = 0; //use https://www.h-schmidt.net/FloatConverter/IEEE754.html to convert between float and binary/hex + for (int i = 7; i >= 0; i--) //bytes are stored in int8 array (64 bits), pico reads backwards + { + tmp_float |= input[i]; //write byte at a time to tmp_float and shift + if(i!=0) + tmp_float<<=8; //preserves order of bits in 64bit int + } + joyX = *(((float*)&tmp_float)); //convert to interprit bits as float (32 bits) + joyY = *(((float*)&tmp_float)+1); + printf("%f ", joyX); + printf("%f\n", joyY); //printing floats in console + tmp_float = 0; //clear float + } + + if (joyX == 0) + { // in1 and in2 are high + // pwm_set_gpio_level (3, count_max); this method would work, but is iffy, as setting the count value for individual pins update next cycle + gpio_put(turn_in1_pin, 1); + gpio_put(turn_in2_pin, 1); + } + else if (joyX < 0) + { // in1 is high and in2 is low + gpio_put(turn_in1_pin, 1); + gpio_put(turn_in2_pin, 0); + } + else + { // in1 is low and in2 is high + gpio_put(turn_in2_pin, 1); + gpio_put(turn_in1_pin, 0); + } + + // wheel motor + if (joyY == 0) + { // in1 and in2 are high + gpio_put(wheel_in1_pin, 1); + gpio_put(wheel_in2_pin, 1); + } + else if (joyY < 0) + { // in1 is high and in2 is low + gpio_put(wheel_in1_pin, 1); + gpio_put(wheel_in2_pin, 0); + } + else + { // in1 is low and in2 is high + gpio_put(wheel_in1_pin, 0); + gpio_put(wheel_in2_pin, 1); + } + + pwm_set_chan_level(pwm_slice, turn_channel, abs((int)(joyX* count_max))); + pwm_set_chan_level(pwm_slice, wheel_channel, abs((int)(joyY * count_max))); + } + + return 0; +} \ No newline at end of file diff --git a/swerve_pico/_OLD/pico_sdk_import.cmake b/swerve_pico/_OLD/pico_sdk_import.cmake new file mode 100644 index 0000000..65f8a6f --- /dev/null +++ b/swerve_pico/_OLD/pico_sdk_import.cmake @@ -0,0 +1,73 @@ +# This is a copy of /external/pico_sdk_import.cmake + +# This can be dropped into an external project to help locate this SDK +# It should be include()ed prior to project() + +if (DEFINED ENV{PICO_SDK_PATH} AND (NOT PICO_SDK_PATH)) + set(PICO_SDK_PATH $ENV{PICO_SDK_PATH}) + message("Using PICO_SDK_PATH from environment ('${PICO_SDK_PATH}')") +endif () + +if (DEFINED ENV{PICO_SDK_FETCH_FROM_GIT} AND (NOT PICO_SDK_FETCH_FROM_GIT)) + set(PICO_SDK_FETCH_FROM_GIT $ENV{PICO_SDK_FETCH_FROM_GIT}) + message("Using PICO_SDK_FETCH_FROM_GIT from environment ('${PICO_SDK_FETCH_FROM_GIT}')") +endif () + +if (DEFINED ENV{PICO_SDK_FETCH_FROM_GIT_PATH} AND (NOT PICO_SDK_FETCH_FROM_GIT_PATH)) + set(PICO_SDK_FETCH_FROM_GIT_PATH $ENV{PICO_SDK_FETCH_FROM_GIT_PATH}) + message("Using PICO_SDK_FETCH_FROM_GIT_PATH from environment ('${PICO_SDK_FETCH_FROM_GIT_PATH}')") +endif () + +set(PICO_SDK_PATH "${PICO_SDK_PATH}" CACHE PATH "Path to the Raspberry Pi Pico SDK") +set(PICO_SDK_FETCH_FROM_GIT "${PICO_SDK_FETCH_FROM_GIT}" CACHE BOOL "Set to ON to fetch copy of SDK from git if not otherwise locatable") +set(PICO_SDK_FETCH_FROM_GIT_PATH "${PICO_SDK_FETCH_FROM_GIT_PATH}" CACHE FILEPATH "location to download SDK") + +if (NOT PICO_SDK_PATH) + if (PICO_SDK_FETCH_FROM_GIT) + include(FetchContent) + set(FETCHCONTENT_BASE_DIR_SAVE ${FETCHCONTENT_BASE_DIR}) + if (PICO_SDK_FETCH_FROM_GIT_PATH) + get_filename_component(FETCHCONTENT_BASE_DIR "${PICO_SDK_FETCH_FROM_GIT_PATH}" REALPATH BASE_DIR "${CMAKE_SOURCE_DIR}") + endif () + # GIT_SUBMODULES_RECURSE was added in 3.17 + if (${CMAKE_VERSION} VERSION_GREATER_EQUAL "3.17.0") + FetchContent_Declare( + pico_sdk + GIT_REPOSITORY https://github.com/raspberrypi/pico-sdk + GIT_TAG master + GIT_SUBMODULES_RECURSE FALSE + ) + else () + FetchContent_Declare( + pico_sdk + GIT_REPOSITORY https://github.com/raspberrypi/pico-sdk + GIT_TAG master + ) + endif () + + if (NOT pico_sdk) + message("Downloading Raspberry Pi Pico SDK") + FetchContent_Populate(pico_sdk) + set(PICO_SDK_PATH ${pico_sdk_SOURCE_DIR}) + endif () + set(FETCHCONTENT_BASE_DIR ${FETCHCONTENT_BASE_DIR_SAVE}) + else () + message(FATAL_ERROR + "SDK location was not specified. Please set PICO_SDK_PATH or set PICO_SDK_FETCH_FROM_GIT to on to fetch from git." + ) + endif () +endif () + +get_filename_component(PICO_SDK_PATH "${PICO_SDK_PATH}" REALPATH BASE_DIR "${CMAKE_BINARY_DIR}") +if (NOT EXISTS ${PICO_SDK_PATH}) + message(FATAL_ERROR "Directory '${PICO_SDK_PATH}' not found") +endif () + +set(PICO_SDK_INIT_CMAKE_FILE ${PICO_SDK_PATH}/pico_sdk_init.cmake) +if (NOT EXISTS ${PICO_SDK_INIT_CMAKE_FILE}) + message(FATAL_ERROR "Directory '${PICO_SDK_PATH}' does not appear to contain the Raspberry Pi Pico SDK") +endif () + +set(PICO_SDK_PATH ${PICO_SDK_PATH} CACHE PATH "Path to the Raspberry Pi Pico SDK" FORCE) + +include(${PICO_SDK_INIT_CMAKE_FILE}) diff --git a/swerve_pico/_OLD/swerve_decoder/CMakeLists.txt b/swerve_pico/_OLD/swerve_decoder/CMakeLists.txt new file mode 100644 index 0000000..914fc65 --- /dev/null +++ b/swerve_pico/_OLD/swerve_decoder/CMakeLists.txt @@ -0,0 +1,55 @@ +# Generated Cmake Pico project file + +cmake_minimum_required(VERSION 3.5) + +set(CMAKE_C_STANDARD 11) +set(CMAKE_CXX_STANDARD 17) + +# Initialise pico_sdk from installed location +# (note this can come from environment, CMake cache etc) + +set(PICO_BOARD pico CACHE STRING "Board type") + +# Pull in Raspberry Pi Pico SDK (must be before project) +include(pico_sdk_import.cmake) + +if (PICO_SDK_VERSION_STRING VERSION_LESS "1.4.0") + message(FATAL_ERROR "Raspberry Pi Pico SDK version 1.4.0 (or later) required. Your version is ${PICO_SDK_VERSION_STRING}") +endif() + +project(i2c_drive_pico C CXX ASM) + +# Initialise the Raspberry Pi Pico SDK +pico_sdk_init() + +# Add executable. Default name is the project name, version 0.1 + +add_executable(i2c_drive_pico i2c_drive_pico.c ) + +pico_set_program_name(i2c_drive_pico "i2c_drive_pico") +pico_set_program_version(i2c_drive_pico "0.1") + +pico_enable_stdio_uart(i2c_drive_pico 0) +pico_enable_stdio_usb(i2c_drive_pico 1) + +# Add the standard library to the build +target_link_libraries(i2c_drive_pico + pico_stdlib + pico_i2c_slave + hardware_i2c + hardware_pwm + ) + +# Add the standard include files to the build +target_include_directories(i2c_drive_pico PRIVATE + ${CMAKE_CURRENT_LIST_DIR} + ${CMAKE_CURRENT_LIST_DIR}/.. # for our common lwipopts or any other standard includes, if required +) + +# Add any user requested libraries +target_link_libraries(i2c_drive_pico + hardware_i2c + ) + +pico_add_extra_outputs(i2c_drive_pico) + diff --git a/swerve_pico/_OLD/swerve_decoder/pico_sdk_import.cmake b/swerve_pico/_OLD/swerve_decoder/pico_sdk_import.cmake new file mode 100644 index 0000000..65f8a6f --- /dev/null +++ b/swerve_pico/_OLD/swerve_decoder/pico_sdk_import.cmake @@ -0,0 +1,73 @@ +# This is a copy of /external/pico_sdk_import.cmake + +# This can be dropped into an external project to help locate this SDK +# It should be include()ed prior to project() + +if (DEFINED ENV{PICO_SDK_PATH} AND (NOT PICO_SDK_PATH)) + set(PICO_SDK_PATH $ENV{PICO_SDK_PATH}) + message("Using PICO_SDK_PATH from environment ('${PICO_SDK_PATH}')") +endif () + +if (DEFINED ENV{PICO_SDK_FETCH_FROM_GIT} AND (NOT PICO_SDK_FETCH_FROM_GIT)) + set(PICO_SDK_FETCH_FROM_GIT $ENV{PICO_SDK_FETCH_FROM_GIT}) + message("Using PICO_SDK_FETCH_FROM_GIT from environment ('${PICO_SDK_FETCH_FROM_GIT}')") +endif () + +if (DEFINED ENV{PICO_SDK_FETCH_FROM_GIT_PATH} AND (NOT PICO_SDK_FETCH_FROM_GIT_PATH)) + set(PICO_SDK_FETCH_FROM_GIT_PATH $ENV{PICO_SDK_FETCH_FROM_GIT_PATH}) + message("Using PICO_SDK_FETCH_FROM_GIT_PATH from environment ('${PICO_SDK_FETCH_FROM_GIT_PATH}')") +endif () + +set(PICO_SDK_PATH "${PICO_SDK_PATH}" CACHE PATH "Path to the Raspberry Pi Pico SDK") +set(PICO_SDK_FETCH_FROM_GIT "${PICO_SDK_FETCH_FROM_GIT}" CACHE BOOL "Set to ON to fetch copy of SDK from git if not otherwise locatable") +set(PICO_SDK_FETCH_FROM_GIT_PATH "${PICO_SDK_FETCH_FROM_GIT_PATH}" CACHE FILEPATH "location to download SDK") + +if (NOT PICO_SDK_PATH) + if (PICO_SDK_FETCH_FROM_GIT) + include(FetchContent) + set(FETCHCONTENT_BASE_DIR_SAVE ${FETCHCONTENT_BASE_DIR}) + if (PICO_SDK_FETCH_FROM_GIT_PATH) + get_filename_component(FETCHCONTENT_BASE_DIR "${PICO_SDK_FETCH_FROM_GIT_PATH}" REALPATH BASE_DIR "${CMAKE_SOURCE_DIR}") + endif () + # GIT_SUBMODULES_RECURSE was added in 3.17 + if (${CMAKE_VERSION} VERSION_GREATER_EQUAL "3.17.0") + FetchContent_Declare( + pico_sdk + GIT_REPOSITORY https://github.com/raspberrypi/pico-sdk + GIT_TAG master + GIT_SUBMODULES_RECURSE FALSE + ) + else () + FetchContent_Declare( + pico_sdk + GIT_REPOSITORY https://github.com/raspberrypi/pico-sdk + GIT_TAG master + ) + endif () + + if (NOT pico_sdk) + message("Downloading Raspberry Pi Pico SDK") + FetchContent_Populate(pico_sdk) + set(PICO_SDK_PATH ${pico_sdk_SOURCE_DIR}) + endif () + set(FETCHCONTENT_BASE_DIR ${FETCHCONTENT_BASE_DIR_SAVE}) + else () + message(FATAL_ERROR + "SDK location was not specified. Please set PICO_SDK_PATH or set PICO_SDK_FETCH_FROM_GIT to on to fetch from git." + ) + endif () +endif () + +get_filename_component(PICO_SDK_PATH "${PICO_SDK_PATH}" REALPATH BASE_DIR "${CMAKE_BINARY_DIR}") +if (NOT EXISTS ${PICO_SDK_PATH}) + message(FATAL_ERROR "Directory '${PICO_SDK_PATH}' not found") +endif () + +set(PICO_SDK_INIT_CMAKE_FILE ${PICO_SDK_PATH}/pico_sdk_init.cmake) +if (NOT EXISTS ${PICO_SDK_INIT_CMAKE_FILE}) + message(FATAL_ERROR "Directory '${PICO_SDK_PATH}' does not appear to contain the Raspberry Pi Pico SDK") +endif () + +set(PICO_SDK_PATH ${PICO_SDK_PATH} CACHE PATH "Path to the Raspberry Pi Pico SDK" FORCE) + +include(${PICO_SDK_INIT_CMAKE_FILE}) diff --git a/swerve_pico/_OLD/swerve_decoder/swerve_decoder.cpp b/swerve_pico/_OLD/swerve_decoder/swerve_decoder.cpp new file mode 100644 index 0000000..d5bc4ed --- /dev/null +++ b/swerve_pico/_OLD/swerve_decoder/swerve_decoder.cpp @@ -0,0 +1,38 @@ +#include +#include +#include +#include +#include +#include + +// use https://www.h-schmidt.net/FloatConverter/IEEE754.html to convert between float and binary/hex + +// small class to store data from read buffer, it converts ints to floats mainly just here to abstract c code +class Decoder { + public: + Decoder(int size){ // initialize class, set size, allocate storage + this->size = size; + float array[size]; + this->floats = array; + for(int i=0;isize; + for(int i=0;ifloats+(i)) = *(((float*) flipped_binary_array)+(size-1-i)); + } + float* GetValues() { + return floats; + } + + private: + float* floats; + int size; +};//put that into pico comms have pico comms d odecoding \ No newline at end of file diff --git a/swerve_pico/_OLD/zeroing_script/CMakeLists.txt b/swerve_pico/_OLD/zeroing_script/CMakeLists.txt new file mode 100644 index 0000000..914fc65 --- /dev/null +++ b/swerve_pico/_OLD/zeroing_script/CMakeLists.txt @@ -0,0 +1,55 @@ +# Generated Cmake Pico project file + +cmake_minimum_required(VERSION 3.5) + +set(CMAKE_C_STANDARD 11) +set(CMAKE_CXX_STANDARD 17) + +# Initialise pico_sdk from installed location +# (note this can come from environment, CMake cache etc) + +set(PICO_BOARD pico CACHE STRING "Board type") + +# Pull in Raspberry Pi Pico SDK (must be before project) +include(pico_sdk_import.cmake) + +if (PICO_SDK_VERSION_STRING VERSION_LESS "1.4.0") + message(FATAL_ERROR "Raspberry Pi Pico SDK version 1.4.0 (or later) required. Your version is ${PICO_SDK_VERSION_STRING}") +endif() + +project(i2c_drive_pico C CXX ASM) + +# Initialise the Raspberry Pi Pico SDK +pico_sdk_init() + +# Add executable. Default name is the project name, version 0.1 + +add_executable(i2c_drive_pico i2c_drive_pico.c ) + +pico_set_program_name(i2c_drive_pico "i2c_drive_pico") +pico_set_program_version(i2c_drive_pico "0.1") + +pico_enable_stdio_uart(i2c_drive_pico 0) +pico_enable_stdio_usb(i2c_drive_pico 1) + +# Add the standard library to the build +target_link_libraries(i2c_drive_pico + pico_stdlib + pico_i2c_slave + hardware_i2c + hardware_pwm + ) + +# Add the standard include files to the build +target_include_directories(i2c_drive_pico PRIVATE + ${CMAKE_CURRENT_LIST_DIR} + ${CMAKE_CURRENT_LIST_DIR}/.. # for our common lwipopts or any other standard includes, if required +) + +# Add any user requested libraries +target_link_libraries(i2c_drive_pico + hardware_i2c + ) + +pico_add_extra_outputs(i2c_drive_pico) + diff --git a/swerve_pico/_OLD/zeroing_script/pico_sdk_import.cmake b/swerve_pico/_OLD/zeroing_script/pico_sdk_import.cmake new file mode 100644 index 0000000..65f8a6f --- /dev/null +++ b/swerve_pico/_OLD/zeroing_script/pico_sdk_import.cmake @@ -0,0 +1,73 @@ +# This is a copy of /external/pico_sdk_import.cmake + +# This can be dropped into an external project to help locate this SDK +# It should be include()ed prior to project() + +if (DEFINED ENV{PICO_SDK_PATH} AND (NOT PICO_SDK_PATH)) + set(PICO_SDK_PATH $ENV{PICO_SDK_PATH}) + message("Using PICO_SDK_PATH from environment ('${PICO_SDK_PATH}')") +endif () + +if (DEFINED ENV{PICO_SDK_FETCH_FROM_GIT} AND (NOT PICO_SDK_FETCH_FROM_GIT)) + set(PICO_SDK_FETCH_FROM_GIT $ENV{PICO_SDK_FETCH_FROM_GIT}) + message("Using PICO_SDK_FETCH_FROM_GIT from environment ('${PICO_SDK_FETCH_FROM_GIT}')") +endif () + +if (DEFINED ENV{PICO_SDK_FETCH_FROM_GIT_PATH} AND (NOT PICO_SDK_FETCH_FROM_GIT_PATH)) + set(PICO_SDK_FETCH_FROM_GIT_PATH $ENV{PICO_SDK_FETCH_FROM_GIT_PATH}) + message("Using PICO_SDK_FETCH_FROM_GIT_PATH from environment ('${PICO_SDK_FETCH_FROM_GIT_PATH}')") +endif () + +set(PICO_SDK_PATH "${PICO_SDK_PATH}" CACHE PATH "Path to the Raspberry Pi Pico SDK") +set(PICO_SDK_FETCH_FROM_GIT "${PICO_SDK_FETCH_FROM_GIT}" CACHE BOOL "Set to ON to fetch copy of SDK from git if not otherwise locatable") +set(PICO_SDK_FETCH_FROM_GIT_PATH "${PICO_SDK_FETCH_FROM_GIT_PATH}" CACHE FILEPATH "location to download SDK") + +if (NOT PICO_SDK_PATH) + if (PICO_SDK_FETCH_FROM_GIT) + include(FetchContent) + set(FETCHCONTENT_BASE_DIR_SAVE ${FETCHCONTENT_BASE_DIR}) + if (PICO_SDK_FETCH_FROM_GIT_PATH) + get_filename_component(FETCHCONTENT_BASE_DIR "${PICO_SDK_FETCH_FROM_GIT_PATH}" REALPATH BASE_DIR "${CMAKE_SOURCE_DIR}") + endif () + # GIT_SUBMODULES_RECURSE was added in 3.17 + if (${CMAKE_VERSION} VERSION_GREATER_EQUAL "3.17.0") + FetchContent_Declare( + pico_sdk + GIT_REPOSITORY https://github.com/raspberrypi/pico-sdk + GIT_TAG master + GIT_SUBMODULES_RECURSE FALSE + ) + else () + FetchContent_Declare( + pico_sdk + GIT_REPOSITORY https://github.com/raspberrypi/pico-sdk + GIT_TAG master + ) + endif () + + if (NOT pico_sdk) + message("Downloading Raspberry Pi Pico SDK") + FetchContent_Populate(pico_sdk) + set(PICO_SDK_PATH ${pico_sdk_SOURCE_DIR}) + endif () + set(FETCHCONTENT_BASE_DIR ${FETCHCONTENT_BASE_DIR_SAVE}) + else () + message(FATAL_ERROR + "SDK location was not specified. Please set PICO_SDK_PATH or set PICO_SDK_FETCH_FROM_GIT to on to fetch from git." + ) + endif () +endif () + +get_filename_component(PICO_SDK_PATH "${PICO_SDK_PATH}" REALPATH BASE_DIR "${CMAKE_BINARY_DIR}") +if (NOT EXISTS ${PICO_SDK_PATH}) + message(FATAL_ERROR "Directory '${PICO_SDK_PATH}' not found") +endif () + +set(PICO_SDK_INIT_CMAKE_FILE ${PICO_SDK_PATH}/pico_sdk_init.cmake) +if (NOT EXISTS ${PICO_SDK_INIT_CMAKE_FILE}) + message(FATAL_ERROR "Directory '${PICO_SDK_PATH}' does not appear to contain the Raspberry Pi Pico SDK") +endif () + +set(PICO_SDK_PATH ${PICO_SDK_PATH} CACHE PATH "Path to the Raspberry Pi Pico SDK" FORCE) + +include(${PICO_SDK_INIT_CMAKE_FILE}) diff --git a/swerve_pico/_OLD/zeroing_script/zeroing.cpp b/swerve_pico/_OLD/zeroing_script/zeroing.cpp new file mode 100644 index 0000000..586562c --- /dev/null +++ b/swerve_pico/_OLD/zeroing_script/zeroing.cpp @@ -0,0 +1,57 @@ +#include +#include +#include +#include +#include +#include + +#include "pid.cpp" + +class Zeroing { //store value of sensor + public: + Zeroing(int pin1, int pin2, int pin3, PID motor1, PID motor2, PID motor3) { + this->readPin1 = pin1; + this->readPin2 = pin2; + this->readPin3 = pin3; + this->motor1 = motor1; + this->motor2 = motor2; + this->motor3 = motor3; + } + void Setup() { + stdio_init_all(); + gpio_init(this->readPin); + gpio_set_dir(this->readPin, GPIO_IN); + } + int Read1() { + this->zerod = gpio_get(this->readPin1); + return this->zerod; + } + int Read2() { + this->zerod = gpio_get(this->readPin2); + return this->zerod; + } + int Read3() { + this->zerod = gpio_get(this->readPin3); + return this->zerod; + } + + void zero() { //put in here for now + while (!(this->Read1())) { + this->motor1.update(1); + } + while (!(this->Read2())) { + this->motor2.update(1); + } + while (!(this->Read3())) { + this->motor3.update(1); + } + } + private: + int readPin1; + int readPin2; + int readPin3; + int zerod = false; + PID motor1; + PID motor2; + PID motor3; +}; \ No newline at end of file diff --git a/swerve_pico/control-library/control-library.cpp b/swerve_pico/control-library/control-library.cpp new file mode 100644 index 0000000..d113b51 --- /dev/null +++ b/swerve_pico/control-library/control-library.cpp @@ -0,0 +1,121 @@ +#include "pico/stdlib.h" +#include "time.h" +#include "control-library.h" + +bool calculateCallback(repeating_timer_t *rt); +bool updateIntergralDerivativeCallback(repeating_timer_t *rt); + +PID::PID(float P, float I, float D, Motor motor, char tuningMode, int timestep, int substep) +{ + this->P = P; + this->I = I; + this->D = D; + this->motor = motor; + this->timestep = timestep; // timestep for updating output + this->substep = this->timestep / (float)substep; // how many times to update integral/derivative per timestep + this->tuningMode = tuningMode; +} + +void PID::Setup() +{ // PICO SPECIFIC: create repeating timer, setup callbacks + repeating_timer_t timer; + this->timer = timer; + add_repeating_timer_ms(this->timestep, calculateCallback, this, &timer); + add_repeating_timer_ms(this->substep, updateIntergralDerivativeCallback, this, &timer); + this->motor.Setup(); + this->active = true; +} + +void PID::updateTarget(float target) +{ + // Idea; pid loop is autonomous and will automatically do whatever due to repeating timers + // if we just update the target with a function call we might be okay + this->target = target; +} + +void PID::reset() +{ + this->output = 0; + this->target = 0; + this->lastError = 0; +} + +/*** + * setActive: sets flag to toggle function of pid callbacks + */ +void PID::setActive(bool active) +{ + this->active = active; +} + +void PID::calculate() +{ + if (this->active) + { + this->output = this->P * this->target + this->I * this->Integral + this->D * this->Derivative; + this->motor.SetPwmPercentage(output); + } +} + +void PID::updateErrorIntegralDerivative() +{ + if (this->active) + { + float error; // error term + if (this->tuningMode = PID_TUNING_MODE_POSITION) + { + error = this->motor.GetPosition() - this->target; + } + else + { + error = this->motor.GetVelocity() - this->target; + } + + // substep is in miliseconds + this->Integral += error * (this->substep / MS_IN_SEC); // left riemann sum at fixed substep + this->Derivative = (error - this->lastError) / (this->substep / MS_IN_SEC); // estimate of derivatvie at fixed substep + this->lastError = error; + } +} + +bool calculateCallback(repeating_timer_t *rt) +{ + ((PID *)rt->user_data)->calculate(); + return true; +}; + +bool updateIntergralDerivativeCallback(repeating_timer_t *rt) +{ + ((PID *)rt->user_data)->updateErrorIntegralDerivative(); + return true; +}; + +SwerveDrive::SwerveDrive(PID turnPID, PID drivePID) +{ + this->turnPID = turnPID; + this->drivePID = drivePID; +} + +void SwerveDrive::Setup() +{ + this->turnPID.Setup(); + this->drivePID.Setup(); +} + +void SwerveDrive::SetPidTargets(float turnMotorPercentage, float wheelMotorPercentage) +{ + this->turnPID.updateTarget(turnMotorPercentage); + this->drivePID.updateTarget(wheelMotorPercentage); +} + +void SwerveDrive::TogglePids(bool value) +{ + this->turnPID.setActive(value); + this->drivePID.setActive(value); +} + +void SwerveDrive::ResetPids() +{ + this->turnPID.reset(); + this->drivePID.reset(); +} \ No newline at end of file diff --git a/swerve_pico/control-library/control-library.h b/swerve_pico/control-library/control-library.h new file mode 100644 index 0000000..dbee8f4 --- /dev/null +++ b/swerve_pico/control-library/control-library.h @@ -0,0 +1,63 @@ +#include +#include + +#include "motor-library.h" + +#define MS_IN_SEC 1000 // DOUBLE CHECK VALUE OF CONVERSION + +enum PIDTuningMode +{ + PID_TUNING_MODE_POSITION, + PID_TUNING_MODE_VELOCITY +}; + + +class PID +{ // pid loop +public: + PID(); + PID(float P, float I, float D, Motor motor, char tuningMode = PID_TUNING_MODE_POSITION, int timestep = 100, int substep = 10); + void Setup(); + void updateTarget(float target); + void reset(); + void setActive(bool active); + + void calculate(); + void updateErrorIntegralDerivative(); +private: + + float P; + float I; + float D; + + float target = 0; + float output = 0; + float input = 0; + float lastError = 0; + + float Integral = 0; + float Derivative = 0; + repeating_timer_t timer = {}; + int timestep; + int substep; + Motor motor; + char tuningMode; + bool active = false; +}; + +/** + * Class to hold two pids, represents one swerve module (one turn wheel and one drive wheel) + */ +class SwerveDrive +{ +public: + SwerveDrive(PID turnPID, PID drivePID); + void Setup(); + void SetPidTargets(float turnMotorPercentage, float wheelMotorPercentage); + void TogglePids(bool value); + void ResetPids(); + +private: + PID turnPID; + PID drivePID; +}; diff --git a/swerve_pico/messaging-library/messaging-library.cpp b/swerve_pico/messaging-library/messaging-library.cpp new file mode 100644 index 0000000..9703a14 --- /dev/null +++ b/swerve_pico/messaging-library/messaging-library.cpp @@ -0,0 +1,155 @@ +#include "messaging-library.h" + + +char MessagingBSDChecksum(char *payloadString, unsigned char payloadLen) +{ + unsigned char checksum = 0; + for (short i = 0; i < payloadLen; i++) + { + checksum = (checksum >> 1) | (checksum << 7); + checksum += payloadString[i]; + } + return checksum; +} + + +char MessagingReadBuffer(char *bufferString, char *resultString, int bufferLength, char *robotState) +{ // turn result to struct later + short idx = 0; + char state = MESSAGE_HEAD; + char currentChar = bufferString[idx]; + char payloadLen = 0; + char payloadIdx = 0; + char messageID = 0; + char calculatedChecksum = 0; + char messageParseResult = MESSAGE_PARSE_NONE; + char robotResultState = 0; + + while (idx < bufferLength) + { + if (state == MESSAGE_HEAD) + { + if (currentChar == (char)HEAD_ID) + { + state = MESSAGE_LENGTH; + } + } + else if (state == MESSAGE_LENGTH) + { + payloadLen = currentChar; + + // expected payload range check + if (payloadLen < 2) + { + state = MESSAGE_ERROR; + } + + payloadIdx = 0; + char *payload = bufferString + idx + 1; + calculatedChecksum = MessagingBSDChecksum(payload, payloadLen); + state = MESSAGE_PAYLOAD; + } + else if (state == MESSAGE_PAYLOAD) + { + + // assign/store id + if (payloadIdx == 0) + { + messageID = currentChar; + } + + // actual payload length check + if (payloadIdx < payloadLen - 1) + { + if (currentChar == (char)TAIL_ID) + { + state = MESSAGE_ERROR; + } + else + { + payloadIdx++; + } + } + else + { + if (currentChar == (char)TAIL_ID) + { + state = MESSAGE_ERROR; + } + else + { + robotResultState = MessagingProcessPayload(bufferString + idx - payloadLen + 2, messageID, payloadLen - 1); + state = MESSAGE_TAIL; + } + } + } + else if (state == MESSAGE_TAIL) + { + if (currentChar == (char)TAIL_ID) + { + state = MESSAGE_CHECKSUM; + } + else + { + state = MESSAGE_ERROR; + } + } + else if (state == MESSAGE_CHECKSUM) + { + if (calculatedChecksum != currentChar) + { + state = MESSAGE_ERROR; + } + else + { + state = MESSAGE_END; + } + } + else if (state == MESSAGE_END) + { + char nextChar = bufferString[idx + 1]; + if (currentChar == '\r' && nextChar == '\n') + { + messageParseResult = MESSAGE_PARSE_SUCCESS; + state = MESSAGE_HEAD; + *robotState = robotResultState; + break; + } + else + { + state = MESSAGE_ERROR; + } + } + else + { + messageParseResult = MESSAGE_PARSE_FAILURE; + break; + } + + idx++; + currentChar = bufferString[idx]; + } + + if (state != MESSAGE_HEAD && state != MESSAGE_ERROR) + { + messageParseResult = MESSAGE_BUFFER_OVERFLOW; + } + + return messageParseResult; +} + +void MessagingWriteMessage(unsigned char payload[], char message[]) +{ + char payloadLen = 0; + for (; payload[payloadLen] != '\0'; payloadLen++) + { + message[payloadLen + 2] = payload[payloadLen]; + } + message[0] = (char)HEAD_ID; + message[1] = payloadLen; + message[payloadLen + 2] = (char)TAIL_ID; + message[payloadLen + 3] = MessagingBSDChecksum((char*)payload, payloadLen); + message[payloadLen + 4] = '\r'; + message[payloadLen + 5] = '\n'; + message[payloadLen + 6] = '\0'; +} diff --git a/swerve_pico/messaging-library/messaging-library.h b/swerve_pico/messaging-library/messaging-library.h new file mode 100644 index 0000000..9b8b993 --- /dev/null +++ b/swerve_pico/messaging-library/messaging-library.h @@ -0,0 +1,68 @@ +#include "swerve-module-id.h" + +#define PAYLOAD_MAX_LEN 127 +#define HEAD_ID 0xCC +#define TAIL_ID 0xB9 + +enum messageParseResult +{ + MESSAGE_BUFFER_OVERFLOW = -2, + MESSAGE_PARSE_FAILURE, + MESSAGE_PARSE_NONE, + MESSAGE_PARSE_SUCCESS +}; + +enum messageReadState +{ + MESSAGE_ERROR = -1, + MESSAGE_HEAD, + MESSAGE_LENGTH, + MESSAGE_PAYLOAD, + MESSAGE_TAIL, + MESSAGE_CHECKSUM, + MESSAGE_END +}; + +/** + * BSDChecksum: Calculates checksum for messaging, using BSDChecksum algorithm. The max length is 128 bytes. The first byte is the message ID, the next 127 bytes is the payload + * + * @param payloadString string of payload and id, 128 bytes max + * @param payloadLen length of payload with id + * + * @return calculated checksum + */ +char MessagingBSDChecksum(char *payloadString, unsigned char payloadLen); +/** + * ReadBuffer: Uses state machine to take the buffer parameter and processes the message into the result string; + * Returns true/false based on if conversion was successful. Changes value of robotState if message is valid. + * + * \nPacket Structure: + * HEAD LENGTH PAYLOAD TAIL CHECKSUM END + * 1 1 (1)<128 1 1 \r\n + * + * Head: 1 byte 0xCC + * Length: 1 byte, length of payload in bytes + * Payload: Variable length < 128 bytes, always proceeded by a 1 byte ID + * Tail: 1 byte 0xB9 + * Checksum: 1 byte checksum calculated over payload using BSDChecksum algorithm + * End: 2 bytes \r\n + * + * @param bufferString string that will be processed + * @param resultString string that the result is stored in + * @param bufferLength length of buffer function parses through + * @param robotState pointer to variable containing state of robot that is modified + * when a message is fully processed + * + * @return true or false based on success of function + */ +char MessagingReadBuffer(char *bufferString, char *resultString, int bufferLength, char *robotState); +/** + * WriteMessage: converts parameter payload into a valid message string and writes it to parameter message. + * Payload corresponds to PAYLOAD in the packet structure: [HEAD, LENGTH, PAYLOAD, TAIL, CHECKSUM, END] + * + * @param payload string that will be converted to a valid message format + * @param message string that the message will be written to + * + * @return None + */ +void MessagingWriteMessage(unsigned char payload[], char message[]); diff --git a/swerve_pico/messaging-library/swerve-module-id.h b/swerve_pico/messaging-library/swerve-module-id.h new file mode 100644 index 0000000..3284e1a --- /dev/null +++ b/swerve_pico/messaging-library/swerve-module-id.h @@ -0,0 +1,54 @@ +/** + * Defined list of valid message ids and helper function to check and process messages into actions + */ +#ifndef _MESSAGING_IDS +#define _MESSAGING_IDS + +#define MOVE_IN_DIRECTION 0x80 +#define HEARTBEAT_CHECK 0x81 + +#define resultArraySize 6 + +enum messageProcessResult +{ + PAYLOAD_ID_NOT_FOUND, + STATE_READ_RESULT_ARRAY, + STATE_ZERO_MOTORS, + STATE_WRITE_HEARTBEAT, + STATE_RETURN_DATA +}; + +float resultArray[resultArraySize] = {0}; +char flippedBinaryArray[sizeof(float) * resultArraySize] = {0}; +/** + * ProcessPayload: reads parameter of specified length and performs actions based on the id + * + * @param payload pointer to beginning of payload + * @param id id of payload + * @param len length of payload + * + * @returns success/failure to process payload, next state of main state machine + */ +char MessagingProcessPayload(char *payload, unsigned char id, unsigned char len) +{ + switch (id) + { + case MOVE_IN_DIRECTION: + // use https://www.h-schmidt.net/FloatConverter/IEEE754.html to convert between float and binary/hex + // store buffer array of uint8 (or char) to n floats + // bytes are stored in the wrong direction, flip and rewrite + + for (char i = 0; i < len; i++) + flippedBinaryArray[i] = payload[len - 1 - i]; + + for (char i = 0; i < resultArraySize; i++) // write to floats (in reversed direction) + *(resultArray + (i)) = *(((float *)flippedBinaryArray) + (resultArraySize - 1 - i)); + + return STATE_READ_RESULT_ARRAY; + case HEARTBEAT_CHECK: + return STATE_WRITE_HEARTBEAT; + default: + return PAYLOAD_ID_NOT_FOUND; + }; +} +#endif \ No newline at end of file diff --git a/swerve_pico/motor-library/motor-library.cpp b/swerve_pico/motor-library/motor-library.cpp new file mode 100644 index 0000000..310ad10 --- /dev/null +++ b/swerve_pico/motor-library/motor-library.cpp @@ -0,0 +1,166 @@ +#include +#include +#include +#include + +#include "pico/stdlib.h" +#include "hardware/pio.h" +#include "hardware/timer.h" +#include "hardware/pwm.h" + +#include "motor-library.h" +#include "quadrature_encoder.pio.h" + +static EncoderFactory factory; + +/** + * file contains syntactic sugar to make getting/setting motors easier + * created as a lazy way to not have to combine all 3 already working files :/ + */ + +/** + * Motor class that holds PWMControl and quadrature_encoder class, encapsulates functions to set and read values of 1 motor + */ +Motor::Motor(int motorPin, int pwmPin, unsigned int encoderPin, float ratio, int chan, int countMax) +{ + this->pwmControl = PWMControl(motorPin, pwmPin, chan, countMax); + this->encoder = factory.createEncoder(encoderPin, ratio); +} + +void Motor::Setup() +{ + this->pwmControl.Setup(); +} + +void Motor::SetPwmPercentage(float percentage) +{ + this->pwmControl.Drive(percentage); +} + +float Motor::GetPosition() +{ + return this->encoder.get_pos(); +} + +float Motor::GetVelocity() +{ + return this->encoder.get_velocity(); +} + +PWMControl::PWMControl(int motorPin, int pwmPin, int chan, int countMax) +{ + // two motors per module, each requiring 3 pins + // two pins to define h-bridge control and one pin for pwm (since there are 2 motors, there are 2 pwm pins) + // constructor assumes that all pins are in sets of 2 and will be next to eachother on an even interval (e.g pin 0 and 1, pin 2 and 4, etc) + this->motor_in1_pin = motorPin; // pin set + this->motor_in2_pin = motorPin + 1; + this->pwm_pin = pwmPin; // pwm pin set + // pwm slices take up two gpio pins, assuming you take up two pins for pwm with the first pin being even, + // the slice is whats activated/outputs a signal and is separate from the pins + // the pwm pins must be configured to let the signal pass through + this->pwm_slice = pwm_gpio_to_slice_num(pwmPin); + this->channel = chan; + // code to check & override default values + if (countMax != COUNT_MAX) + this->countMax = countMax; +} + +// initialize all pins +void PWMControl::Setup() +{ // PICO SPECIFIC + stdio_init_all(); + // h-bridge requires two binary signals for direction, initialize the in1, in2 pins + gpio_init(this->motor_in1_pin); + gpio_init(this->motor_in2_pin); + + // TODO: check if default output signal is 0, for now put this in + // sets all output signals to 0 + gpio_put(this->motor_in1_pin, 0); + gpio_put(this->motor_in2_pin, 0); + + // define pin output for gpio pins + gpio_set_dir(this->motor_in1_pin, GPIO_OUT); + gpio_set_dir(this->motor_in2_pin, GPIO_OUT); + + // define pin output for pwm pins + gpio_set_function(this->pwm_pin, GPIO_FUNC_PWM); + + // set max cycle count of pwm slice (this does counting) + pwm_set_wrap(this->pwm_slice, COUNT_MAX); + + // set output signal to 0 + // pwm pins can output anything from 0 to countmax of the pwm_slice (this outputs based on the counter) + pwm_set_chan_level(this->pwm_slice, this->channel, 0); + + // activate pwm slice + pwm_set_enabled(this->pwm_slice, true); +} + +// drive function control entirely by PID loops +void PWMControl::Drive(float drivePercentange) +{ // currently assuming turn & wheel are from -1 to 1, probably need to change later + if (drivePercentange == 0) + { // in1 and in2 are high + gpio_put(motor_in1_pin, 1); + gpio_put(motor_in2_pin, 1); + } + else if (drivePercentange < 0) + { // in1 is high and in2 is low + gpio_put(motor_in1_pin, 1); + gpio_put(motor_in2_pin, 0); + } + else + { // in1 is low and in2 is high + gpio_put(motor_in2_pin, 1); + gpio_put(motor_in1_pin, 0); + } + + // set pwm pin output as % of slice output + pwm_set_chan_level(pwm_slice, channel, abs((int)(drivePercentange * COUNT_MAX))); +} + + +// Create an encoder. Reccommended NOT to use this class, use EncoderFactory::createEncoder() +// @param pinA the pin that encoder A channel is connected to, the B channel should connect to the next pin +// @param sm the state machine to keep track of the encoder, 0-3 +// @param which pio +// @param ratio the ratio by which to multiply encoder ticks +Encoder::Encoder(uint pinA, uint sm, PIO pio, float ratio, bool addProgram) +{ + this->pio = pio; + this->sm = sm; + this->ratio = ratio; + + uint offset = 0; + + // we don't really need to keep the offset, as this program must be loaded + // at offset 0 + if (addProgram) + uint offset = pio_add_program(pio, &quadrature_encoder_program); + + quadrature_encoder_program_init(pio, sm, offset, pinA, 0); +} + +// updates the pos and velocity, call periodically. +// @param delta_time the time, in miliseconds, since last calling update +void Encoder::update(int delta_time) +{ + pos = quadrature_encoder_get_count(pio, sm) * ratio * DEG_PER_ROT; + velocity = ((prev_pos - pos) / delta_time) * 1000; + prev_pos = pos; +} + +// get position of wheel in ticks, multiplied by any provided ratio. resets on init. +// update() must be called periodically for this to be accurate +float Encoder::get_pos() +{ + return pos; +} + +// get velocity of wheel in ticks per second, multiplied by any provided ratio. +// update() must be called periodically for this to be accurate +float Encoder::get_velocity() +{ + return velocity; +} + diff --git a/swerve_pico/motor-library/motor-library.h b/swerve_pico/motor-library/motor-library.h new file mode 100644 index 0000000..2bdccce --- /dev/null +++ b/swerve_pico/motor-library/motor-library.h @@ -0,0 +1,87 @@ +// #pragma once +#include +#include +#include "quadrature_encoder.pio.h" +#include + +#define COUNT_MAX 65535 + +const float ROT_PER_TICK = 1.0 / (4 * 374.0); +const float PULLEY_RATIO = 0.3185 / 1.528; +const float DEG_PER_ROT = 360.0; + + + +class PWMControl +{ +public: + PWMControl(); + PWMControl(int motorPin, int pwmPin, int chan = PWM_CHAN_A, int countMax = COUNT_MAX); + void Setup(); + void Drive(float drivePercentange); + +private: + int motor_in1_pin; + int motor_in2_pin; + int pwm_pin; + int pwm_slice; + int channel = PWM_CHAN_A; + int countMax = COUNT_MAX; +}; + + +class Encoder +{ +public: + Encoder(); + Encoder(unsigned int pinA, unsigned int sm, PIO pio, float ratio = 1.0, bool addProgram = true); + void update(int delta_time); + float get_pos(); + float get_velocity(); + +private: + float prev_pos, pos; + float velocity; + float ratio; + PIO pio; + unsigned int sm; +}; + +class EncoderFactory +{ +public: + // Create an encoder, automatically configuring the state machine and pio. + // @param pinA the A encoder channel, the B channel should be connected to the next pin + // @param ratio the ratio by which to multiply encoder outputs. ratio of 1 results in tick / sec + static Encoder createEncoder(unsigned int pinA, float ratio = 1.0) + { + if (encoder_count > 7) + { + throw std::out_of_range("reached encoder limit of 8"); + } + + unsigned int sm = encoder_count % 4; + PIO pio = encoder_count < 4 ? pio0 : pio1; + + encoder_count++; + return Encoder(pinA, sm, pio, ratio, sm == 0); + } + +private: + static unsigned int encoder_count; +}; + +class Motor +{ +public: + Motor(); + Motor(int motorPin, int pwmPin, unsigned int encoderPin, float ratio = 1.0, int chan = PWM_CHAN_A, int countMax = COUNT_MAX); + void Setup(); + void SetPwmPercentage(float percentage); + float GetPosition(); + float GetVelocity(); + +private: + PWMControl pwmControl; + Encoder encoder; +}; \ No newline at end of file diff --git a/swerve_pico/motor-library/quadrature_encoder.pio b/swerve_pico/motor-library/quadrature_encoder.pio new file mode 100644 index 0000000..8b1e618 --- /dev/null +++ b/swerve_pico/motor-library/quadrature_encoder.pio @@ -0,0 +1,141 @@ +; +; Copyright (c) 2023 Raspberry Pi (Trading) Ltd. +; +; SPDX-License-Identifier: BSD-3-Clause +; + +.program quadrature_encoder + +; the code must be loaded at address 0, because it uses computed jumps +.origin 0 + + +; the code works by running a loop that continuously shifts the 2 phase pins into +; ISR and looks at the lower 4 bits to do a computed jump to an instruction that +; does the proper "do nothing" | "increment" | "decrement" action for that pin +; state change (or no change) + +; ISR holds the last state of the 2 pins during most of the code. The Y register +; keeps the current encoder count and is incremented / decremented according to +; the steps sampled + +; the program keeps trying to write the current count to the RX FIFO without +; blocking. To read the current count, the user code must drain the FIFO first +; and wait for a fresh sample (takes ~4 SM cycles on average). The worst case +; sampling loop takes 10 cycles, so this program is able to read step rates up +; to sysclk / 10 (e.g., sysclk 125MHz, max step rate = 12.5 Msteps/sec) + +; 00 state + JMP update ; read 00 + JMP decrement ; read 01 + JMP increment ; read 10 + JMP update ; read 11 + +; 01 state + JMP increment ; read 00 + JMP update ; read 01 + JMP update ; read 10 + JMP decrement ; read 11 + +; 10 state + JMP decrement ; read 00 + JMP update ; read 01 + JMP update ; read 10 + JMP increment ; read 11 + +; to reduce code size, the last 2 states are implemented in place and become the +; target for the other jumps + +; 11 state + JMP update ; read 00 + JMP increment ; read 01 +decrement: + ; note: the target of this instruction must be the next address, so that + ; the effect of the instruction does not depend on the value of Y. The + ; same is true for the "JMP X--" below. Basically "JMP Y--, " + ; is just a pure "decrement Y" instruction, with no other side effects + JMP Y--, update ; read 10 + + ; this is where the main loop starts +.wrap_target +update: + MOV ISR, Y ; read 11 + PUSH noblock + +sample_pins: + ; we shift into ISR the last state of the 2 input pins (now in OSR) and + ; the new state of the 2 pins, thus producing the 4 bit target for the + ; computed jump into the correct action for this state. Both the PUSH + ; above and the OUT below zero out the other bits in ISR + OUT ISR, 2 + IN PINS, 2 + + ; save the state in the OSR, so that we can use ISR for other purposes + MOV OSR, ISR + ; jump to the correct state machine action + MOV PC, ISR + + ; the PIO does not have a increment instruction, so to do that we do a + ; negate, decrement, negate sequence +increment: + MOV Y, ~Y + JMP Y--, increment_cont +increment_cont: + MOV Y, ~Y +.wrap ; the .wrap here avoids one jump instruction and saves a cycle too + + + +% c-sdk { + +#include "hardware/clocks.h" +#include "hardware/gpio.h" + +// max_step_rate is used to lower the clock of the state machine to save power +// if the application doesn't require a very high sampling rate. Passing zero +// will set the clock to the maximum + +static inline void quadrature_encoder_program_init(PIO pio, uint sm, uint offset, uint pin, int max_step_rate) +{ + pio_sm_set_consecutive_pindirs(pio, sm, pin, 2, false); + gpio_pull_up(pin); + gpio_pull_up(pin + 1); + + pio_sm_config c = quadrature_encoder_program_get_default_config(offset); + + sm_config_set_in_pins(&c, pin); // for WAIT, IN + sm_config_set_jmp_pin(&c, pin); // for JMP + // shift to left, autopull disabled + sm_config_set_in_shift(&c, false, false, 32); + // don't join FIFO's + sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_NONE); + + // passing "0" as the sample frequency, + if (max_step_rate == 0) { + sm_config_set_clkdiv(&c, 1.0); + } else { + // one state machine loop takes at most 10 cycles + float div = (float)clock_get_hz(clk_sys) / (10 * max_step_rate); + sm_config_set_clkdiv(&c, div); + } + + pio_sm_init(pio, sm, offset, &c); + pio_sm_set_enabled(pio, sm, true); +} + +static inline int32_t quadrature_encoder_get_count(PIO pio, uint sm) +{ + uint ret; + int n; + + // if the FIFO has N entries, we fetch them to drain the FIFO, + // plus one entry which will be guaranteed to not be stale + n = pio_sm_get_rx_fifo_level(pio, sm) + 1; + while (n > 0) { + ret = pio_sm_get_blocking(pio, sm); + n--; + } + return ret; +} + +%} diff --git a/swerve_pico/pico_sdk_import.cmake b/swerve_pico/pico_sdk_import.cmake new file mode 100644 index 0000000..65f8a6f --- /dev/null +++ b/swerve_pico/pico_sdk_import.cmake @@ -0,0 +1,73 @@ +# This is a copy of /external/pico_sdk_import.cmake + +# This can be dropped into an external project to help locate this SDK +# It should be include()ed prior to project() + +if (DEFINED ENV{PICO_SDK_PATH} AND (NOT PICO_SDK_PATH)) + set(PICO_SDK_PATH $ENV{PICO_SDK_PATH}) + message("Using PICO_SDK_PATH from environment ('${PICO_SDK_PATH}')") +endif () + +if (DEFINED ENV{PICO_SDK_FETCH_FROM_GIT} AND (NOT PICO_SDK_FETCH_FROM_GIT)) + set(PICO_SDK_FETCH_FROM_GIT $ENV{PICO_SDK_FETCH_FROM_GIT}) + message("Using PICO_SDK_FETCH_FROM_GIT from environment ('${PICO_SDK_FETCH_FROM_GIT}')") +endif () + +if (DEFINED ENV{PICO_SDK_FETCH_FROM_GIT_PATH} AND (NOT PICO_SDK_FETCH_FROM_GIT_PATH)) + set(PICO_SDK_FETCH_FROM_GIT_PATH $ENV{PICO_SDK_FETCH_FROM_GIT_PATH}) + message("Using PICO_SDK_FETCH_FROM_GIT_PATH from environment ('${PICO_SDK_FETCH_FROM_GIT_PATH}')") +endif () + +set(PICO_SDK_PATH "${PICO_SDK_PATH}" CACHE PATH "Path to the Raspberry Pi Pico SDK") +set(PICO_SDK_FETCH_FROM_GIT "${PICO_SDK_FETCH_FROM_GIT}" CACHE BOOL "Set to ON to fetch copy of SDK from git if not otherwise locatable") +set(PICO_SDK_FETCH_FROM_GIT_PATH "${PICO_SDK_FETCH_FROM_GIT_PATH}" CACHE FILEPATH "location to download SDK") + +if (NOT PICO_SDK_PATH) + if (PICO_SDK_FETCH_FROM_GIT) + include(FetchContent) + set(FETCHCONTENT_BASE_DIR_SAVE ${FETCHCONTENT_BASE_DIR}) + if (PICO_SDK_FETCH_FROM_GIT_PATH) + get_filename_component(FETCHCONTENT_BASE_DIR "${PICO_SDK_FETCH_FROM_GIT_PATH}" REALPATH BASE_DIR "${CMAKE_SOURCE_DIR}") + endif () + # GIT_SUBMODULES_RECURSE was added in 3.17 + if (${CMAKE_VERSION} VERSION_GREATER_EQUAL "3.17.0") + FetchContent_Declare( + pico_sdk + GIT_REPOSITORY https://github.com/raspberrypi/pico-sdk + GIT_TAG master + GIT_SUBMODULES_RECURSE FALSE + ) + else () + FetchContent_Declare( + pico_sdk + GIT_REPOSITORY https://github.com/raspberrypi/pico-sdk + GIT_TAG master + ) + endif () + + if (NOT pico_sdk) + message("Downloading Raspberry Pi Pico SDK") + FetchContent_Populate(pico_sdk) + set(PICO_SDK_PATH ${pico_sdk_SOURCE_DIR}) + endif () + set(FETCHCONTENT_BASE_DIR ${FETCHCONTENT_BASE_DIR_SAVE}) + else () + message(FATAL_ERROR + "SDK location was not specified. Please set PICO_SDK_PATH or set PICO_SDK_FETCH_FROM_GIT to on to fetch from git." + ) + endif () +endif () + +get_filename_component(PICO_SDK_PATH "${PICO_SDK_PATH}" REALPATH BASE_DIR "${CMAKE_BINARY_DIR}") +if (NOT EXISTS ${PICO_SDK_PATH}) + message(FATAL_ERROR "Directory '${PICO_SDK_PATH}' not found") +endif () + +set(PICO_SDK_INIT_CMAKE_FILE ${PICO_SDK_PATH}/pico_sdk_init.cmake) +if (NOT EXISTS ${PICO_SDK_INIT_CMAKE_FILE}) + message(FATAL_ERROR "Directory '${PICO_SDK_PATH}' does not appear to contain the Raspberry Pi Pico SDK") +endif () + +set(PICO_SDK_PATH ${PICO_SDK_PATH} CACHE PATH "Path to the Raspberry Pi Pico SDK" FORCE) + +include(${PICO_SDK_INIT_CMAKE_FILE}) diff --git a/swerve_pico/sensor-library/sensor-library.cpp b/swerve_pico/sensor-library/sensor-library.cpp new file mode 100644 index 0000000..0c210c4 --- /dev/null +++ b/swerve_pico/sensor-library/sensor-library.cpp @@ -0,0 +1,21 @@ +#include +#include + +#include "sensor-library.h" + +ZeroingSensor::ZeroingSensor(int pin) +{ + this->readPin = pin; +} + +void ZeroingSensor::Setup() +{ + stdio_init_all(); + gpio_init(this->readPin); + gpio_set_dir(this->readPin, GPIO_IN); +} + +int ZeroingSensor::Read() +{ + gpio_get(this->readPin); +} \ No newline at end of file diff --git a/swerve_pico/sensor-library/sensor-library.h b/swerve_pico/sensor-library/sensor-library.h new file mode 100644 index 0000000..7daa008 --- /dev/null +++ b/swerve_pico/sensor-library/sensor-library.h @@ -0,0 +1,10 @@ +class ZeroingSensor +{ +public: + ZeroingSensor(int pin); + void Setup(); + int Read(); + +private: + int readPin; +}; \ No newline at end of file diff --git a/swerve_pico/swerve-pico.cpp b/swerve_pico/swerve-pico.cpp new file mode 100644 index 0000000..8f8b0b5 --- /dev/null +++ b/swerve_pico/swerve-pico.cpp @@ -0,0 +1,207 @@ +#include + +#include +#include +#include +#include + +#include "messaging-library.h" +#include "control-library.h" +#include "sensor-library.h" + +char robotState = 0; + + +// Define constants for I2C communication +#define I2C_PICO_ADDR 0x08 +#define I2C_SDA_PIN 0 +#define I2C_SCL_PIN 1 +#define I2C_PORT i2c0 +#define I2C_BAUDRATE 100 * 1000 + +// Define the length of the data packet +#define I2C_DATA_LENGTH 10 + +#define MESSAGE_START 0xFA +#define MESSAGE_STOP 0xFB + + +// digital low on in# pins indicates direction, both high is no signal +#define turn_in1_pin 4 // 1A, forward direction +#define turn_in2_pin 5 // 1B, backward direction + +// #define motor_pwm_pin 9 // 2A, 2B take up by motor speed +#define turn_pwm_pin 9 // 2A, turn motor speed +#define wheel_pwm_pin 8 // 2B, wheel motor speed +#define pwm_slice 4 +#define turn_channel PWM_CHAN_B +#define wheel_channel PWM_CHAN_A + +#define wheel_in1_pin 6 // 3A, forward direction +#define wheel_in2_pin 7 // 3B, backard direction + +// #define freq 500 // note: use clock management frequencies to set frequency +// #define duty_cycle 1 +#define count_max 65535 +//temporary address, make comm protocol to send/recieve messages upon linkage/ on bootup +#define PI_ADDRESS 1 + +// Buffer for incoming data +uint8_t incoming_data[I2C_DATA_LENGTH]; + +// Status of the input data +uint8_t input_status = 0; + +// Last event that occurred +int last_event = 0; + +// Index of the current data byte +int data_index = 0; + +// Buffer for the input data +uint8_t input[I2C_DATA_LENGTH - 2]; +uint8_t output[I2C_DATA_LENGTH - 2]; + +// Handler for I2C events +static void i2c_handler(i2c_inst_t *i2c, i2c_slave_event_t event) +{ + switch (event) + { + case I2C_SLAVE_RECEIVE:{ + // Read the data + uint8_t tmp = i2c_read_byte_raw(i2c); + // Check if the data is valid + // TODO: probably revert this back to the original, we don't really need the MESSAGE_START stuff + if ((incoming_data[0] == 0x00 && tmp != MESSAGE_START) || data_index >= I2C_DATA_LENGTH) + { + printf("Invalid data %x\n", tmp); + break; + } + // Store the data + incoming_data[data_index] = tmp; + // printf("Data: %d\n", incoming_data[data_index]); + data_index++; + // set the event status to received + last_event = 1; + break; + } + + + case I2C_SLAVE_REQUEST: // Pi is requesting data + // Write the data into the void + // i2c_write_byte_raw(i2c, (uint8_t)input_status); + // set the event status to sent + i2c_write_raw_blocking(i2c, output, I2C_DATA_LENGTH - 2); + last_event = 2; + break; + + case I2C_SLAVE_FINISH: // Pi has signalled Stop / Restart + // if the last event was a receive event and the data is valid + if (last_event == 1) + if (incoming_data[0] == MESSAGE_START && incoming_data[I2C_DATA_LENGTH - 1] == MESSAGE_STOP) + { + // move the data into the input array + for (int i = 0; i < I2C_DATA_LENGTH - 2; i++) + { + input[i] = (int)incoming_data[i + 1]; + } + // set the input status to ready + input_status = 1; + + // Reset incoming_data + for (int i = 0; i < I2C_DATA_LENGTH; i++) + { + incoming_data[i] = 0x00; + } + } + data_index = 0; + break; + default: + break; + } +} + + + +int main(){ + /** + * DEFINE AND SETUP MOTORS + */ + Motor turn_motor1 = Motor(0,0,0); + Motor drive_motor1 = Motor(0,0,0); + Motor turn_motor2 = Motor(0,0,0); + Motor drive_motor2 = Motor(0,0,0); + Motor turn_motor3 = Motor(0,0,0); + Motor drive_motor3 = Motor(0,0,0); + + PID pid1 = PID(0,0,0, turn_motor1); + PID pid2 = PID(0,0,0, drive_motor1); + PID pid3 = PID(0,0,0, turn_motor2); + PID pid4 = PID(0,0,0, drive_motor2); + PID pid5 = PID(0,0,0, turn_motor3); + PID pid6 = PID(0,0,0, drive_motor3); + + SwerveDrive module1 = SwerveDrive(pid1,pid2); + SwerveDrive module2 = SwerveDrive(pid3,pid4); + SwerveDrive module3 = SwerveDrive(pid5,pid6); + + ZeroingSensor sensor1 = ZeroingSensor(0); + ZeroingSensor sensor2 = ZeroingSensor(0); + ZeroingSensor sensor3 = ZeroingSensor(0); + + //I2C Setup + + stdio_init_all(); + + // Initialize I2C at 100kHz + i2c_init(I2C_PORT, I2C_BAUDRATE); + gpio_set_function(I2C_SDA_PIN, GPIO_FUNC_I2C); + gpio_set_function(I2C_SCL_PIN, GPIO_FUNC_I2C); + + // Set I2C address for Pico + i2c_slave_init(I2C_PORT, I2C_PICO_ADDR, &i2c_handler); + + + while(1){ + if(robotState == STATE_READ_RESULT_ARRAY){ + module1.SetPidTargets(resultArray[0],resultArray[1]); + module2.SetPidTargets(resultArray[2],resultArray[3]); + module3.SetPidTargets(resultArray[4],resultArray[5]); + }else if(robotState == STATE_ZERO_MOTORS){ + module1.TogglePids(false); + module2.TogglePids(false); + module3.TogglePids(false); + while(!sensor1.Read()){ + turn_motor1.SetPwmPercentage(0.1); + } + turn_motor1.SetPwmPercentage(0); + while(!sensor2.Read()){ + turn_motor2.SetPwmPercentage(0.1); + } + turn_motor2.SetPwmPercentage(0); + while(!sensor3.Read()){ + turn_motor3.SetPwmPercentage(0.1); + } + turn_motor3.SetPwmPercentage(0); + + module1.ResetPids(); + module2.ResetPids(); + module3.ResetPids(); + + module1.TogglePids(true); + module2.TogglePids(true); + module3.TogglePids(true); + }else if(robotState == STATE_RETURN_DATA){ + // MessagingWriteMessage("DATAAAAAAAA", output); + // SEND DATA + }else if(robotState == STATE_WRITE_HEARTBEAT){ + // MessagingWriteMessage((unsigned char)0x0, (char*)output); + // SEND DATA + } + if(input_status == 1){ + char* resultString; + MessagingReadBuffer((char*)incoming_data,resultString,I2C_DATA_LENGTH,&robotState); + } + } + return 0; +} \ No newline at end of file