├── Code ├── Teensy41Main │ ├── lib │ │ ├── DataTransfer │ │ └── README.md │ ├── .gitignore │ ├── test │ │ └── README │ ├── platformio.ini │ ├── src │ │ ├── dataEnums.h │ │ ├── motorSetup.cpp │ │ └── main.cpp │ └── include │ │ ├── README │ │ └── motorSetup.h └── quadController │ ├── lib │ ├── DataTransfer │ └── README.md │ ├── .gitignore │ ├── .vscode │ └── extensions.json │ ├── platformio.ini │ ├── test │ └── README │ ├── src │ ├── dataEnums.h │ └── main.cpp │ └── README.md ├── images ├── V1-1Side.png ├── V1Front.png ├── V1-1Front.png ├── V1TopBack.png ├── V1-1Rendering.png └── quadController.png ├── .gitignore ├── Hardware ├── PCBV1-0 │ ├── Gerbers.zip │ └── layoutV1-0.png ├── QuadV1_BOM.numbers └── README.md ├── LICENSE └── README.md /Code/Teensy41Main/lib/DataTransfer: -------------------------------------------------------------------------------- 1 | ../../../../CustomArduinoLibs/DataTransfer -------------------------------------------------------------------------------- /Code/quadController/lib/DataTransfer: -------------------------------------------------------------------------------- 1 | ../../../../CustomArduinoLibs/DataTransfer -------------------------------------------------------------------------------- /images/V1-1Side.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/seanboe/QuadrupedProject/HEAD/images/V1-1Side.png -------------------------------------------------------------------------------- /images/V1Front.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/seanboe/QuadrupedProject/HEAD/images/V1Front.png -------------------------------------------------------------------------------- /images/V1-1Front.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/seanboe/QuadrupedProject/HEAD/images/V1-1Front.png -------------------------------------------------------------------------------- /images/V1TopBack.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/seanboe/QuadrupedProject/HEAD/images/V1TopBack.png -------------------------------------------------------------------------------- /images/V1-1Rendering.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/seanboe/QuadrupedProject/HEAD/images/V1-1Rendering.png -------------------------------------------------------------------------------- /images/quadController.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/seanboe/QuadrupedProject/HEAD/images/quadController.png -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | Code/Teensy41Main/.pio 2 | Code/Teensy41Main/.vscode 3 | Code/Teensy41Main/lib/Kinematics 4 | .DS_Store 5 | -------------------------------------------------------------------------------- /Hardware/PCBV1-0/Gerbers.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/seanboe/QuadrupedProject/HEAD/Hardware/PCBV1-0/Gerbers.zip -------------------------------------------------------------------------------- /Hardware/QuadV1_BOM.numbers: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/seanboe/QuadrupedProject/HEAD/Hardware/QuadV1_BOM.numbers -------------------------------------------------------------------------------- /Hardware/PCBV1-0/layoutV1-0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/seanboe/QuadrupedProject/HEAD/Hardware/PCBV1-0/layoutV1-0.png -------------------------------------------------------------------------------- /Code/quadController/lib/README.md: -------------------------------------------------------------------------------- 1 | Library dependencies: 2 | 3 | [DataTransfer](https://github.com/seanboe/DataTransfer) 4 | 5 | Once you've downloaded it, put it in this lib folder. -------------------------------------------------------------------------------- /Code/quadController/.gitignore: -------------------------------------------------------------------------------- 1 | .pio 2 | .vscode/.browse.c_cpp.db* 3 | .vscode/c_cpp_properties.json 4 | .vscode/launch.json 5 | .vscode/ipch 6 | 7 | lib/DataTransfer 8 | 9 | include 10 | -------------------------------------------------------------------------------- /Code/Teensy41Main/.gitignore: -------------------------------------------------------------------------------- 1 | .pio 2 | .vscode/.browse.c_cpp.db* 3 | .vscode/c_cpp_properties.json 4 | .vscode/launch.json 5 | .vscode/ipch 6 | 7 | lib/QuadrupedKinematics 8 | lib/DataTransfer 9 | 10 | src/tests.txt 11 | 12 | -------------------------------------------------------------------------------- /Code/Teensy41Main/lib/README.md: -------------------------------------------------------------------------------- 1 | Library dependencies: 2 | 3 | [QuadrupedKinematics](https://github.com/seanboe/QuadrupedKinematics) 4 | [DataTransfer](https://github.com/seanboe/DataTransfer) 5 | 6 | Once you've downloaded them, put them in this lib folder. -------------------------------------------------------------------------------- /Code/quadController/.vscode/extensions.json: -------------------------------------------------------------------------------- 1 | { 2 | // See http://go.microsoft.com/fwlink/?LinkId=827846 3 | // for the documentation about the extensions.json format 4 | "recommendations": [ 5 | "platformio.platformio-ide" 6 | ] 7 | } 8 | -------------------------------------------------------------------------------- /Code/quadController/platformio.ini: -------------------------------------------------------------------------------- 1 | ; PlatformIO Project Configuration File 2 | ; 3 | ; Build options: build flags, source filter 4 | ; Upload options: custom upload port, speed and extra flags 5 | ; Library options: dependencies, extra library storages 6 | ; Advanced options: extra scripting 7 | ; 8 | ; Please visit documentation for the other options and examples 9 | ; https://docs.platformio.org/page/projectconf.html 10 | 11 | [env:teensy40] 12 | platform = teensy 13 | board = teensy40 14 | framework = arduino 15 | lib_deps = nrf24/RF24@^1.4.0 16 | -------------------------------------------------------------------------------- /Code/Teensy41Main/test/README: -------------------------------------------------------------------------------- 1 | 2 | This directory is intended for PlatformIO Unit Testing and project tests. 3 | 4 | Unit Testing is a software testing method by which individual units of 5 | source code, sets of one or more MCU program modules together with associated 6 | control data, usage procedures, and operating procedures, are tested to 7 | determine whether they are fit for use. Unit testing finds problems early 8 | in the development cycle. 9 | 10 | More information about PlatformIO Unit Testing: 11 | - https://docs.platformio.org/page/plus/unit-testing.html 12 | -------------------------------------------------------------------------------- /Code/quadController/test/README: -------------------------------------------------------------------------------- 1 | 2 | This directory is intended for PlatformIO Unit Testing and project tests. 3 | 4 | Unit Testing is a software testing method by which individual units of 5 | source code, sets of one or more MCU program modules together with associated 6 | control data, usage procedures, and operating procedures, are tested to 7 | determine whether they are fit for use. Unit testing finds problems early 8 | in the development cycle. 9 | 10 | More information about PlatformIO Unit Testing: 11 | - https://docs.platformio.org/page/plus/unit-testing.html 12 | -------------------------------------------------------------------------------- /Code/Teensy41Main/platformio.ini: -------------------------------------------------------------------------------- 1 | ; PlatformIO Project Configuration File 2 | ; 3 | ; Build options: build flags, source filter 4 | ; Upload options: custom upload port, speed and extra flags 5 | ; Library options: dependencies, extra library storages 6 | ; Advanced options: extra scripting 7 | ; 8 | ; Please visit documentation for the other options and examples 9 | ; https://docs.platformio.org/page/projectconf.html 10 | 11 | [env:teensy41] 12 | platform = teensy 13 | board = teensy41 14 | framework = arduino 15 | lib_deps = 16 | siteswapjuggler/Ramp@^0.6.0 17 | nrf24/RF24@^1.4.0 18 | -------------------------------------------------------------------------------- /Code/Teensy41Main/src/dataEnums.h: -------------------------------------------------------------------------------- 1 | // THIS SHOULD BE IDENTICAL ON BOTH DEVICES 2 | 3 | 4 | // These are enums I'm using for each corresponding piece of data to make parsing data more readable 5 | // i.e. the controller uses bit BUTTON_LEFT, and the quadruped reads BUTTON_LEFT, as opposed to 0 and 0. More easily read. 6 | // This file should be IDENTICAL for the devices receiving data from here. 7 | 8 | typedef enum { 9 | BUTTON_LEFT = 0, BUTTON_RIGHT, JOYL_X, JOYL_Y, JOYR_X, JOYR_Y 10 | } BIT_INPUT_TYPE; 11 | 12 | typedef enum { 13 | JOYL_X_ANALOG = 1, JOYL_Y_ANALOG, JOYR_X_ANALOG, JOYR_Y_ANALOG 14 | } BYTE_INPUT_TYPE; -------------------------------------------------------------------------------- /Code/quadController/src/dataEnums.h: -------------------------------------------------------------------------------- 1 | // THIS SHOULD BE IDENTICAL ON BOTH DEVICES 2 | 3 | 4 | // These are enums I'm using for each corresponding piece of data to make parsing data more readable 5 | // i.e. the controller uses bit BUTTON_LEFT, and the quadruped reads BUTTON_LEFT, as opposed to 0 and 0. More easily read. 6 | // This file should be IDENTICAL for the devices receiving data from here. 7 | 8 | typedef enum { 9 | BUTTON_LEFT = 0, BUTTON_RIGHT, JOYL_X, JOYL_Y, JOYR_X, JOYR_Y 10 | } BIT_INPUT_TYPE; 11 | 12 | typedef enum { 13 | JOYL_X_ANALOG = 1, JOYL_Y_ANALOG, JOYR_X_ANALOG, JOYR_Y_ANALOG 14 | } BYTE_INPUT_TYPE; -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2020 Sean Boerhout 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /Code/Teensy41Main/include/README: -------------------------------------------------------------------------------- 1 | 2 | This directory is intended for project header files. 3 | 4 | A header file is a file containing C declarations and macro definitions 5 | to be shared between several project source files. You request the use of a 6 | header file in your project source file (C, C++, etc) located in `src` folder 7 | by including it, with the C preprocessing directive `#include'. 8 | 9 | ```src/main.c 10 | 11 | #include "header.h" 12 | 13 | int main (void) 14 | { 15 | ... 16 | } 17 | ``` 18 | 19 | Including a header file produces the same results as copying the header file 20 | into each source file that needs it. Such copying would be time-consuming 21 | and error-prone. With a header file, the related declarations appear 22 | in only one place. If they need to be changed, they can be changed in one 23 | place, and programs that include the header file will automatically use the 24 | new version when next recompiled. The header file eliminates the labor of 25 | finding and changing all the copies as well as the risk that a failure to 26 | find one copy will result in inconsistencies within a program. 27 | 28 | In C, the usual convention is to give header files names that end with `.h'. 29 | It is most portable to use only letters, digits, dashes, and underscores in 30 | header file names, and at most one dot. 31 | 32 | Read more about using header files in official GCC documentation: 33 | 34 | * Include Syntax 35 | * Include Operation 36 | * Once-Only Headers 37 | * Computed Includes 38 | 39 | https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html 40 | -------------------------------------------------------------------------------- /Hardware/README.md: -------------------------------------------------------------------------------- 1 | # Hardware 2 | 3 | ## _Stay Tuned for Version 2!!!!!!!!!!!!!!!!!!_ 4 | __It will very likely feature a [BNO055 chip](https://www.bosch-sensortec.com/products/smart-sensors/bno055/), a BEATIFUL IMU with on-board 9-axis fusion for absolute orientation. I am reallllyyyy 5 | looking forward to this!__ 6 | 7 | This is where all my custom hardware is posted. Gerbers for control board V1.0 is already available, and CAD files will be released shortly. 8 | 9 | ## Quadruped Control Board V1.0 10 | 11 | It's done! Note that I haven't had the chance to test it yet (the board hasn't arrived) and a BOM will be coming shortly. 12 | 13 | 14 | 15 | #### Features: 16 | - 2 layer board 17 | - Includes 12 breakouts for all servos 18 | - On-board IMU data (via an [LSM6DSOX](https://www.st.com/resource/en/datasheet/lsm6dsox.pdf)) 19 | - Battery capacity monitoring (via an [LC709203F](https://www.onsemi.com/pdf/datasheet/lc709203f-d.pdf)) 20 | - LoRA radio -900Mhz 21 | - Motor power shutoff capability 22 | - Controlled by a Teensy 4.1 23 | - Battery power/USB power selection for Teensy 24 | - 6 breakout GPIO, including an I2C bus and associated power/ground lines 25 | 26 | *Known issues with v1.0* 😭 27 | - Holes for header pins are too small to allow them to comfortably fit. You will need to "jam" them in or solder on top of the hole. 28 | - xt-60 connector +/- is flipped (silkscreen outline is wrong). Simply flip the connector on the board, or solder the wires reverse for the male connector that goes onto the board. 29 | - miniBoost and LoRA radio header holes are flipped (they are the mirror image of what they should be) 😂 30 | 31 | ### Final Verdict: 32 | Do __NOT__ use this 😂 33 | The PWM works (sensors yet to be tested), but there are too many mistakes for me to be adequately proud of it. A NEW VERSION IS COMING! -------------------------------------------------------------------------------- /Code/Teensy41Main/include/motorSetup.h: -------------------------------------------------------------------------------- 1 | #ifndef _MOTOR_SETUP_ 2 | #define _MOTOR_SETUP_ 3 | 4 | #include 5 | 6 | // The motor struct definition and various enums are defined inside here 7 | #include 8 | 9 | // degrees to microseconds sale factor (determined experimentally, MUST be the same for every motor) 10 | #define DEGREES_TO_MICROS 7.5 11 | 12 | //Leg 1: 13 | extern Motor L1M1; 14 | extern Motor L1M2; 15 | extern Motor L1M3; 16 | 17 | //Leg 2: 18 | extern Motor L2M1; 19 | extern Motor L2M2; 20 | extern Motor L2M3; 21 | 22 | //Leg 2: 23 | extern Motor L3M1; 24 | extern Motor L3M2; 25 | extern Motor L3M3; 26 | 27 | //Leg 2: 28 | extern Motor L4M1; 29 | extern Motor L4M2; 30 | extern Motor L4M3; 31 | 32 | extern Motor motors[ROBOT_LEG_COUNT * MOTORS_PER_LEG]; 33 | 34 | 35 | // Whether a motor is standard or mirrored depends on whether its final 36 | // angle is calculated like the motors in leg 2. Standard motors have a 37 | // contextual offset calculated like motors in leg 2, mirrored motors have 38 | // have a contextual offset calculated like leg 1 (mirrored to leg 2) 39 | typedef enum { 40 | M1_standard, M1_mirrored, 41 | M2_standard, M2_mirrored, 42 | M3_standard, M3_mirrored 43 | } ContexType; 44 | 45 | typedef enum { 46 | DEGREES, MILLIS 47 | } unitType; 48 | 49 | typedef enum { 50 | DYNAMIC, STATIC 51 | } ActivityType; 52 | 53 | // Function definitions 54 | void initMotorVals(Motor _motors[]); 55 | uint16_t indexOfMotor(LegID leg, MotorID motor); 56 | int16_t constrainAngle(Motor _motors[], LegID legID, MotorID motorID, int16_t demandAngle); 57 | int16_t applyContextualOffset(Motor _motors[], LegID legID, MotorID motorID, int16_t demandAngle); 58 | uint16_t degreesToMicros(uint8_t inputDegrees, uint8_t calibOffset); 59 | int16_t getPreparedAngles(Motor _motors[], LegID legID, MotorID motorID, unitType angleUnitType, ActivityType activityType); 60 | 61 | 62 | #endif -------------------------------------------------------------------------------- /Code/quadController/README.md: -------------------------------------------------------------------------------- 1 | ## Quadruped Remote Controller 2 | 3 |

QuadrupedProject

4 | 5 | This is a __temporary__ controller for the quadruped built on a breadboard with two joysticks and two buttons. A future project will be to design a PCB to make a PS4 style, arduino compatible remote, but that's for later. 6 | 7 | If you want to make it, you'll need: 8 | - [5V booster](https://www.adafruit.com/product/4654) 9 | - [RFM69HCW LoRA radio](https://www.adafruit.com/product/4654) 10 | - [Joysticks](https://www.amazon.com/ARCELI-Joystick-Controller-Dual-axis-Breakout/dp/B077Z8QN3S/ref=sr_1_6?dchild=1&keywords=joystick+arduino&qid=1621526560&sr=8-6) 11 | - *You'll need to modify the headers for this* 12 | - Buttons 13 | - Teensy 4.0 14 | - A 1s Lipo battery 15 | 16 | ### Pinouts: 17 | 18 | | Teensy 4.0 | RFM69HCW | Teensy 4.0| Joystick Left| Teensy 4.0| Joystick Right| 19 | | :-------------: | :-------------: | :-------------: | :-------------: | :-------------: | :-------------: | 20 | | 3V|VIN|GND|GND|GND|GND| 21 | | GND|GND|3V|+5V|3V|+5V| 22 | | NC|EN|15|VRx|16|VRx| 23 | |3|G0|14|VRy|17|VRy| 24 | |13|SCK| 25 | |12|MISO| 26 | |11|MOSI| 27 | |4|CS| 28 | |2|RST| 29 | 30 | The left and right buttons are a attached to pins 23 and 22, respectively, on the Teensy. Either use a 10K resistor for pullup, or 31 | specify both pins as INPUT_PULLUP. 32 | 33 | The 5V boost is set up to provide the teensy with 5V for wireless controlling. 34 | 35 | ### Library dependencies: 36 | To run my code, you'll need the [RadioHead Library](https://github.com/adafruit/RadioHead). Create an include folder in the 37 | QuadController directory and place the library in there. 38 | 39 | Also, I'm using the DataTransfer library here; I'm not giving it its own repo because it's quite simple, so I'm including it here. You can find it in lib (this is *exactly* the same as the one in Teensy41Main) -------------------------------------------------------------------------------- /Code/quadController/src/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | #include "dataEnums.h" 6 | 7 | // Joystick pins 8 | #define JOYL_X_PIN 15 9 | #define JOYL_Y_PIN 14 10 | #define JOYR_X_PIN 16 11 | #define JOYR_Y_PIN 17 12 | 13 | // Button pins 14 | #define BUTTON_LEFT_PIN 23 15 | #define BUTTON_RIGHT_PIN 22 16 | 17 | // Filter parameters 18 | #define TOLERANCE 2 19 | #define JOYSTICK_CENTER 128 // mapped value; 0-1023 to 0-255 20 | 21 | //Radio Encryption Key 22 | const byte radioKey[6] = "00001"; 23 | 24 | Transfer dataBuffer(21, 20, BUFFER_SIZE); 25 | 26 | uint8_t filterAnalog(uint8_t value, uint8_t tolerance); 27 | 28 | 29 | void setup() { 30 | 31 | // Pin setup 32 | pinMode(JOYL_X_PIN, INPUT); 33 | pinMode(JOYL_Y_PIN, INPUT); 34 | pinMode(JOYR_X_PIN, INPUT); 35 | pinMode(JOYR_Y_PIN, INPUT); 36 | pinMode(BUTTON_LEFT_PIN, INPUT_PULLUP); 37 | pinMode(BUTTON_RIGHT_PIN, INPUT_PULLUP); 38 | 39 | Serial.begin(9600); 40 | dataBuffer.init(0, TRANSMITTER, radioKey); 41 | 42 | } 43 | 44 | void loop() { 45 | 46 | // I'm organizing my data buffer in the first byte as such: BUTTON_LEFT, BUTTON_RIGHT, JOYL_X, JOYL_Y, JOYR_X, JOYR_Y reading from RIGHT TO LEFT 47 | //(if each button/joystick is active, then the bit will read 1) 48 | 49 | dataBuffer.write(BIT, BUTTON_LEFT, (!digitalRead(BUTTON_LEFT_PIN) ? 1 : 0)); 50 | dataBuffer.write(BIT, BUTTON_RIGHT, (!digitalRead(BUTTON_RIGHT_PIN) ? 1 : 0)); 51 | dataBuffer.write(BIT, JOYL_X, ((filterAnalog(map(analogRead(JOYL_X_PIN), 0, 1023, 255, 0), TOLERANCE) != JOYSTICK_CENTER) ? 1 : 0 )); 52 | dataBuffer.write(BIT, JOYL_Y, ((filterAnalog(map(analogRead(JOYL_Y_PIN), 0, 1023, 255, 0), TOLERANCE) != JOYSTICK_CENTER) ? 1 : 0 )); 53 | dataBuffer.write(BIT, JOYR_X, ((filterAnalog(map(analogRead(JOYR_X_PIN), 0, 1023, 255, 0), TOLERANCE) != JOYSTICK_CENTER) ? 1 : 0 )); 54 | dataBuffer.write(BIT, JOYR_Y, ((filterAnalog(map(analogRead(JOYR_Y_PIN), 0, 1023, 255, 0), TOLERANCE) != JOYSTICK_CENTER) ? 1 : 0 )); 55 | 56 | 57 | // The next 4 bytes are for holding analog joystick data (in that order) if their respective bits are high 58 | 59 | dataBuffer.write(BYTE, JOYL_X_ANALOG, filterAnalog(map(analogRead(JOYL_X_PIN), 0, 1023, 255, 0), TOLERANCE)); 60 | dataBuffer.write(BYTE, JOYL_Y_ANALOG, filterAnalog(map(analogRead(JOYL_Y_PIN), 0, 1023, 255, 0), TOLERANCE)); 61 | dataBuffer.write(BYTE, JOYR_X_ANALOG, filterAnalog(map(analogRead(JOYR_X_PIN), 0, 1023, 255, 0), TOLERANCE)); 62 | dataBuffer.write(BYTE, JOYR_Y_ANALOG, filterAnalog(map(analogRead(JOYR_Y_PIN), 0, 1023, 255, 0), TOLERANCE)); 63 | 64 | dataBuffer.send(); 65 | 66 | delay(100); 67 | 68 | } 69 | 70 | uint8_t filterAnalog(uint8_t value, uint8_t tolerance) { 71 | if (abs(value - JOYSTICK_CENTER) > TOLERANCE) 72 | return value; 73 | else 74 | return JOYSTICK_CENTER; 75 | } 76 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 |

2 |
3 | QuadrupedProject 4 |
5 | Quadruped Project 6 |
7 |

8 | 9 |

10 | A quest to make a simple and affordable, yet advanced, quadruped robot. 11 |
12 |
13 | 14 | 15 | 16 |

17 | 18 | ### Software Setup 19 | This project is compiled and uploaded to a teensy 4.1 via [PlatformIO](https://platformio.org), an extension to VS code. 20 | Once you download Code/Teensy41Main, you should be able to open the folder by selecting platformio.ini. 21 | Assuming you have all dependent libraries installed, you should be able to run the code! 22 | 23 | 24 | ### Library dependencies 25 | To run this code, you'll need to install a few libraries for Arduino/Teensy. 26 | - [QuadrupedKinematics](https://github.com/seanboe/QuadrupedKinematics) 27 | - [Servo](https://www.arduino.cc/reference/en/libraries/servo/) 28 | - [RAMP](https://github.com/siteswapjuggler/RAMP) 29 | - [Radiohead](https://github.com/adafruit/RadioHead) 30 | 31 | Note that QuadrupedKinematics is a custom library; you will need to download it and place in the lib 32 | in your platformIO project folder (which you can download above) 33 | 34 | ### Hardware 35 | You can find the CAD files in .step format [here](https://github.com/seanboe/QuadrupedProject) -will update _*very soon*_ 36 | 37 | A bill of materials (BOM) can be found [here](https://github.com/seanboe/QuadrupedProject/tree/master/Hardware) as a .numbers file or 38 | [here](https://docs.google.com/spreadsheets/d/18XhNiGI3mZoEecLmq4_vx1SQUpdlzQzPsUQPOoMVXBk/edit#gid=0) as a google spreadsheet. 39 | 40 | This doesn't include components for the PCB yet (I'm designing a new one) 41 | 42 | I've also designed a PCB for this robot:
43 | 44 |
45 | Gerber files for version 1.0 can be found here. 46 | 47 | I'm also recording most of the progress for the project on my blog, which is [here](https://seanboe.github.io/blog/), 48 | and I release short updates on my [YouTube Channel](https://www.youtube.com/channel/UCSMmECMAWD-FGQWWuThr7_w) 49 | 50 | ### License 51 | 52 | MIT 53 | 54 | ### Other 55 | Please note that most of the development for this project is done in the [QuadrupedKinematics](https://github.com/seanboe/QuadrupedKinematics) 56 | repository. This is where most of the control processes and math is stored. 57 | -------------------------------------------------------------------------------- /Code/Teensy41Main/src/motorSetup.cpp: -------------------------------------------------------------------------------- 1 | // Holds all the stuff specifically for the motors 2 | // Define each motor's characteristicts here! Each motor has a struct object. 3 | // The struct itself is defined in Kinematics.h to keep everything consistent. 4 | // Here is the definition for reference: 5 | // typedef struct { 6 | // uint8_t controlPin; 7 | 8 | // // Angle/calculation stuff 9 | // int16_t angleDegrees; 10 | // int16_t previousDegrees; // previous degrees since last call to updateDynamicPositions() 11 | // int16_t dynamicDegrees; 12 | 13 | // // Calibration 14 | // uint16_t calibOffset; // This is an offset for calibration (to keep the motor accurate) 15 | // uint8_t maxPos; 16 | // uint8_t minPos; 17 | // uint16_t applicationOffset; // This is an offset for putting the calculated angles in contex. 18 | // // It is likely that the zero positions of the motors isn't where 19 | // // calculations assumes it to be, so you need an offset to make 20 | // // sure that the angle is correct relative to the motor's zero. 21 | // } Motor; 22 | 23 | #include 24 | 25 | // The struct and various enums are defined inside here 26 | #include 27 | 28 | #include 29 | 30 | // Called in setup 31 | void initMotorVals(Motor _motors[]) { 32 | 33 | _motors[0] = L1M1; 34 | _motors[1] = L1M2; 35 | _motors[2] = L1M3; 36 | _motors[3] = L2M1; 37 | _motors[4] = L2M2; 38 | _motors[5] = L2M3; 39 | _motors[6] = L3M1; 40 | _motors[7] = L3M2; 41 | _motors[8] = L3M3; 42 | _motors[9] = L4M1; 43 | _motors[10] = L4M2; 44 | _motors[11] = L4M3; 45 | 46 | // ********************** LEG 1 ********************** 47 | _motors[indexOfMotor(LEG_1, M1)].controlPin = 14; 48 | _motors[indexOfMotor(LEG_1, M1)].calibOffset = 0; 49 | _motors[indexOfMotor(LEG_1, M1)].maxPos = 135; 50 | _motors[indexOfMotor(LEG_1, M1)].minPos = 30; 51 | _motors[indexOfMotor(LEG_1, M1)].applicationOffset = 90; 52 | 53 | _motors[indexOfMotor(LEG_1, M2)].controlPin = 37; 54 | _motors[indexOfMotor(LEG_1, M2)].calibOffset = 80; 55 | _motors[indexOfMotor(LEG_1, M2)].maxPos = 270; 56 | _motors[indexOfMotor(LEG_1, M2)].minPos = 0; 57 | _motors[indexOfMotor(LEG_1, M2)].applicationOffset = 0; 58 | 59 | _motors[indexOfMotor(LEG_1, M3)].controlPin = 36; 60 | _motors[indexOfMotor(LEG_1, M3)].calibOffset = 10; 61 | _motors[indexOfMotor(LEG_1, M3)].maxPos = 130; 62 | _motors[indexOfMotor(LEG_1, M3)].minPos = 45; 63 | _motors[indexOfMotor(LEG_1, M3)].applicationOffset = 0; 64 | 65 | // ********************** LEG 2 ********************** 66 | _motors[indexOfMotor(LEG_2, M1)].controlPin = 23; 67 | _motors[indexOfMotor(LEG_2, M1)].calibOffset = 60; 68 | _motors[indexOfMotor(LEG_2, M1)].maxPos = 120; 69 | _motors[indexOfMotor(LEG_2, M1)].minPos = 45; 70 | _motors[indexOfMotor(LEG_2, M1)].applicationOffset = 90; 71 | 72 | _motors[indexOfMotor(LEG_2, M2)].controlPin = 22; 73 | _motors[indexOfMotor(LEG_2, M2)].calibOffset = 120; 74 | _motors[indexOfMotor(LEG_2, M2)].maxPos = 270; 75 | _motors[indexOfMotor(LEG_2, M2)].minPos = 0; 76 | _motors[indexOfMotor(LEG_2, M2)].applicationOffset = 90; 77 | 78 | _motors[indexOfMotor(LEG_2, M3)].controlPin = 15; 79 | _motors[indexOfMotor(LEG_2, M3)].calibOffset = 55; 80 | _motors[indexOfMotor(LEG_2, M3)].maxPos = 130; 81 | _motors[indexOfMotor(LEG_2, M3)].minPos = 45; 82 | _motors[indexOfMotor(LEG_2, M3)].applicationOffset = 90; 83 | 84 | // ********************** LEG 3 ********************** 85 | _motors[indexOfMotor(LEG_3, M1)].controlPin = 0; 86 | _motors[indexOfMotor(LEG_3, M1)].calibOffset = 35; 87 | _motors[indexOfMotor(LEG_3, M1)].maxPos = 135; 88 | _motors[indexOfMotor(LEG_3, M1)].minPos = 30; 89 | _motors[indexOfMotor(LEG_3, M1)].applicationOffset = 90; 90 | 91 | _motors[indexOfMotor(LEG_3, M2)].controlPin = 1; 92 | _motors[indexOfMotor(LEG_3, M2)].calibOffset = 45; 93 | _motors[indexOfMotor(LEG_3, M2)].maxPos = 270; 94 | _motors[indexOfMotor(LEG_3, M2)].minPos = 0; 95 | _motors[indexOfMotor(LEG_3, M2)].applicationOffset = 90; 96 | 97 | _motors[indexOfMotor(LEG_3, M3)].controlPin = 2; 98 | _motors[indexOfMotor(LEG_3, M3)].calibOffset = 0; 99 | _motors[indexOfMotor(LEG_3, M3)].maxPos = 130; 100 | _motors[indexOfMotor(LEG_3, M3)].minPos = 45; 101 | _motors[indexOfMotor(LEG_3, M3)].applicationOffset = 90; 102 | 103 | // ********************** LEG 4 ********************** 104 | _motors[indexOfMotor(LEG_4, M1)].controlPin = 3; 105 | _motors[indexOfMotor(LEG_4, M1)].calibOffset = 80; 106 | _motors[indexOfMotor(LEG_4, M1)].maxPos = 120; 107 | _motors[indexOfMotor(LEG_4, M1)].minPos = 45; 108 | _motors[indexOfMotor(LEG_4, M1)].applicationOffset = 90; 109 | 110 | _motors[indexOfMotor(LEG_4, M2)].controlPin = 4; 111 | _motors[indexOfMotor(LEG_4, M2)].calibOffset = 70; 112 | _motors[indexOfMotor(LEG_4, M2)].maxPos = 270; 113 | _motors[indexOfMotor(LEG_4, M2)].minPos = 0; 114 | _motors[indexOfMotor(LEG_4, M2)].applicationOffset = 0; 115 | 116 | _motors[indexOfMotor(LEG_4, M3)].controlPin = 5; 117 | _motors[indexOfMotor(LEG_4, M3)].calibOffset = 70; 118 | _motors[indexOfMotor(LEG_4, M3)].maxPos = 130; 119 | _motors[indexOfMotor(LEG_4, M3)].minPos = 45; 120 | _motors[indexOfMotor(LEG_4, M3)].applicationOffset = 0; 121 | } 122 | 123 | 124 | 125 | uint16_t indexOfMotor(LegID leg, MotorID motor) { 126 | return ((leg - 1) * MOTORS_PER_LEG + motor) - 1; 127 | } 128 | 129 | int16_t constrainAngle(Motor _motors[], LegID legID, MotorID motorID, int16_t demandAngle) { 130 | if (demandAngle > _motors[indexOfMotor(legID, motorID)].maxPos) 131 | return _motors[indexOfMotor(legID, motorID)].maxPos; 132 | else if (demandAngle < _motors[indexOfMotor(legID, motorID)].minPos) 133 | return _motors[indexOfMotor(legID, motorID)].minPos; 134 | return demandAngle; 135 | } 136 | 137 | // This applies the offsets for the motors based on CONTEXT. 138 | // The library calculates the angles for the all the JOINTS, 139 | // which isn't actually the angle each motor must achieve 140 | // since the 0 position of the motors is different for some. 141 | // This is because they all have a defined zero-degrees 142 | // position and can turn 270 degrees but the motors must be 143 | // mirrored to each other on each side of the bot. 144 | 145 | int16_t applyContextualOffset(Motor _motors[], LegID legID, MotorID motorID, int16_t demandAngle) { 146 | ContexType contexType = M1_standard; 147 | // int16_t demandAngle = _motors[indexOfMotor(legID, motorID)].angleDegrees; 148 | int16_t angle = demandAngle; 149 | 150 | switch (legID) { 151 | 152 | case LEG_1: 153 | switch (motorID) { 154 | case M1: contexType = M1_mirrored; break; 155 | case M2: contexType = M2_mirrored; break; 156 | case M3: contexType = M3_mirrored; break; 157 | } break; 158 | case LEG_2: 159 | switch (motorID) { 160 | case M1: contexType = M1_standard; break; 161 | case M2: contexType = M2_standard; break; 162 | case M3: contexType = M3_standard; break; 163 | } break; 164 | case LEG_3: 165 | switch (motorID) { 166 | case M1: contexType = M1_mirrored; break; 167 | case M2: contexType = M2_standard; break; 168 | case M3: contexType = M3_standard; break; 169 | } break; 170 | case LEG_4: 171 | switch (motorID) { 172 | case M1: contexType = M1_standard; break; 173 | case M2: contexType = M2_mirrored; break; 174 | case M3: contexType = M3_mirrored; break; 175 | } break; 176 | 177 | } 178 | 179 | switch (contexType) { 180 | 181 | case M1_standard: angle = _motors[indexOfMotor(legID, motorID)].applicationOffset + angle; break; 182 | case M1_mirrored: angle = _motors[indexOfMotor(legID, motorID)].applicationOffset - angle; break; 183 | 184 | case M2_standard: angle = _motors[indexOfMotor(legID, motorID)].applicationOffset + angle; break; 185 | case M2_mirrored: angle = 90 - demandAngle; break; 186 | 187 | case M3_standard: angle = (2 * _motors[indexOfMotor(legID, motorID)].applicationOffset) - angle; break; 188 | case M3_mirrored: angle = angle; break; 189 | } 190 | 191 | angle = constrainAngle(_motors, legID, motorID, angle); 192 | return angle; 193 | }; 194 | 195 | 196 | uint16_t degreesToMicros(uint8_t inputDegrees, uint8_t calibOffset) { 197 | int microsecondsInput = ((DEGREES_TO_MICROS * inputDegrees) + 500 + calibOffset); // 500 is a "magic number" of micros for the motors; before that they do nothing 198 | return microsecondsInput; 199 | }; 200 | 201 | 202 | // All-in-one function 203 | int16_t getPreparedAngles(Motor _motors[], LegID legID, MotorID motorID, unitType angleUnitType, ActivityType activityType) { 204 | 205 | int16_t angle = 0; 206 | 207 | // The prepared angle in contex 208 | if (activityType == DYNAMIC) 209 | angle = applyContextualOffset(_motors, legID, motorID, _motors[indexOfMotor(legID, motorID)].dynamicDegrees); 210 | else if (activityType == STATIC) 211 | angle = applyContextualOffset(_motors, legID, motorID, _motors[indexOfMotor(legID, motorID)].angleDegrees); 212 | 213 | if (angleUnitType == MILLIS) { 214 | return degreesToMicros(angle, motors[indexOfMotor(legID, motorID)].calibOffset); 215 | } 216 | return angle; 217 | } -------------------------------------------------------------------------------- /Code/Teensy41Main/src/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include // Holds the stuff for constraining, offsetting, and initializing the motor values 5 | #include 6 | 7 | #include 8 | #include "dataEnums.h" 9 | 10 | #include 11 | 12 | // default axis lengths (the 'safe' position for all the motors) 13 | #define DEFAULT_X -30 14 | #define DEFAULT_Y 0 // This is the same as LIMB_1; I want the foot to be directly under the shoulder ie straight, not under the bearing 15 | #define DEFAULT_Z 177 16 | // #define DEFAULT_Z 177 // This is the foot-shoulder length when the leg makes a 45-45-90 triangle 17 | 18 | #define RELAY_PIN 33 19 | 20 | // ******************************* LEG STRUCTURE DECLARATION ******************************** 21 | //Leg 1: 22 | Motor L1M1; 23 | Motor L1M2; 24 | Motor L1M3; 25 | 26 | //Leg 2: 27 | Motor L2M1; 28 | Motor L2M2; 29 | Motor L2M3; 30 | 31 | //Leg 2: 32 | Motor L3M1; 33 | Motor L3M2; 34 | Motor L3M3; 35 | 36 | //Leg 2: 37 | Motor L4M1; 38 | Motor L4M2; 39 | Motor L4M3; 40 | 41 | Motor motors[ROBOT_LEG_COUNT * MOTORS_PER_LEG]; 42 | 43 | Servo L1M1_SERVO; 44 | Servo L1M2_SERVO; 45 | Servo L1M3_SERVO; 46 | 47 | Servo L2M1_SERVO; 48 | Servo L2M2_SERVO; 49 | Servo L2M3_SERVO; 50 | 51 | Servo L3M1_SERVO; 52 | Servo L3M2_SERVO; 53 | Servo L3M3_SERVO; 54 | 55 | Servo L4M1_SERVO; 56 | Servo L4M2_SERVO; 57 | Servo L4M3_SERVO; 58 | 59 | 60 | Quadruped robot; 61 | 62 | const byte radioKey[6] = "00001"; 63 | Transfer controllerData(21, 20, BUFFER_SIZE); 64 | 65 | #define JOYSTICK_ANALOG_CENTER 128 66 | #define JOYSTICK_DESIRED_CENTER 0 67 | 68 | int8_t parseControllerData(uint8_t controllerData) { 69 | return (int8_t)(controllerData - JOYSTICK_ANALOG_CENTER); 70 | } 71 | 72 | void setup() { 73 | Serial.begin(9600); 74 | while (!Serial) 75 | delay(10); 76 | 77 | // Activate motors 78 | pinMode(RELAY_PIN, OUTPUT); 79 | digitalWrite(RELAY_PIN, HIGH); 80 | Serial.println("Relay activated"); 81 | delay(1000); 82 | Serial.println("Relay engaged"); 83 | 84 | initMotorVals(motors); 85 | robot.init(DEFAULT_X, DEFAULT_Y, DEFAULT_Z, motors); 86 | 87 | controllerData.init(0, RECEIVER, radioKey); 88 | 89 | 90 | L1M1_SERVO.attach(motors[indexOfMotor(LEG_1, M1)].controlPin); 91 | L1M2_SERVO.attach(motors[indexOfMotor(LEG_1, M2)].controlPin); 92 | L1M3_SERVO.attach(motors[indexOfMotor(LEG_1, M3)].controlPin); 93 | 94 | L2M1_SERVO.attach(motors[indexOfMotor(LEG_2, M1)].controlPin); 95 | L2M2_SERVO.attach(motors[indexOfMotor(LEG_2, M2)].controlPin); 96 | L2M3_SERVO.attach(motors[indexOfMotor(LEG_2, M3)].controlPin); 97 | 98 | L3M1_SERVO.attach(motors[indexOfMotor(LEG_3, M1)].controlPin); 99 | L3M2_SERVO.attach(motors[indexOfMotor(LEG_3, M2)].controlPin); 100 | L3M3_SERVO.attach(motors[indexOfMotor(LEG_3, M3)].controlPin); 101 | 102 | L4M1_SERVO.attach(motors[indexOfMotor(LEG_4, M1)].controlPin); 103 | L4M2_SERVO.attach(motors[indexOfMotor(LEG_4, M2)].controlPin); 104 | L4M3_SERVO.attach(motors[indexOfMotor(LEG_4, M3)].controlPin); 105 | 106 | // write primary servo positions 107 | L1M1_SERVO.writeMicroseconds(getPreparedAngles(motors, LEG_1, M1, MILLIS, STATIC)); 108 | L1M2_SERVO.writeMicroseconds(getPreparedAngles(motors, LEG_1, M2, MILLIS, STATIC)); 109 | L1M3_SERVO.writeMicroseconds(getPreparedAngles(motors, LEG_1, M3, MILLIS, STATIC)); 110 | 111 | L2M1_SERVO.writeMicroseconds(getPreparedAngles(motors, LEG_2, M1, MILLIS, STATIC)); 112 | L2M2_SERVO.writeMicroseconds(getPreparedAngles(motors, LEG_2, M2, MILLIS, STATIC)); 113 | L2M3_SERVO.writeMicroseconds(getPreparedAngles(motors, LEG_2, M3, MILLIS, STATIC)); 114 | 115 | L3M1_SERVO.writeMicroseconds(getPreparedAngles(motors, LEG_3, M1, MILLIS, STATIC)); 116 | L3M2_SERVO.writeMicroseconds(getPreparedAngles(motors, LEG_3, M2, MILLIS, STATIC)); 117 | L3M3_SERVO.writeMicroseconds(getPreparedAngles(motors, LEG_3, M3, MILLIS, STATIC)); 118 | 119 | L4M1_SERVO.writeMicroseconds(getPreparedAngles(motors, LEG_4, M1, MILLIS, STATIC)); 120 | L4M2_SERVO.writeMicroseconds(getPreparedAngles(motors, LEG_4, M2, MILLIS, STATIC)); 121 | L4M3_SERVO.writeMicroseconds(getPreparedAngles(motors, LEG_4, M3, MILLIS, STATIC)); 122 | 123 | Serial.println(); 124 | 125 | delay(3000); 126 | } 127 | 128 | bool stop = false; 129 | 130 | void loop() { 131 | 132 | controllerData.receive(); 133 | 134 | robot.walk(parseControllerData(controllerData.read(BYTE, JOYL_Y_ANALOG)), parseControllerData(controllerData.read(BYTE, JOYL_X_ANALOG))); 135 | 136 | // static int amount = 0; 137 | // if (Serial.available()) { 138 | // amount = Serial.parseInt(); 139 | // } 140 | 141 | // if (amount != 0 && stop != true) { 142 | // // robot.walk(rampX.update(), 50); 143 | // robot.walk(0, 50); 144 | // } 145 | // // robot.walk(0,0); 146 | // // robot.walk(50, -100); 147 | // else if (amount == 0) 148 | // robot.walk(0, 0); 149 | // // if (((millis() % 15000) == 0) || stop == true) { 150 | // // robot.walk(0, 0); 151 | // // stop = true; 152 | // // } 153 | 154 | // // robot.walk(rampX.update(), 50); 155 | 156 | 157 | 158 | L1M1_SERVO.writeMicroseconds(getPreparedAngles(motors, LEG_1, M1, MILLIS, STATIC)); 159 | L1M2_SERVO.writeMicroseconds(getPreparedAngles(motors, LEG_1, M2, MILLIS, STATIC)); 160 | L1M3_SERVO.writeMicroseconds(getPreparedAngles(motors, LEG_1, M3, MILLIS, STATIC)); 161 | 162 | L2M1_SERVO.writeMicroseconds(getPreparedAngles(motors, LEG_2, M1, MILLIS, STATIC)); 163 | L2M2_SERVO.writeMicroseconds(getPreparedAngles(motors, LEG_2, M2, MILLIS, STATIC)); 164 | L2M3_SERVO.writeMicroseconds(getPreparedAngles(motors, LEG_2, M3, MILLIS, STATIC)); 165 | 166 | L3M1_SERVO.writeMicroseconds(getPreparedAngles(motors, LEG_3, M1, MILLIS, STATIC)); 167 | L3M2_SERVO.writeMicroseconds(getPreparedAngles(motors, LEG_3, M2, MILLIS, STATIC)); 168 | L3M3_SERVO.writeMicroseconds(getPreparedAngles(motors, LEG_3, M3, MILLIS, STATIC)); 169 | 170 | L4M1_SERVO.writeMicroseconds(getPreparedAngles(motors, LEG_4, M1, MILLIS, STATIC)); 171 | L4M2_SERVO.writeMicroseconds(getPreparedAngles(motors, LEG_4, M2, MILLIS, STATIC)); 172 | L4M3_SERVO.writeMicroseconds(getPreparedAngles(motors, LEG_4, M3, MILLIS, STATIC)); 173 | } 174 | 175 | 176 | 177 | 178 | 179 | // /// 180 | // Kinematics leg1(1, motorList) 181 | // leg1.setFootEndpoint(x, y, z) 182 | 183 | // Kinematics legs(motorList) 184 | // for leg in legs: 185 | // leg.setFootEndPoints() 186 | 187 | // Kine::Kine(motors) { 188 | // int length = sizeof() 189 | // for (i = 0; i < length; i++) 190 | // motors[i].angleDegrees = 10; 191 | // } 192 | 193 | 194 | // /// 195 | 196 | 197 | 198 | // #include 199 | 200 | // #include 201 | // #include // Holds the stuff for constraining, offsetting, and initializing the motor values 202 | 203 | // #include 204 | 205 | // #include "Ramp.h" 206 | 207 | // // default axis lengths (the 'safe' position for all the motors) 208 | // #define DEFAULT_X 0 209 | // #define DEFAULT_Y 45 // This is the same as LIMB_1; I want the foot to be directly under the shoulder ie straight, not under the bearing 210 | // #define DEFAULT_Z 177 // This is the foot-shoulder length when the leg makes a 45-45-90 triangle 211 | 212 | // #define RELAY_PIN 33 213 | 214 | // // ******************************* LEG STRUCTURE DECLARATION ******************************** 215 | // //Leg 1: 216 | // Motor L1M1; 217 | // Motor L1M2; 218 | // Motor L1M3; 219 | 220 | // //Leg 2: 221 | // Motor L2M1; 222 | // Motor L2M2; 223 | // Motor L2M3; 224 | 225 | // //Leg 2: 226 | // Motor L3M1; 227 | // Motor L3M2; 228 | // Motor L3M3; 229 | 230 | // //Leg 2: 231 | // Motor L4M1; 232 | // Motor L4M2; 233 | // Motor L4M3; 234 | 235 | // Motor motors[ROBOT_LEG_COUNT * MOTORS_PER_LEG]; 236 | 237 | // Kinematics leg1(LEG_1); 238 | // Kinematics leg2(LEG_2); 239 | // Kinematics leg3(LEG_3); 240 | // Kinematics leg4(LEG_4); 241 | 242 | // Servo L1M1_SERVO; 243 | // Servo L1M2_SERVO; 244 | // Servo L1M3_SERVO; 245 | 246 | // Servo L2M1_SERVO; 247 | // Servo L2M2_SERVO; 248 | // Servo L2M3_SERVO; 249 | 250 | // Servo L3M1_SERVO; 251 | // Servo L3M2_SERVO; 252 | // Servo L3M3_SERVO; 253 | 254 | // Servo L4M1_SERVO; 255 | // Servo L4M2_SERVO; 256 | // Servo L4M3_SERVO; 257 | 258 | // void setup() { 259 | // motors[3].angleDegrees = 10; 260 | // Serial.begin(9600); 261 | // while (!Serial) 262 | // delay(10); 263 | 264 | // // Activate motors 265 | // pinMode(RELAY_PIN, OUTPUT); 266 | // digitalWrite(RELAY_PIN, HIGH); 267 | // Serial.println("Relay activated"); 268 | // delay(1000); 269 | // Serial.println("Relay engaged"); 270 | 271 | 272 | // // Setup motors and initialize kinematics library 273 | // initMotorVals(motors); 274 | // leg1.init(DEFAULT_X, DEFAULT_Y, DEFAULT_Z, motors); 275 | // leg2.init(DEFAULT_X, DEFAULT_Y, DEFAULT_Z, motors); 276 | // leg3.init(DEFAULT_X, DEFAULT_Y, DEFAULT_Z, motors); 277 | // leg4.init(DEFAULT_X, DEFAULT_Y, DEFAULT_Z, motors); 278 | 279 | // L1M1_SERVO.attach(motors[indexOfMotor(LEG_1, M1)].controlPin); 280 | // L1M2_SERVO.attach(motors[indexOfMotor(LEG_1, M2)].controlPin); 281 | // L1M3_SERVO.attach(motors[indexOfMotor(LEG_1, M3)].controlPin); 282 | 283 | // L2M1_SERVO.attach(motors[indexOfMotor(LEG_2, M1)].controlPin); 284 | // L2M2_SERVO.attach(motors[indexOfMotor(LEG_2, M2)].controlPin); 285 | // L2M3_SERVO.attach(motors[indexOfMotor(LEG_2, M3)].controlPin); 286 | 287 | // L3M1_SERVO.attach(motors[indexOfMotor(LEG_3, M1)].controlPin); 288 | // L3M2_SERVO.attach(motors[indexOfMotor(LEG_3, M2)].controlPin); 289 | // L3M3_SERVO.attach(motors[indexOfMotor(LEG_3, M3)].controlPin); 290 | 291 | // L4M1_SERVO.attach(motors[indexOfMotor(LEG_4, M1)].controlPin); 292 | // L4M2_SERVO.attach(motors[indexOfMotor(LEG_4, M2)].controlPin); 293 | // L4M3_SERVO.attach(motors[indexOfMotor(LEG_4, M3)].controlPin); 294 | 295 | // // write primary servo positions 296 | // L1M1_SERVO.writeMicroseconds(getPreparedAngles(motors, LEG_1, M1, MILLIS, STATIC)); 297 | // L1M2_SERVO.writeMicroseconds(getPreparedAngles(motors, LEG_1, M2, MILLIS, STATIC)); 298 | // L1M3_SERVO.writeMicroseconds(getPreparedAngles(motors, LEG_1, M3, MILLIS, STATIC)); 299 | 300 | // L2M1_SERVO.writeMicroseconds(getPreparedAngles(motors, LEG_2, M1, MILLIS, STATIC)); 301 | // L2M2_SERVO.writeMicroseconds(getPreparedAngles(motors, LEG_2, M2, MILLIS, STATIC)); 302 | // L2M3_SERVO.writeMicroseconds(getPreparedAngles(motors, LEG_2, M3, MILLIS, STATIC)); 303 | 304 | // L3M1_SERVO.writeMicroseconds(getPreparedAngles(motors, LEG_3, M1, MILLIS, STATIC)); 305 | // L3M2_SERVO.writeMicroseconds(getPreparedAngles(motors, LEG_3, M2, MILLIS, STATIC)); 306 | // L3M3_SERVO.writeMicroseconds(getPreparedAngles(motors, LEG_3, M3, MILLIS, STATIC)); 307 | 308 | // L4M1_SERVO.writeMicroseconds(getPreparedAngles(motors, LEG_4, M1, MILLIS, STATIC)); 309 | // L4M2_SERVO.writeMicroseconds(getPreparedAngles(motors, LEG_4, M2, MILLIS, STATIC)); 310 | // L4M3_SERVO.writeMicroseconds(getPreparedAngles(motors, LEG_4, M3, MILLIS, STATIC)); 311 | 312 | // Serial.println(); 313 | 314 | // // Serial.println(applyContextualOffset(motors, LEG_1, M1, motors[indexOfMotor(LEG_1, M1)].angleDegrees)); 315 | // // Serial.println(applyContextualOffset(motors, LEG_1, M2, motors[indexOfMotor(LEG_1, M2)].angleDegrees)); 316 | // // Serial.println(applyContextualOffset(motors, LEG_1, M3, motors[indexOfMotor(LEG_1, M3)].angleDegrees)); 317 | 318 | // // Serial.println(applyContextualOffset(motors, LEG_2, M1, motors[indexOfMotor(LEG_2, M1)].angleDegrees)); 319 | // // Serial.println(applyContextualOffset(motors, LEG_2, M2, motors[indexOfMotor(LEG_2, M2)].angleDegrees)); 320 | // // Serial.println(applyContextualOffset(motors, LEG_2, M3, motors[indexOfMotor(LEG_2, M3)].angleDegrees)); 321 | 322 | // delay(3000); 323 | 324 | // } 325 | 326 | // void loop() { 327 | 328 | // // calculateGait(); 329 | 330 | // static int amount = 177; 331 | // if (Serial.available()) { 332 | // amount = Serial.parseInt(); 333 | // // leg2.solveFootPosition(0, amount, 177); // below 334 | // // leg2.setFootEndpoint(amount, 45, DEFAULT_Z); 335 | // } 336 | 337 | // leg1.setFootEndpoint(DEFAULT_X-25, DEFAULT_Y, amount); 338 | // leg2.setFootEndpoint(50, 50, amount); 339 | // // leg2.setFootEndpoint(DEFAULT_X-25, DEFAULT_Y, amount); 340 | // leg3.setFootEndpoint(DEFAULT_X-25, DEFAULT_Y, amount); 341 | // leg4.setFootEndpoint(DEFAULT_X-25, DEFAULT_Y, amount); 342 | 343 | // leg1.updateDynamicFootPosition(); 344 | // leg2.updateDynamicFootPosition(); 345 | // leg3.updateDynamicFootPosition(); 346 | // leg4.updateDynamicFootPosition(); 347 | 348 | // L1M1_SERVO.writeMicroseconds(getPreparedAngles(motors, LEG_1, M1, MILLIS, DYNAMIC)); 349 | // L1M2_SERVO.writeMicroseconds(getPreparedAngles(motors, LEG_1, M2, MILLIS, DYNAMIC)); 350 | // L1M3_SERVO.writeMicroseconds(getPreparedAngles(motors, LEG_1, M3, MILLIS, DYNAMIC)); 351 | 352 | // L2M1_SERVO.writeMicroseconds(getPreparedAngles(motors, LEG_2, M1, MILLIS, DYNAMIC)); 353 | // L2M2_SERVO.writeMicroseconds(getPreparedAngles(motors, LEG_2, M2, MILLIS, DYNAMIC)); 354 | // L2M3_SERVO.writeMicroseconds(getPreparedAngles(motors, LEG_2, M3, MILLIS, DYNAMIC)); 355 | 356 | // L3M1_SERVO.writeMicroseconds(getPreparedAngles(motors, LEG_3, M1, MILLIS, DYNAMIC)); 357 | // L3M2_SERVO.writeMicroseconds(getPreparedAngles(motors, LEG_3, M2, MILLIS, DYNAMIC)); 358 | // L3M3_SERVO.writeMicroseconds(getPreparedAngles(motors, LEG_3, M3, MILLIS, DYNAMIC)); 359 | 360 | // L4M1_SERVO.writeMicroseconds(getPreparedAngles(motors, LEG_4, M1, MILLIS, DYNAMIC)); 361 | // L4M2_SERVO.writeMicroseconds(getPreparedAngles(motors, LEG_4, M2, MILLIS, DYNAMIC)); 362 | // L4M3_SERVO.writeMicroseconds(getPreparedAngles(motors, LEG_4, M3, MILLIS, DYNAMIC)); 363 | // } 364 | 365 | 366 | 367 | 368 | 369 | 370 | 371 | 372 | 373 | // // /// 374 | // // Kinematics leg1(1, motorList) 375 | // // leg1.setFootEndpoint(x, y, z) 376 | 377 | // // Kinematics legs(motorList) 378 | // // for leg in legs: 379 | // // leg.setFootEndPoints() 380 | 381 | // // Kine::Kine(motors) { 382 | // // int length = sizeof() 383 | // // for (i = 0; i < length; i++) 384 | // // motors[i].angleDegrees = 10; 385 | // // } 386 | 387 | 388 | // // /// --------------------------------------------------------------------------------