diff --git a/microcontroller_code/drivetrain_steering_node/.gitignore b/microcontroller_code/drivetrain_steering_node/.gitignore new file mode 100644 index 00000000..89cc49cb --- /dev/null +++ b/microcontroller_code/drivetrain_steering_node/.gitignore @@ -0,0 +1,5 @@ +.pio +.vscode/.browse.c_cpp.db* +.vscode/c_cpp_properties.json +.vscode/launch.json +.vscode/ipch diff --git a/microcontroller_code/drivetrain_steering_node/.vscode/extensions.json b/microcontroller_code/drivetrain_steering_node/.vscode/extensions.json new file mode 100644 index 00000000..080e70d0 --- /dev/null +++ b/microcontroller_code/drivetrain_steering_node/.vscode/extensions.json @@ -0,0 +1,10 @@ +{ + // See http://go.microsoft.com/fwlink/?LinkId=827846 + // for the documentation about the extensions.json format + "recommendations": [ + "platformio.platformio-ide" + ], + "unwantedRecommendations": [ + "ms-vscode.cpptools-extension-pack" + ] +} diff --git a/microcontroller_code/drivetrain_steering_node/README.md b/microcontroller_code/drivetrain_steering_node/README.md new file mode 100644 index 00000000..dcd1f8e3 --- /dev/null +++ b/microcontroller_code/drivetrain_steering_node/README.md @@ -0,0 +1,5 @@ +# drivetrain_steering_node + +PlatformIO + Arduino framework project for controlling the steering stepper motors on _La Princesa_. Runs on a Pi Pico controlling 4 DRV8825 drivers (one driver for each wheel). + +Will eventually use micro ROS like the 'MotorTest' node does but for now has rough test code to verify that the steppers work. \ No newline at end of file diff --git a/microcontroller_code/drivetrain_steering_node/include/README b/microcontroller_code/drivetrain_steering_node/include/README new file mode 100644 index 00000000..49819c0d --- /dev/null +++ b/microcontroller_code/drivetrain_steering_node/include/README @@ -0,0 +1,37 @@ + +This directory is intended for project header files. + +A header file is a file containing C declarations and macro definitions +to be shared between several project source files. You request the use of a +header file in your project source file (C, C++, etc) located in `src` folder +by including it, with the C preprocessing directive `#include'. + +```src/main.c + +#include "header.h" + +int main (void) +{ + ... +} +``` + +Including a header file produces the same results as copying the header file +into each source file that needs it. Such copying would be time-consuming +and error-prone. With a header file, the related declarations appear +in only one place. If they need to be changed, they can be changed in one +place, and programs that include the header file will automatically use the +new version when next recompiled. The header file eliminates the labor of +finding and changing all the copies as well as the risk that a failure to +find one copy will result in inconsistencies within a program. + +In C, the convention is to give header files names that end with `.h'. + +Read more about using header files in official GCC documentation: + +* Include Syntax +* Include Operation +* Once-Only Headers +* Computed Includes + +https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html diff --git a/microcontroller_code/drivetrain_steering_node/lib/README b/microcontroller_code/drivetrain_steering_node/lib/README new file mode 100644 index 00000000..93793971 --- /dev/null +++ b/microcontroller_code/drivetrain_steering_node/lib/README @@ -0,0 +1,46 @@ + +This directory is intended for project specific (private) libraries. +PlatformIO will compile them to static libraries and link into the executable file. + +The source code of each library should be placed in a separate directory +("lib/your_library_name/[Code]"). + +For example, see the structure of the following example libraries `Foo` and `Bar`: + +|--lib +| | +| |--Bar +| | |--docs +| | |--examples +| | |--src +| | |- Bar.c +| | |- Bar.h +| | |- library.json (optional. for custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html +| | +| |--Foo +| | |- Foo.c +| | |- Foo.h +| | +| |- README --> THIS FILE +| +|- platformio.ini +|--src + |- main.c + +Example contents of `src/main.c` using Foo and Bar: +``` +#include +#include + +int main (void) +{ + ... +} + +``` + +The PlatformIO Library Dependency Finder will find automatically dependent +libraries by scanning project source files. + +More information about PlatformIO Library Dependency Finder +- https://docs.platformio.org/page/librarymanager/ldf.html diff --git a/microcontroller_code/drivetrain_steering_node/platformio.ini b/microcontroller_code/drivetrain_steering_node/platformio.ini new file mode 100644 index 00000000..0e2da0b6 --- /dev/null +++ b/microcontroller_code/drivetrain_steering_node/platformio.ini @@ -0,0 +1,117 @@ +; PlatformIO Project Configuration File +; +; Build options: build flags, source filter +; Upload options: custom upload port, speed and extra flags +; Library options: dependencies, extra library storages +; Advanced options: extra scripting +; +; Please visit documentation for the other options and examples +; https://docs.platformio.org/page/projectconf.html + + +[platformio] +default_envs = rpipicow + + + +[env] +framework = arduino +; upload_port = /dev/ttyACM0 +platform = https://github.com/maxgerhardt/platform-raspberrypi.git +; Micro-ROS configuration +board_microros_transport = serial +board_microros_distro = ${sysenv.ROS_DISTRO} +monitor_speed = 921600 +; monitor_port = /dev/ttyACM0 +lib_deps = + PaulStoffregen/Time + https://github.com/RobTillaart/DRV8825 + adafruit/Adafruit Motor Shield V2 Library@^1.1.4 +build_flags = + '-D LOCAL_MAC={ 0xAA, 0xBB, 0xCC, 0xEE, 0xDD, 0xFF }' + '-D LOCAL_IP={ 192, 168, 1, 101 }' + '-D AGENT_IP={ 192, 168, 1, 100 }' + '-D AGENT_PORT=8888' + '-D WIFI_SSID="wifi_ssid"' + '-D WIFI_PASSWORD="wifi_password"' + +[env:rpipico] +framework = arduino +monitor_speed = 115200 +; monitor_port = /dev/ttyACM0 +platform = https://github.com/maxgerhardt/platform-raspberrypi.git +board = rpipico +board_build.core = earlephilhower +lib_ldf_mode = chain+ +#enable serial.begin +build_flags = -D PIO_FRAMEWORK_ARDUINO_ENABLE_CDC=1 + +[env:rpipicow] +framework = arduino +monitor_speed = 115200 +; monitor_port = /dev/ttyACM0 +platform = https://github.com/maxgerhardt/platform-raspberrypi.git +board = rpipicow +board_build.core = earlephilhower +lib_ldf_mode = chain+ +#enable serial.begin +build_flags = -D PIO_FRAMEWORK_ARDUINO_ENABLE_CDC=1 + +[env:rpipico2w] +framework = arduino +monitor_speed = 115200 +; monitor_port = /dev/ttyACM0 +platform = https://github.com/maxgerhardt/platform-raspberrypi.git +board = rpipico2 +board_build.core = earlephilhower +lib_ldf_mode = chain+ +#enable serial.begin +build_flags = -D PIO_FRAMEWORK_ARDUINO_ENABLE_CDC=1 + + +[env:uno] +platform = atmelavr +board = uno +monitor_speed = 115200 + +[env:megaatmega2560] +platform = atmelavr +board = megaatmega2560 + +[env:teensy41] +board = teensy41 +framework = arduino +upload_protocol = teensy-cli +monitor_speed = 115200 +lib_deps = + https://github.com/micro-ROS/micro_ros_platformio + PaulStoffregen/Time + micro_ros_arduino + https://github.com/micro-ROS/micro_ros_arduino.git + adafruit/Adafruit Motor Shield V2 Library@^1.1.4 +build_flags = + -D ARDUINO_ARCH_TEENSY + +[env:teensy40] +board = teensy40 + +[env:teensy36] +board = teensy36 + +[env:teensy35] +board = teensy35 + +[env:teensy31] +board = teensy31 + +[env:dev] +board = teensy40 +build_flags = + -I ../config + -D USE_DEV_CONFIG + +[env:vattenkar] +board = teensy40 +build_flags = + -I ../config + -D USE_VATTENKAR_CONFIG diff --git a/microcontroller_code/drivetrain_steering_node/src/main.cpp b/microcontroller_code/drivetrain_steering_node/src/main.cpp new file mode 100644 index 00000000..a8a7d3e2 --- /dev/null +++ b/microcontroller_code/drivetrain_steering_node/src/main.cpp @@ -0,0 +1,61 @@ +#include +#include +#include +// #include "Adafruit_MS_PWMServoDriver.h" + +Adafruit_MotorShield AFMS; +Adafruit_StepperMotor *myMotor; +Adafruit_StepperMotor *myMotor2; + + +void setup() { + Serial.begin(115200); + pinMode(LED_BUILTIN, OUTPUT); + digitalWrite(LED_BUILTIN, HIGH); + + delay(5000); // delay so i can see logs :p + + Serial.println("Setting up Wire"); + + Wire.setSDA(0); + Wire.setSCL(1); + Wire.begin(); + + Wire.setClock(100000); // 100 kHz instead of 400 kHz (default) + + AFMS = Adafruit_MotorShield(); + bool success = AFMS.begin(1600, &Wire); + if (!success) { + Serial.println("Could not find Motor Shield. Check wiring."); + } + else { + Serial.println("Motor Shield found"); + } + + myMotor = AFMS.getStepper(200, 1); + myMotor2 = AFMS.getStepper(200, 2); + myMotor->setSpeed(15); + myMotor2->setSpeed(15); + Serial.println("Setup complete"); + +} + +void loop() { + myMotor->onestep(FORWARD, SINGLE); + // myMotor2->onestep(FORWARD, SINGLE); + + // Serial.println("Stepping forward"); + // digitalWrite(LED_BUILTIN, HIGH); + // myMotor->step(1000, FORWARD, SINGLE); + // myMotor2->step(1000, FORWARD, SINGLE); + // Serial.println("Stepping backward"); + // digitalWrite(LED_BUILTIN, LOW); + // myMotor->step(1000, BACKWARD, SINGLE); + // myMotor2->step(1000, BACKWARD, SINGLE); + + + // Serial.print("Step time: "); + // Serial.println(millis()); + delay(10); +} + diff --git a/microcontroller_code/drivetrain_steering_node/src/main.cpp_real b/microcontroller_code/drivetrain_steering_node/src/main.cpp_real new file mode 100644 index 00000000..cec1a73b --- /dev/null +++ b/microcontroller_code/drivetrain_steering_node/src/main.cpp_real @@ -0,0 +1,84 @@ +#include +// currently using this library: https://github.com/RobTillaart/DRV8825/tree/master?tab=readme-ov-file +// I considered using this library: https://github.com/laurb9/StepperDriver but the current one is working fine so far +#include "DRV8825.h" + +DRV8825 frontLeftSteeringMotor; +DRV8825 frontRightSteeringMotor; +DRV8825 backLeftSteeringMotor; +DRV8825 backRightSteeringMotor; + +/* +Pins connected to the Pico on the protoboard as of 2/25/2026 according to Pablo. +NOTE: GP pins are NOT the same as the physical pin numbers on the Pico. +use https://pico.pinout.xyz as a reference for mapping GP pins to physical pins. + ------------------------------------------------------------- + - GP 2/3: direction + step for the first stepper driver + - GP 4/5: direction + step for the second stepper driver + - GP 6/7: direction + step for the third stepper driver + - GP 8/9: direction + step for the fourth stepper driver + + - GP 10/11: sleep + reset for the fourth stepper driver + - GP 12/13: sleep + reset for the third stepper driver + - GP 14/15: sleep + reset for the second stepper driver + - GP 18/17: sleep + reset for the first stepper driver +*/ + +#define UNUSED_PIN 255 +void setup() { + // The motor to stepper driver assigments are probably wrong but the sets of pins for each stepper driver should be mostly correct (needs further testing). + // So far we have only tested the back left steering motor, shich is connected to the first stepper driver. + backLeftSteeringMotor.begin(2, 3, UNUSED_PIN, 18, 17); + frontRightSteeringMotor.begin(4, 5, UNUSED_PIN, 14,15); + frontLeftSteeringMotor.begin(6, 7, UNUSED_PIN, 12,13); + backRightSteeringMotor.begin(8, 9, UNUSED_PIN, 18,17); + + backLeftSteeringMotor.setStepPulseLength(50); + frontLeftSteeringMotor.setStepPulseLength(50); + backRightSteeringMotor.setStepPulseLength(50); + frontRightSteeringMotor.setStepPulseLength(50); + + + + pinMode(LED_BUILTIN, OUTPUT); +} + +bool moveCW = true; + +void loop() { + bool ledStatus = moveCW; + digitalWrite(LED_BUILTIN, ledStatus ? HIGH : LOW); + + if(moveCW) { + frontLeftSteeringMotor.setDirection(DRV8825_CLOCK_WISE); + frontRightSteeringMotor.setDirection(DRV8825_CLOCK_WISE); + backLeftSteeringMotor.setDirection(DRV8825_CLOCK_WISE); + backRightSteeringMotor.setDirection(DRV8825_CLOCK_WISE); + } else { + frontLeftSteeringMotor.setDirection(DRV8825_COUNTERCLOCK_WISE); + frontRightSteeringMotor.setDirection(DRV8825_COUNTERCLOCK_WISE); + backLeftSteeringMotor.setDirection(DRV8825_COUNTERCLOCK_WISE); + backRightSteeringMotor.setDirection(DRV8825_COUNTERCLOCK_WISE); + } + + if (frontLeftSteeringMotor.getSteps() > 1000) { + moveCW = !moveCW; + frontLeftSteeringMotor.resetSteps(); + } + + + frontLeftSteeringMotor.step(); + frontRightSteeringMotor.step(); + backLeftSteeringMotor.step(); + backRightSteeringMotor.step(); + + Serial.print("FL,"); Serial.print(frontLeftSteeringMotor.getPosition()); Serial.print(","); Serial.print(frontLeftSteeringMotor.getSteps()); Serial.print(","); Serial.print(frontLeftSteeringMotor.getStepPulseLength()); Serial.print(","); Serial.print(frontLeftSteeringMotor.getStepsPerRotation()); + Serial.print(" FR,"); Serial.print(frontRightSteeringMotor.getPosition()); Serial.print(","); Serial.print(frontRightSteeringMotor.getSteps()); Serial.print(","); Serial.print(frontRightSteeringMotor.getStepPulseLength()); Serial.print(","); Serial.print(frontRightSteeringMotor.getStepsPerRotation()); + Serial.print(" BL,"); Serial.print(backLeftSteeringMotor.getPosition()); Serial.print(","); Serial.print(backLeftSteeringMotor.getSteps()); Serial.print(","); Serial.print(backLeftSteeringMotor.getStepPulseLength()); Serial.print(","); Serial.print(backLeftSteeringMotor.getStepsPerRotation()); + Serial.print(" BR,"); Serial.print(backRightSteeringMotor.getPosition()); Serial.print(","); Serial.print(backRightSteeringMotor.getSteps()); Serial.print(","); Serial.print(backRightSteeringMotor.getStepPulseLength()); Serial.print(","); Serial.print(backRightSteeringMotor.getStepsPerRotation()); + Serial.println(); + + delay(1); + + // one other interesting observation - stepper motor behavior changes based on whether I have the serial monitor open or not. +} \ No newline at end of file diff --git a/microcontroller_code/drivetrain_steering_node/test/README b/microcontroller_code/drivetrain_steering_node/test/README new file mode 100644 index 00000000..9b1e87bc --- /dev/null +++ b/microcontroller_code/drivetrain_steering_node/test/README @@ -0,0 +1,11 @@ + +This directory is intended for PlatformIO Test Runner and project tests. + +Unit Testing is a software testing method by which individual units of +source code, sets of one or more MCU program modules together with associated +control data, usage procedures, and operating procedures, are tested to +determine whether they are fit for use. Unit testing finds problems early +in the development cycle. + +More information about PlatformIO Unit Testing: +- https://docs.platformio.org/en/latest/advanced/unit-testing/index.html diff --git a/obstacle_detection.code-workspace b/obstacle_detection.code-workspace index 5d87edaf..b8f5c603 100644 --- a/obstacle_detection.code-workspace +++ b/obstacle_detection.code-workspace @@ -5,27 +5,48 @@ }, { "path": "ws_urc/src" + }, + { + "path": "microcontroller_code/MotorTest" + }, + { + "path": "microcontroller_code/drivetrain_steering_node" } ], "settings": { - "ros.distro": "humble", + "ros.distro": "jazzy", "python.autoComplete.extraPaths": [ - "/home/therion/dev/school/URC/ws_urc/install/video_streamer/lib/python3.10/site-packages", - "/home/therion/dev/school/URC/ws_urc/install/obstacle_detection/lib/python3.10/site-packages", - "/home/therion/dev/school/URC/ws_urc/install/drivetrain/lib/python3.10/site-packages", - "/home/therion/dev/school/URC/ws_urc/install/depth_image_viewer/lib/python3.10/site-packages", - "/home/therion/dev/school/URC/ws_urc/install/action_drivetrain_interface/local/lib/python3.10/dist-packages", - "/opt/ros/humble/lib/python3.10/site-packages", - "/opt/ros/humble/local/lib/python3.10/dist-packages" + "${workspaceFolder}/ws_urc/install/video_streamer/lib/python3.10/site-packages", + "${workspaceFolder}/ws_urc/install/obstacle_detection/lib/python3.10/site-packages", + "${workspaceFolder}/ws_urc/install/drivetrain/lib/python3.10/site-packages", + "${workspaceFolder}/ws_urc/install/depth_image_viewer/lib/python3.10/site-packages", + "${workspaceFolder}/ws_urc/install/action_drivetrain_interface/local/lib/python3.10/dist-packages", + "/opt/ros/jazzy/lib/python3.10/site-packages", + "${workspaceFolder}/opt/ros/jazzy/local/lib/python3.10/dist-packages" ], "python.analysis.extraPaths": [ - "/home/therion/dev/school/URC/ws_urc/install/video_streamer/lib/python3.10/site-packages", - "/home/therion/dev/school/URC/ws_urc/install/obstacle_detection/lib/python3.10/site-packages", - "/home/therion/dev/school/URC/ws_urc/install/drivetrain/lib/python3.10/site-packages", - "/home/therion/dev/school/URC/ws_urc/install/depth_image_viewer/lib/python3.10/site-packages", - "/home/therion/dev/school/URC/ws_urc/install/action_drivetrain_interface/local/lib/python3.10/dist-packages", - "/opt/ros/humble/lib/python3.10/site-packages", - "/opt/ros/humble/local/lib/python3.10/dist-packages" - ] + "${workspaceFolder}/ws_urc/install/video_streamer/lib/python3.10/site-packages", + "${workspaceFolder}/ws_urc/install/obstacle_detection/lib/python3.10/site-packages", + "${workspaceFolder}/ws_urc/install/drivetrain/lib/python3.10/site-packages", + "${workspaceFolder}/ws_urc/install/depth_image_viewer/lib/python3.10/site-packages", + "${workspaceFolder}/ws_urc/install/action_drivetrain_interface/local/lib/python3.10/dist-packages", + "/opt/ros/jazzy/lib/python3.10/site-packages", + "/opt/ros/jazzy/local/lib/python3.10/dist-packages" + ], + "files.associations": { + "__bit_reference": "cpp", + "__hash_table": "cpp", + "__split_buffer": "cpp", + "__tree": "cpp", + "array": "cpp", + "deque": "cpp", + "forward_list": "cpp", + "initializer_list": "cpp", + "map": "cpp", + "string": "cpp", + "string_view": "cpp", + "unordered_map": "cpp", + "vector": "cpp" + } } }