├── .gitignore ├── extras ├── D11-Firmware │ ├── config.h │ ├── FreeRAM.h │ ├── src │ │ ├── Encoder │ │ │ ├── keywords.txt │ │ │ ├── Encoder.cpp │ │ │ ├── README.md │ │ │ ├── library.properties │ │ │ ├── library.json │ │ │ ├── examples │ │ │ │ ├── Basic │ │ │ │ │ └── Basic.pde │ │ │ │ ├── TwoKnobs │ │ │ │ │ └── TwoKnobs.pde │ │ │ │ ├── NoInterrupts │ │ │ │ │ └── NoInterrupts.pde │ │ │ │ └── SpeedTest │ │ │ │ │ └── SpeedTest.pde │ │ │ └── utility │ │ │ │ ├── interrupt_config.h │ │ │ │ ├── direct_pin_read.h │ │ │ │ └── interrupt_pins.h │ │ └── PID │ │ │ ├── library.properties │ │ │ ├── README.txt │ │ │ ├── library.json │ │ │ ├── keywords.txt │ │ │ ├── PID_v1.h │ │ │ └── PID_v1.cpp │ ├── Battery.h │ ├── ServoMotor.h │ ├── ServoMotor.cpp │ ├── DCMotor.h │ ├── Events.cpp │ ├── DCMotor.cpp │ ├── Events.h │ ├── Battery.cpp │ ├── EncoderWrapper.h │ ├── Common.h │ ├── Encoder.cpp │ ├── PID.cpp │ ├── PID.h │ ├── adc.ino │ └── D11-Firmware.ino └── D11-Bootloader │ ├── nvm.h │ ├── reset.h │ ├── D11-Bootloader.ino │ └── nvm.ino ├── examples ├── MKRMotorCarrier │ ├── Motor_test │ │ ├── CP3-SC1-GE3-PH.png │ │ └── Motor_test.ino │ ├── Servo_test │ │ ├── CP3-SC2-GE3-IL.png │ │ └── Servo_test.ino │ ├── Motor_test_encoder │ │ ├── CP3-SC1-GE3-PH.png │ │ └── Motor_test_encoder.ino │ └── Test │ │ └── Test.ino ├── NanoMotorCarrier │ ├── Battery_Charging │ │ └── Battery_Charging.ino │ ├── EncoderTest │ │ └── EncoderTest.ino │ ├── ServoTest │ │ └── ServoTest.ino │ ├── DCMotorTest │ │ └── DCMotorTest.ino │ ├── IMU_Test │ │ └── IMU_Test.ino │ └── PID_Position_test │ │ └── PID_Position_test.ino └── Flasher │ └── Flasher.ino ├── .codespellrc ├── library.properties ├── src ├── src │ ├── libfixmath │ │ ├── uint32.h │ │ ├── fixmath.h │ │ ├── uint32.c │ │ ├── fract32.c │ │ ├── Makefile │ │ ├── fract32.h │ │ ├── fix16_sqrt.c │ │ ├── fix16_str.c │ │ ├── int64.h │ │ ├── fix16_exp.c │ │ └── fix16_trig.c │ └── PID │ │ ├── library.properties │ │ ├── README.txt │ │ ├── library.json │ │ ├── keywords.txt │ │ └── PID_v1.h ├── Battery.h ├── NanoMotorCarrier.h ├── MKRMotorCarrier.h ├── ServoMotor.h ├── Encoder.h ├── Battery.cpp ├── MotorController.h ├── ServoMotor.cpp ├── DCMotor.h ├── ArduinoMotorCarrier.h ├── Encoder.cpp ├── DCMotor.cpp ├── Common.h ├── ArduinoMotorCarrier.cpp ├── MotorController.cpp ├── PID.h └── PID.cpp ├── .github ├── dependabot.yml └── workflows │ ├── spell-check.yml │ ├── check-arduino.yml │ ├── report-size-deltas.yml │ ├── compile-firmware.yml │ ├── compile-bootloader.yml │ ├── compile-examples.yml │ └── sync-labels.yml ├── docs ├── readme.md └── api.md ├── keywords.txt ├── README.md └── LICENSE /.gitignore: -------------------------------------------------------------------------------- 1 | .idea/ 2 | -------------------------------------------------------------------------------- /extras/D11-Firmware/config.h: -------------------------------------------------------------------------------- 1 | // uncomment to compile for Nano Motor Carrier 2 | 3 | // #define NANO_MOTOR_CARRIER 4 | -------------------------------------------------------------------------------- /extras/D11-Firmware/FreeRAM.h: -------------------------------------------------------------------------------- 1 | extern "C" char *sbrk(int i); 2 | int FreeRam (void) 3 | { 4 | char stack_dummy = 0; 5 | return &stack_dummy - sbrk(0); 6 | } 7 | -------------------------------------------------------------------------------- /examples/MKRMotorCarrier/Motor_test/CP3-SC1-GE3-PH.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/arduino-libraries/ArduinoMotorCarrier/HEAD/examples/MKRMotorCarrier/Motor_test/CP3-SC1-GE3-PH.png -------------------------------------------------------------------------------- /examples/MKRMotorCarrier/Servo_test/CP3-SC2-GE3-IL.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/arduino-libraries/ArduinoMotorCarrier/HEAD/examples/MKRMotorCarrier/Servo_test/CP3-SC2-GE3-IL.png -------------------------------------------------------------------------------- /extras/D11-Firmware/src/Encoder/keywords.txt: -------------------------------------------------------------------------------- 1 | ENCODER_USE_INTERRUPTS LITERAL1 2 | ENCODER_OPTIMIZE_INTERRUPTS LITERAL1 3 | ENCODER_DO_NOT_USE_INTERRUPTS LITERAL1 4 | Encoder KEYWORD1 5 | -------------------------------------------------------------------------------- /examples/MKRMotorCarrier/Motor_test_encoder/CP3-SC1-GE3-PH.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/arduino-libraries/ArduinoMotorCarrier/HEAD/examples/MKRMotorCarrier/Motor_test_encoder/CP3-SC1-GE3-PH.png -------------------------------------------------------------------------------- /extras/D11-Firmware/Battery.h: -------------------------------------------------------------------------------- 1 | #include "Arduino.h" 2 | 3 | class Battery { 4 | public: 5 | Battery(int pinA); 6 | void getRaw(); 7 | void getConverted(); 8 | void getFiltered(); 9 | void readBattery(); 10 | private: 11 | int pin; 12 | int readBuf[10]; 13 | uint8_t index; 14 | }; 15 | -------------------------------------------------------------------------------- /.codespellrc: -------------------------------------------------------------------------------- 1 | # See: https://github.com/codespell-project/codespell#using-a-config-file 2 | [codespell] 3 | # In the event of a false positive, add the problematic word, in all lowercase, to a comma-separated list here: 4 | ignore-words-list = , 5 | check-filenames = 6 | check-hidden = 7 | skip = ./.git,./extras/D11-Firmware/src,./src/src 8 | -------------------------------------------------------------------------------- /extras/D11-Firmware/src/Encoder/Encoder.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "Encoder.h" 3 | 4 | // Yes, all the code is in the header file, to provide the user 5 | // configure options with #define (before they include it), and 6 | // to facilitate some crafty optimizations! 7 | 8 | Encoder_internal_state_t * Encoder::interruptArgs[]; 9 | 10 | 11 | -------------------------------------------------------------------------------- /extras/D11-Firmware/ServoMotor.h: -------------------------------------------------------------------------------- 1 | #include "Arduino.h" 2 | 3 | class ServoMotor { 4 | public: 5 | ServoMotor(int pinA) : pin(pinA) { 6 | pinMode(pinA, OUTPUT); 7 | }; 8 | void setDuty(int duty); 9 | void setFrequency(int frequency); 10 | private: 11 | int pin; 12 | int duty = 0; 13 | int frequency = 50; 14 | }; 15 | -------------------------------------------------------------------------------- /extras/D11-Firmware/ServoMotor.cpp: -------------------------------------------------------------------------------- 1 | #include "ServoMotor.h" 2 | 3 | void ServoMotor::setDuty(int duty) { 4 | this->duty = duty; 5 | if (duty < 0) { 6 | pinMode(pin, INPUT); 7 | } else { 8 | analogWrite(pin, duty); 9 | } 10 | } 11 | 12 | void ServoMotor::setFrequency(int frequency) { 13 | // NB: not implemented at the moment! 14 | //this->frequency = frequency; 15 | } 16 | -------------------------------------------------------------------------------- /library.properties: -------------------------------------------------------------------------------- 1 | name=ArduinoMotorCarrier 2 | version=2.0.3 3 | author=Arduino 4 | maintainer=Arduino 5 | sentence=Allows use of the Arduino Motor Carrier 6 | paragraph=(Nano and MKR version) 7 | category=Signal Input/Output 8 | url=https://www.arduino.cc/reference/en/libraries/arduinomotorcarrier/ 9 | architectures=samd,mbed_portenta 10 | includes=ArduinoMotorCarrier.h 11 | -------------------------------------------------------------------------------- /extras/D11-Firmware/src/Encoder/README.md: -------------------------------------------------------------------------------- 1 | # Encoder Library 2 | 3 | Encoder counts pulses from quadrature encoded signals, which are commonly available from rotary knobs, motor or shaft sensors and other position sensors. 4 | 5 | http://www.pjrc.com/teensy/td_libs_Encoder.html 6 | 7 | http://www.youtube.com/watch?v=2puhIong-cs 8 | 9 | ![Encoder Knobs Demo](http://www.pjrc.com/teensy/td_libs_Encoder_1.jpg) 10 | -------------------------------------------------------------------------------- /src/src/libfixmath/uint32.h: -------------------------------------------------------------------------------- 1 | #ifndef __libfixmath_uint32_h__ 2 | #define __libfixmath_uint32_h__ 3 | 4 | #ifdef __cplusplus 5 | extern "C" 6 | { 7 | #endif 8 | 9 | #include 10 | 11 | /*! Performs an unsigned log-base2 on the specified unsigned integer and returns the result. 12 | */ 13 | extern uint32_t uint32_log2(uint32_t inVal); 14 | 15 | #ifdef __cplusplus 16 | } 17 | #endif 18 | 19 | #endif 20 | -------------------------------------------------------------------------------- /src/src/PID/library.properties: -------------------------------------------------------------------------------- 1 | name=PID 2 | version=1.2.1 3 | author=Brett Beauregard 4 | maintainer=Brett Beauregard 5 | sentence=PID controller 6 | paragraph=A PID controller seeks to keep some input variable close to a desired setpoint by adjusting an output. The way in which it does this can be 'tuned' by adjusting three parameters (P,I,D). 7 | category=Signal Input/Output 8 | url=http://playground.arduino.cc/Code/PIDLibrary 9 | architectures=* 10 | -------------------------------------------------------------------------------- /extras/D11-Firmware/src/PID/library.properties: -------------------------------------------------------------------------------- 1 | name=PID 2 | version=1.2.1 3 | author=Brett Beauregard 4 | maintainer=Brett Beauregard 5 | sentence=PID controller 6 | paragraph=A PID controller seeks to keep some input variable close to a desired setpoint by adjusting an output. The way in which it does this can be 'tuned' by adjusting three parameters (P,I,D). 7 | category=Signal Input/Output 8 | url=http://playground.arduino.cc/Code/PIDLibrary 9 | architectures=* 10 | -------------------------------------------------------------------------------- /src/src/libfixmath/fixmath.h: -------------------------------------------------------------------------------- 1 | #ifndef __libfixmath_fixmath_h__ 2 | #define __libfixmath_fixmath_h__ 3 | 4 | #ifdef __cplusplus 5 | extern "C" 6 | { 7 | #endif 8 | 9 | /*! 10 | \file fixmath.h 11 | \brief Functions to perform fast accurate fixed-point math operations. 12 | */ 13 | 14 | #include "uint32.h" 15 | #include "int64.h" 16 | #include "fract32.h" 17 | #include "fix16.h" 18 | 19 | #ifdef __cplusplus 20 | } 21 | #endif 22 | 23 | #endif 24 | -------------------------------------------------------------------------------- /extras/D11-Firmware/src/Encoder/library.properties: -------------------------------------------------------------------------------- 1 | name=Encoder 2 | version=1.4.1 3 | author=Paul Stoffregen 4 | maintainer=Paul Stoffregen 5 | sentence=Counts quadrature pulses from rotary & linear position encoders. 6 | paragraph=Encoder counts pulses from quadrature encoded signals, which are commonly available from rotary knobs, motor or shaft sensors and other position sensors. 7 | category=Signal Input/Output 8 | url=http://www.pjrc.com/teensy/td_libs_Encoder.html 9 | architectures=* 10 | 11 | -------------------------------------------------------------------------------- /extras/D11-Bootloader/nvm.h: -------------------------------------------------------------------------------- 1 | #define APP_START 0x00001000 //This gives 1536 bytes of bootloader space. 2 | //#define FLASH_SIZE 0x00004000 //This gives 1536 bytes of bootloader space. 3 | 4 | /* Target application size can be 15 kB */ 5 | /* APP_SIZE is the application section size in kB */ 6 | /* Change as per APP_START */ 7 | #define APP_SIZE 5 //This is how much flash memory is left for the application. 8 | 9 | /* Memory pointer for flash memory */ 10 | #define NVM_MEMORY ((volatile uint16_t *)FLASH_ADDR) 11 | -------------------------------------------------------------------------------- /extras/D11-Firmware/DCMotor.h: -------------------------------------------------------------------------------- 1 | #ifndef __DC_MOTOR_H__ 2 | #define __DC_MOTOR_H__ 3 | 4 | #include "Arduino.h" 5 | 6 | class DCMotor { 7 | public: 8 | DCMotor(Tc* tcc, int pinA, int pinB) : in1(pinA), in2(pinB) { 9 | pinMode(in1, OUTPUT); 10 | pinMode(in2, OUTPUT); 11 | }; 12 | void setDuty(int duty); 13 | void setFrequency(int frequency); 14 | int duty = 0; 15 | void* pid; 16 | private: 17 | int in1; 18 | int in2; 19 | int frequency = 100; 20 | }; 21 | 22 | #endif 23 | -------------------------------------------------------------------------------- /src/src/libfixmath/uint32.c: -------------------------------------------------------------------------------- 1 | #include "uint32.h" 2 | 3 | 4 | 5 | uint32_t uint32_log2(uint32_t inVal) { 6 | if(inVal == 0) 7 | return 0; 8 | uint32_t tempOut = 0; 9 | if(inVal >= (1 << 16)) { inVal >>= 16; tempOut += 16; } 10 | if(inVal >= (1 << 8)) { inVal >>= 8; tempOut += 8; } 11 | if(inVal >= (1 << 4)) { inVal >>= 4; tempOut += 4; } 12 | if(inVal >= (1 << 2)) { inVal >>= 2; tempOut += 2; } 13 | if(inVal >= (1 << 1)) { tempOut += 1; } 14 | return tempOut; 15 | } 16 | -------------------------------------------------------------------------------- /extras/D11-Firmware/src/Encoder/library.json: -------------------------------------------------------------------------------- 1 | { 2 | "name": "Encoder", 3 | "keywords": "encoder, signal, pulse", 4 | "description": "Encoder counts pulses from quadrature encoded signals, which are commonly available from rotary knobs, motor or shaft sensors and other position sensors", 5 | "repository": 6 | { 7 | "type": "git", 8 | "url": "https://github.com/PaulStoffregen/Encoder.git" 9 | }, 10 | "frameworks": "arduino", 11 | "platforms": 12 | [ 13 | "atmelavr", 14 | "teensy" 15 | ] 16 | } 17 | -------------------------------------------------------------------------------- /extras/D11-Firmware/Events.cpp: -------------------------------------------------------------------------------- 1 | //#include "LinkedList.h" 2 | #include "Events.h" 3 | 4 | static uint8_t counter = 0; 5 | 6 | TimedEvent* events[10] = {NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL}; 7 | 8 | void executeTimedEvents() { 9 | for (int i = 0; events[i] != NULL; i++) { 10 | events[i]->tryExec(); 11 | } 12 | } 13 | 14 | void registerTimedEvent(void(*callback)(void* args), void* args, int howOften) { 15 | TimedEvent* evt = new TimedEvent(callback, args, howOften); 16 | events[counter++] = evt; 17 | } 18 | -------------------------------------------------------------------------------- /src/src/PID/README.txt: -------------------------------------------------------------------------------- 1 | *************************************************************** 2 | * Arduino PID Library - Version 1.2.1 3 | * by Brett Beauregard brettbeauregard.com 4 | * 5 | * This Library is licensed under the MIT License 6 | *************************************************************** 7 | 8 | - For an ultra-detailed explanation of why the code is the way it is, please visit: 9 | http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-introduction/ 10 | 11 | - For function documentation see: http://playground.arduino.cc/Code/PIDLibrary 12 | -------------------------------------------------------------------------------- /extras/D11-Firmware/src/PID/README.txt: -------------------------------------------------------------------------------- 1 | *************************************************************** 2 | * Arduino PID Library - Version 1.2.1 3 | * by Brett Beauregard brettbeauregard.com 4 | * 5 | * This Library is licensed under the MIT License 6 | *************************************************************** 7 | 8 | - For an ultra-detailed explanation of why the code is the way it is, please visit: 9 | http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-introduction/ 10 | 11 | - For function documentation see: http://playground.arduino.cc/Code/PIDLibrary 12 | -------------------------------------------------------------------------------- /.github/dependabot.yml: -------------------------------------------------------------------------------- 1 | # See: https://docs.github.com/en/github/administering-a-repository/configuration-options-for-dependency-updates#about-the-dependabotyml-file 2 | version: 2 3 | 4 | updates: 5 | # Configure check for outdated GitHub Actions actions in workflows. 6 | # See: https://docs.github.com/en/github/administering-a-repository/keeping-your-actions-up-to-date-with-dependabot 7 | - package-ecosystem: github-actions 8 | directory: / # Check the repository's workflows under /.github/workflows/ 9 | schedule: 10 | interval: daily 11 | labels: 12 | - "topic: infrastructure" 13 | -------------------------------------------------------------------------------- /extras/D11-Firmware/DCMotor.cpp: -------------------------------------------------------------------------------- 1 | #include "DCMotor.h" 2 | 3 | void DCMotor::setDuty(int duty) { 4 | this->duty = duty; 5 | if (duty == 0) { 6 | analogWrite(in1, 0); 7 | analogWrite(in2, 0); 8 | return; 9 | } 10 | 11 | // scale duty to period 12 | duty = duty * 255 / 100; 13 | 14 | if (duty > 0) { 15 | analogWrite(in1, 0); 16 | analogWrite(in2, duty); 17 | } else { 18 | analogWrite(in2, 0); 19 | analogWrite(in1, -duty); 20 | } 21 | } 22 | 23 | void DCMotor::setFrequency(int frequency) { 24 | // NB: not implemented at the moment! 25 | //this->frequency = frequency; 26 | } 27 | -------------------------------------------------------------------------------- /src/src/PID/library.json: -------------------------------------------------------------------------------- 1 | { 2 | "name": "PID", 3 | "keywords": "PID, controller, signal", 4 | "description": "A PID controller seeks to keep some input variable close to a desired setpoint by adjusting an output. The way in which it does this can be 'tuned' by adjusting three parameters (P,I,D).", 5 | "url": "http://playground.arduino.cc/Code/PIDLibrary", 6 | "include": "PID_v1", 7 | "authors": 8 | [ 9 | { 10 | "name": "Brett Beauregard" 11 | } 12 | ], 13 | "repository": 14 | { 15 | "type": "git", 16 | "url": "https://github.com/br3ttb/Arduino-PID-Library.git" 17 | }, 18 | "frameworks": "arduino" 19 | } 20 | -------------------------------------------------------------------------------- /extras/D11-Firmware/src/PID/library.json: -------------------------------------------------------------------------------- 1 | { 2 | "name": "PID", 3 | "keywords": "PID, controller, signal", 4 | "description": "A PID controller seeks to keep some input variable close to a desired setpoint by adjusting an output. The way in which it does this can be 'tuned' by adjusting three parameters (P,I,D).", 5 | "url": "http://playground.arduino.cc/Code/PIDLibrary", 6 | "include": "PID_v1", 7 | "authors": 8 | [ 9 | { 10 | "name": "Brett Beauregard" 11 | } 12 | ], 13 | "repository": 14 | { 15 | "type": "git", 16 | "url": "https://github.com/br3ttb/Arduino-PID-Library.git" 17 | }, 18 | "frameworks": "arduino" 19 | } 20 | -------------------------------------------------------------------------------- /.github/workflows/spell-check.yml: -------------------------------------------------------------------------------- 1 | name: Spell Check 2 | 3 | # See: https://docs.github.com/en/free-pro-team@latest/actions/reference/events-that-trigger-workflows 4 | on: 5 | push: 6 | pull_request: 7 | schedule: 8 | # Run every Tuesday at 8 AM UTC to catch new misspelling detections resulting from dictionary updates. 9 | - cron: "0 8 * * TUE" 10 | workflow_dispatch: 11 | repository_dispatch: 12 | 13 | jobs: 14 | spellcheck: 15 | runs-on: ubuntu-latest 16 | 17 | steps: 18 | - name: Checkout repository 19 | uses: actions/checkout@v6 20 | 21 | - name: Spell check 22 | uses: codespell-project/actions-codespell@master 23 | -------------------------------------------------------------------------------- /extras/D11-Firmware/Events.h: -------------------------------------------------------------------------------- 1 | #include "Arduino.h" 2 | 3 | class TimedEvent { 4 | public: 5 | TimedEvent(void(*cb)(void* args), void* _args, int _howOften) { 6 | callback = cb; 7 | args = _args; 8 | howOften = _howOften; 9 | } 10 | void tryExec() { 11 | if (millis() < nextExecution && (nextExecution != 0)) { 12 | return; 13 | } 14 | callback(args); 15 | reschedule(); 16 | } 17 | private: 18 | void(*callback)(void* args) = NULL; 19 | void* args; 20 | long howOften; 21 | unsigned long nextExecution = 0; 22 | void reschedule() { 23 | nextExecution = millis() + howOften; 24 | }; 25 | }; 26 | 27 | void executeTimedEvents(); 28 | void registerTimedEvent(void(*callback)(void* args), void* arg, int howOften); 29 | -------------------------------------------------------------------------------- /src/src/libfixmath/fract32.c: -------------------------------------------------------------------------------- 1 | #include "fract32.h" 2 | 3 | 4 | 5 | fract32_t fract32_create(uint32_t inNumerator, uint32_t inDenominator) { 6 | if(inDenominator <= inNumerator) 7 | return 0xFFFFFFFF; 8 | uint32_t tempMod = (inNumerator % inDenominator); 9 | uint32_t tempDiv = (0xFFFFFFFF / (inDenominator - 1)); 10 | return (tempMod * tempDiv); 11 | } 12 | 13 | fract32_t fract32_invert(fract32_t inFract) { 14 | return (0xFFFFFFFF - inFract); 15 | } 16 | 17 | #ifndef FIXMATH_NO_64BIT 18 | uint32_t fract32_usmul(uint32_t inVal, fract32_t inFract) { 19 | return (uint32_t)(((uint64_t)inVal * (uint64_t)inFract) >> 32); 20 | } 21 | 22 | int32_t fract32_smul(int32_t inVal, fract32_t inFract) { 23 | if(inVal < 0) 24 | return -(int32_t)fract32_usmul(-inVal, inFract); 25 | return fract32_usmul(inVal, inFract); 26 | } 27 | #endif 28 | -------------------------------------------------------------------------------- /.github/workflows/check-arduino.yml: -------------------------------------------------------------------------------- 1 | name: Check Arduino 2 | 3 | # See: https://docs.github.com/en/free-pro-team@latest/actions/reference/events-that-trigger-workflows 4 | on: 5 | push: 6 | pull_request: 7 | schedule: 8 | # Run every Tuesday at 8 AM UTC to catch breakage caused by new rules added to Arduino Lint. 9 | - cron: "0 8 * * TUE" 10 | workflow_dispatch: 11 | repository_dispatch: 12 | 13 | jobs: 14 | lint: 15 | runs-on: ubuntu-latest 16 | 17 | steps: 18 | - name: Checkout repository 19 | uses: actions/checkout@v6 20 | 21 | - name: Arduino Lint 22 | uses: arduino/arduino-lint-action@v2 23 | with: 24 | compliance: specification 25 | library-manager: update 26 | # Always use this setting for official repositories. Remove for 3rd party projects. 27 | official: true 28 | project-type: library 29 | -------------------------------------------------------------------------------- /extras/D11-Firmware/src/Encoder/examples/Basic/Basic.pde: -------------------------------------------------------------------------------- 1 | /* Encoder Library - Basic Example 2 | * http://www.pjrc.com/teensy/td_libs_Encoder.html 3 | * 4 | * This example code is in the public domain. 5 | */ 6 | 7 | #include 8 | 9 | // Change these two numbers to the pins connected to your encoder. 10 | // Best Performance: both pins have interrupt capability 11 | // Good Performance: only the first pin has interrupt capability 12 | // Low Performance: neither pin has interrupt capability 13 | Encoder myEnc(5, 6); 14 | // avoid using pins with LEDs attached 15 | 16 | void setup() { 17 | Serial.begin(9600); 18 | Serial.println("Basic Encoder Test:"); 19 | } 20 | 21 | long oldPosition = -999; 22 | 23 | void loop() { 24 | long newPosition = myEnc.read(); 25 | if (newPosition != oldPosition) { 26 | oldPosition = newPosition; 27 | Serial.println(newPosition); 28 | } 29 | } 30 | -------------------------------------------------------------------------------- /extras/D11-Bootloader/reset.h: -------------------------------------------------------------------------------- 1 | enum system_reset_cause { 2 | /** The system was last reset by a software reset. */ 3 | SYSTEM_RESET_CAUSE_SOFTWARE = PM_RCAUSE_SYST, 4 | /** The system was last reset by the watchdog timer. */ 5 | SYSTEM_RESET_CAUSE_WDT = PM_RCAUSE_WDT, 6 | /** The system was last reset because the external reset line was pulled low. */ 7 | SYSTEM_RESET_CAUSE_EXTERNAL_RESET = PM_RCAUSE_EXT, 8 | /** The system was last reset by the BOD33. */ 9 | SYSTEM_RESET_CAUSE_BOD33 = PM_RCAUSE_BOD33, 10 | /** The system was last reset by the BOD12. */ 11 | SYSTEM_RESET_CAUSE_BOD12 = PM_RCAUSE_BOD12, 12 | /** The system was last reset by the POR (Power on reset). */ 13 | SYSTEM_RESET_CAUSE_POR = PM_RCAUSE_POR, 14 | }; 15 | 16 | static inline enum system_reset_cause system_get_reset_cause(void) 17 | { 18 | return (enum system_reset_cause)PM->RCAUSE.reg; 19 | } 20 | -------------------------------------------------------------------------------- /src/src/PID/keywords.txt: -------------------------------------------------------------------------------- 1 | ####################################### 2 | # Syntax Coloring Map For PID Library 3 | ####################################### 4 | 5 | ####################################### 6 | # Datatypes (KEYWORD1) 7 | ####################################### 8 | 9 | PID KEYWORD1 10 | 11 | ####################################### 12 | # Methods and Functions (KEYWORD2) 13 | ####################################### 14 | 15 | SetMode KEYWORD2 16 | Compute KEYWORD2 17 | SetOutputLimits KEYWORD2 18 | SetTunings KEYWORD2 19 | SetControllerDirection KEYWORD2 20 | SetSampleTime KEYWORD2 21 | GetKp KEYWORD2 22 | GetKi KEYWORD2 23 | GetKd KEYWORD2 24 | GetMode KEYWORD2 25 | GetDirection KEYWORD2 26 | 27 | ####################################### 28 | # Constants (LITERAL1) 29 | ####################################### 30 | 31 | AUTOMATIC LITERAL1 32 | MANUAL LITERAL1 33 | DIRECT LITERAL1 34 | REVERSE LITERAL1 35 | P_ON_E LITERAL1 36 | P_ON_M LITERAL1 37 | -------------------------------------------------------------------------------- /extras/D11-Firmware/src/PID/keywords.txt: -------------------------------------------------------------------------------- 1 | ####################################### 2 | # Syntax Coloring Map For PID Library 3 | ####################################### 4 | 5 | ####################################### 6 | # Datatypes (KEYWORD1) 7 | ####################################### 8 | 9 | PID KEYWORD1 10 | 11 | ####################################### 12 | # Methods and Functions (KEYWORD2) 13 | ####################################### 14 | 15 | SetMode KEYWORD2 16 | Compute KEYWORD2 17 | SetOutputLimits KEYWORD2 18 | SetTunings KEYWORD2 19 | SetControllerDirection KEYWORD2 20 | SetSampleTime KEYWORD2 21 | GetKp KEYWORD2 22 | GetKi KEYWORD2 23 | GetKd KEYWORD2 24 | GetMode KEYWORD2 25 | GetDirection KEYWORD2 26 | 27 | ####################################### 28 | # Constants (LITERAL1) 29 | ####################################### 30 | 31 | AUTOMATIC LITERAL1 32 | MANUAL LITERAL1 33 | DIRECT LITERAL1 34 | REVERSE LITERAL1 35 | P_ON_E LITERAL1 36 | P_ON_M LITERAL1 37 | -------------------------------------------------------------------------------- /.github/workflows/report-size-deltas.yml: -------------------------------------------------------------------------------- 1 | name: Report Size Deltas 2 | 3 | # See: https://docs.github.com/en/free-pro-team@latest/actions/reference/events-that-trigger-workflows 4 | on: 5 | push: 6 | paths: 7 | - ".github/workflows/report-size-deltas.yml" 8 | schedule: 9 | # Run at the minimum interval allowed by GitHub Actions. 10 | # Note: GitHub Actions periodically has outages which result in workflow failures. 11 | # In this event, the workflows will start passing again once the service recovers. 12 | - cron: "*/5 * * * *" 13 | workflow_dispatch: 14 | repository_dispatch: 15 | 16 | jobs: 17 | report: 18 | runs-on: ubuntu-latest 19 | steps: 20 | - name: Comment size deltas reports to PRs 21 | uses: arduino/report-size-deltas@v1 22 | with: 23 | # Regex matching the names of the workflow artifacts created by the "Compile Examples" workflow 24 | sketches-reports-source: ^sketches-report-.+ 25 | -------------------------------------------------------------------------------- /extras/D11-Firmware/Battery.cpp: -------------------------------------------------------------------------------- 1 | #include "Battery.h" 2 | #include "Events.h" 3 | #include "Wire.h" 4 | #include "config.h" 5 | 6 | #ifdef NANO_MOTOR_CARRIER 7 | #define SCALE_FACTOR (236) 8 | #else 9 | #define SCALE_FACTOR (77) 10 | #endif 11 | 12 | void Battery::readBattery() { 13 | index++; 14 | index = index%10; 15 | readBuf[index] = (int)analogRead(pin); 16 | } 17 | 18 | void readBattery_wrapper(void* arg) 19 | { 20 | Battery* obj = (Battery*)arg; 21 | obj->readBattery(); 22 | } 23 | 24 | Battery::Battery(int pinA) { 25 | pin = pinA; 26 | registerTimedEvent(readBattery_wrapper, this, 1000); 27 | readBuf[0] = (int)analogRead(pin); 28 | } 29 | 30 | void Battery::getRaw() { 31 | Wire.write(readBuf[index]); 32 | } 33 | 34 | void Battery::getConverted() { 35 | Wire.write(readBuf[index] / SCALE_FACTOR); 36 | } 37 | 38 | void Battery::getFiltered() { 39 | long total = 0; 40 | for (int i = 0; i < 10; i++) { 41 | total += readBuf[i]; 42 | } 43 | Wire.write((int)(total/(10*SCALE_FACTOR))); 44 | } 45 | -------------------------------------------------------------------------------- /examples/NanoMotorCarrier/Battery_Charging/Battery_Charging.ino: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | 4 | //Variable to store the battery voltage 5 | float batteryVoltage; 6 | 7 | // the setup function runs once when you press reset or power the board 8 | void setup() { 9 | Serial.begin(115200); 10 | //while (!Serial); 11 | 12 | if (controller.begin()) 13 | { 14 | Serial.print("Nano Motor Shield connected, firmware version "); 15 | Serial.println(controller.getFWVersion()); 16 | } 17 | else 18 | { 19 | Serial.println("Couldn't connect! Is the red led blinking? You may need to update the firmware with FWUpdater sketch"); 20 | while (1); 21 | } 22 | } 23 | 24 | // the loop function runs over and over again forever 25 | void loop() { 26 | 27 | batteryVoltage = battery.getRaw()/236.0; 28 | Serial.print("Battery voltage: "); 29 | Serial.print(batteryVoltage,3); 30 | //Serial.println("V"); 31 | Serial.print("V, Raw "); 32 | Serial.println(battery.getRaw()); 33 | delay(5000); //wait for a few seconds 34 | 35 | } 36 | -------------------------------------------------------------------------------- /src/Battery.h: -------------------------------------------------------------------------------- 1 | /* 2 | Battery.h - Library for Arduino Motor Carriers 3 | Copyright (c) 2018-2019 Arduino AG. All right reserved. 4 | This library is free software; you can redistribute it and/or 5 | modify it under the terms of the GNU Lesser General Public 6 | License as published by the Free Software Foundation; either 7 | version 2.1 of the License, or (at your option) any later version. 8 | This library is distributed in the hope that it will be useful, 9 | but WITHOUT ANY WARRANTY; without even the implied warranty of 10 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 11 | Lesser General Public License for more details. 12 | You should have received a copy of the GNU Lesser General Public 13 | License along with this library; if not, write to the Free Software 14 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 15 | */ 16 | 17 | #include "Arduino.h" 18 | 19 | namespace mc { 20 | 21 | class Battery { 22 | public: 23 | Battery(); 24 | int getRaw(); 25 | int getConverted(); 26 | int getFiltered(); 27 | }; 28 | } 29 | -------------------------------------------------------------------------------- /src/NanoMotorCarrier.h: -------------------------------------------------------------------------------- 1 | /*- Library for Arduino Motor Carriers 2 | Copyright (c) 2018-2019 Arduino AG. All right reserved. 3 | This library is free software; you can redistribute it and/or 4 | modify it under the terms of the GNU Lesser General Public 5 | License as published by the Free Software Foundation; either 6 | version 2.1 of the License, or (at your option) any later version. 7 | This library is distributed in the hope that it will be useful, 8 | but WITHOUT ANY WARRANTY; without even the implied warranty of 9 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 10 | Lesser General Public License for more details. 11 | You should have received a copy of the GNU Lesser General Public 12 | License along with this library; if not, write to the Free Software 13 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 14 | */ 15 | 16 | #ifndef __NANOMOTORSHIELD__ 17 | #define __NANOMOTORSHIELD__ 18 | 19 | #warning "This header is going to be deprecated please use ArduinoMotorCarrier.h" 20 | #include "ArduinoMotorCarrier.h" 21 | 22 | #endif 23 | -------------------------------------------------------------------------------- /src/MKRMotorCarrier.h: -------------------------------------------------------------------------------- 1 | /* 2 | MKRMotorCarrier.h - Library for Arduino Motor Carriers 3 | Copyright (c) 2018-2019 Arduino AG. All right reserved. 4 | This library is free software; you can redistribute it and/or 5 | modify it under the terms of the GNU Lesser General Public 6 | License as published by the Free Software Foundation; either 7 | version 2.1 of the License, or (at your option) any later version. 8 | This library is distributed in the hope that it will be useful, 9 | but WITHOUT ANY WARRANTY; without even the implied warranty of 10 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 11 | Lesser General Public License for more details. 12 | You should have received a copy of the GNU Lesser General Public 13 | License along with this library; if not, write to the Free Software 14 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 15 | */ 16 | 17 | #ifndef __MKRMOTORSHIELD__ 18 | #define __MKRMOTORSHIELD__ 19 | 20 | #warning "This header is going to be deprecated please use ArduinoMotorCarrier.h" 21 | #include "ArduinoMotorCarrier.h" 22 | 23 | #endif 24 | -------------------------------------------------------------------------------- /extras/D11-Firmware/EncoderWrapper.h: -------------------------------------------------------------------------------- 1 | #include "src/Encoder/Encoder.h" 2 | #include "src/FpF.hpp" 3 | 4 | #define Fix16 mn::MFixedPoint::FpF32<8> 5 | 6 | class EncoderWrapper { 7 | public: 8 | EncoderWrapper(int pinA, int pinB, int index); 9 | void getRawCount(); 10 | void getOverflowUnderflow(); 11 | void getCountPerSecond(); 12 | void resetCounter(long value); 13 | int read(); 14 | void setIrqOnCount(long value) { 15 | targetCount = value; 16 | irqCountEnabled = true; 17 | } 18 | void setIrqOnVelocity(long value) { 19 | if (value != 0) { 20 | targetVelocity = Fix16(value & 0xFFFFFF); 21 | //irqRatio = Fix16(value >> 24) / 100.0f; 22 | irqVelocityEnabled = true; 23 | } else { 24 | irqVelocityEnabled = false; 25 | } 26 | } 27 | bool underflow; 28 | bool overflow; 29 | Fix16 velocity; 30 | Fix16 position; 31 | Fix16 irqRatio = 2.0f; 32 | bool irqCountEnabled = false; 33 | bool irqVelocityEnabled = false; 34 | int targetCount = -1; 35 | Fix16 targetVelocity = -1.0; 36 | private: 37 | Encoder* enc; 38 | }; 39 | -------------------------------------------------------------------------------- /src/ServoMotor.h: -------------------------------------------------------------------------------- 1 | /* 2 | ServoMotor.cpp - Library for Arduino Motor Carriers 3 | Copyright (c) 2018-2019 Arduino AG. All right reserved. 4 | This library is free software; you can redistribute it and/or 5 | modify it under the terms of the GNU Lesser General Public 6 | License as published by the Free Software Foundation; either 7 | version 2.1 of the License, or (at your option) any later version. 8 | This library is distributed in the hope that it will be useful, 9 | but WITHOUT ANY WARRANTY; without even the implied warranty of 10 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 11 | Lesser General Public License for more details. 12 | You should have received a copy of the GNU Lesser General Public 13 | License along with this library; if not, write to the Free Software 14 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 15 | */ 16 | 17 | #include "Arduino.h" 18 | 19 | namespace mc { 20 | 21 | class ServoMotor { 22 | public: 23 | ServoMotor(); 24 | void setAngle(int angle); 25 | void detach(); 26 | void setFrequency(int frequency); 27 | private: 28 | int instance; 29 | }; 30 | } 31 | -------------------------------------------------------------------------------- /docs/readme.md: -------------------------------------------------------------------------------- 1 | # ArduinoMotorCarrier library 2 | 3 | 4 | This library allows you to use both the Arduino Nano Motor Carrier and the MKR Motor Carrier. The carrier supports inputs for servo motors, dc motors, and encoders, and can read the battery voltage level. This library, therefore, has functions and creates objects to support such hardware. In addition, the library supports PID control for position and velocity. Note that not all hardware features are supported in both carriers so take a look at the specifications for each board. You don't need to initiate any objects manually, they are automatically created when you include "ArduinoMotorCarrier.h" 5 | 6 | To use this library: 7 | 8 | ``` 9 | #include 10 | ``` 11 | 12 | ## Circuit 13 | 14 | Connect the carrier to the main compatible board (MKR1010 or Nano 33 IoT), connect the LiPo battery, turn the switch on and connect the board to the computer using a micro USB cable. You can also connect the motors that you need for your project (servo or DC w/ or w/o encoders). 15 | 16 | ## Examples 17 | 18 | * [Battery Read](https://docs.arduino.cc/tutorials/mkr-motor-carrier/mkr-motor-carrier-battery): Reading the battery level with the MKR Motor Carrier. 19 | -------------------------------------------------------------------------------- /src/src/libfixmath/Makefile: -------------------------------------------------------------------------------- 1 | #Project settings 2 | PROJECT = libfixmath 3 | LIB = 4 | SRC = . 5 | INC = 6 | 7 | #Compiler settings 8 | CPP = gcc 9 | CC = gcc 10 | AS = gcc 11 | LD = gcc 12 | AR = ar 13 | CPP_FLAGS = -O2 $(INC) -Wall -Wextra -c 14 | CC_FLAGS = -O2 $(INC) -Wall -Wextra -c 15 | AS_FLAGS = $(CC_FLAGS) -D_ASSEMBLER_ 16 | LD_FLAGS = -Wall 17 | 18 | # Find all source files 19 | SRC_CPP = $(foreach dir, $(SRC), $(wildcard $(dir)/*.cpp)) 20 | SRC_C = $(foreach dir, $(SRC), $(wildcard $(dir)/*.c)) 21 | SRC_S = $(foreach dir, $(SRC), $(wildcard $(dir)/*.S)) 22 | OBJ_CPP = $(patsubst %.cpp, %.o, $(SRC_CPP)) 23 | OBJ_C = $(patsubst %.c, %.o, $(SRC_C)) 24 | OBJ_S = $(patsubst %.S, %.o, $(SRC_S)) 25 | OBJ = $(OBJ_CPP) $(OBJ_C) $(OBJ_S) 26 | 27 | # Compile rules. 28 | .PHONY : all 29 | all: $(PROJECT).a 30 | 31 | $(PROJECT).a: $(OBJ) 32 | $(AR) rcs $(PROJECT).a $(OBJ) 33 | 34 | $(OBJ_CPP) : %.o : %.cpp 35 | $(CPP) $(CPP_FLAGS) -o $@ $< 36 | 37 | $(OBJ_C) : %.o : %.c 38 | $(CC) $(CC_FLAGS) -o $@ $< 39 | 40 | $(OBJ_S) : %.o : %.S 41 | $(AS) $(AS_FLAGS) -o $@ $< 42 | 43 | 44 | 45 | # Clean rules 46 | .PHONY : clean 47 | clean: 48 | rm -f $(PROJECT).a $(OBJ) 49 | -------------------------------------------------------------------------------- /src/src/libfixmath/fract32.h: -------------------------------------------------------------------------------- 1 | #ifndef __libfixmath_fract32_h__ 2 | #define __libfixmath_fract32_h__ 3 | 4 | #ifdef __cplusplus 5 | extern "C" 6 | { 7 | #endif 8 | 9 | #include 10 | 11 | typedef uint32_t fract32_t; 12 | 13 | /*! Creates a fraction using unsigned integers. 14 | \param inNumerator the unsigned integer numerator 15 | \param inDenominator the unsigned integer denominator 16 | \return a fraction using the given numerator and denominator 17 | */ 18 | extern fract32_t fract32_create(uint32_t inNumerator, uint32_t inDenominator); 19 | 20 | /*! Inverts the given fraction, swapping the numerator and the denominator. 21 | */ 22 | extern fract32_t fract32_invert(fract32_t inFract); 23 | 24 | #ifndef FIXMATH_NO_64BIT 25 | /*! Performs unsigned saturated (overflow-protected) multiplication with the two given fractions and returns the result as an unsigned integer. 26 | */ 27 | extern uint32_t fract32_usmul(uint32_t inVal, fract32_t inFract); 28 | 29 | /*! Performs saturated (overflow-protected) multiplication with the two given fractions and returns the result as a signed integer. 30 | */ 31 | extern int32_t fract32_smul(int32_t inVal, fract32_t inFract); 32 | #endif 33 | 34 | #ifdef __cplusplus 35 | } 36 | #endif 37 | 38 | #endif 39 | -------------------------------------------------------------------------------- /src/Encoder.h: -------------------------------------------------------------------------------- 1 | /* 2 | Encoder.h - Library for Arduino Motor Carriers 3 | Copyright (c) 2018-2019 Arduino AG. All right reserved. 4 | This library is free software; you can redistribute it and/or 5 | modify it under the terms of the GNU Lesser General Public 6 | License as published by the Free Software Foundation; either 7 | version 2.1 of the License, or (at your option) any later version. 8 | This library is distributed in the hope that it will be useful, 9 | but WITHOUT ANY WARRANTY; without even the implied warranty of 10 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 11 | Lesser General Public License for more details. 12 | You should have received a copy of the GNU Lesser General Public 13 | License along with this library; if not, write to the Free Software 14 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 15 | */ 16 | 17 | #ifndef __ENCODER_H__ 18 | #define __ENCODER_H__ 19 | 20 | #include "Arduino.h" 21 | 22 | namespace mc { 23 | 24 | class Encoder { 25 | public: 26 | Encoder(); 27 | int getRawCount(); 28 | int getOverflowUnderflow(); 29 | int getCountPerSecond(); 30 | void resetCounter(long value); 31 | void setIrqOnCount(long value); 32 | void setIrqOnVelocity(long value, uint8_t margin = 2); 33 | private: 34 | int instance; 35 | }; 36 | } 37 | 38 | #endif 39 | -------------------------------------------------------------------------------- /src/Battery.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Battery.cpp - Library for Arduino Motor Carriers 3 | Copyright (c) 2018-2019 Arduino AG. All right reserved. 4 | This library is free software; you can redistribute it and/or 5 | modify it under the terms of the GNU Lesser General Public 6 | License as published by the Free Software Foundation; either 7 | version 2.1 of the License, or (at your option) any later version. 8 | This library is distributed in the hope that it will be useful, 9 | but WITHOUT ANY WARRANTY; without even the implied warranty of 10 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 11 | Lesser General Public License for more details. 12 | You should have received a copy of the GNU Lesser General Public 13 | License along with this library; if not, write to the Free Software 14 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 15 | */ 16 | 17 | #include "Battery.h" 18 | #include "Common.h" 19 | 20 | mc::Battery::Battery() { 21 | } 22 | 23 | int mc::Battery::getRaw() { 24 | int ret; 25 | getData(GET_RAW_ADC_BATTERY, (uint8_t*)&ret); 26 | return ret; 27 | } 28 | 29 | int mc::Battery::getConverted() { 30 | int ret; 31 | getData(GET_CONVERTED_ADC_BATTERY, (uint8_t*)&ret); 32 | return ret; 33 | } 34 | 35 | int mc::Battery::getFiltered() { 36 | int ret; 37 | getData(GET_FILTERED_ADC_BATTERY, (uint8_t*)&ret); 38 | return ret; 39 | } 40 | -------------------------------------------------------------------------------- /src/MotorController.h: -------------------------------------------------------------------------------- 1 | /* 2 | MotorController.h - Library for Arduino Motor Carriers 3 | Copyright (c) 2018-2019 Arduino AG. All right reserved. 4 | This library is free software; you can redistribute it and/or 5 | modify it under the terms of the GNU Lesser General Public 6 | License as published by the Free Software Foundation; either 7 | version 2.1 of the License, or (at your option) any later version. 8 | This library is distributed in the hope that it will be useful, 9 | but WITHOUT ANY WARRANTY; without even the implied warranty of 10 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 11 | Lesser General Public License for more details. 12 | You should have received a copy of the GNU Lesser General Public 13 | License along with this library; if not, write to the Free Software 14 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 15 | */ 16 | 17 | #ifndef __MKRMOTORCONTROLLER__ 18 | #define __MKRMOTORCONTROLLER__ 19 | 20 | #include "Arduino.h" 21 | 22 | namespace mc { 23 | 24 | class MotorController { 25 | public: 26 | MotorController() {}; 27 | String getFWVersion(); 28 | void reboot(); 29 | void ping(); 30 | int begin(); 31 | float getTemperature(); 32 | uint8_t getIrqStatus(); 33 | int getFreeRam(); 34 | volatile uint8_t irq_status; 35 | private: 36 | void enable_battery_charging(); 37 | 38 | }; 39 | } 40 | 41 | #endif //__MKRMOTORCONTROLLER__ 42 | -------------------------------------------------------------------------------- /src/ServoMotor.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | ServoMotor.cpp - Library for Arduino Motor Carriers 3 | Copyright (c) 2018-2019 Arduino AG. All right reserved. 4 | This library is free software; you can redistribute it and/or 5 | modify it under the terms of the GNU Lesser General Public 6 | License as published by the Free Software Foundation; either 7 | version 2.1 of the License, or (at your option) any later version. 8 | This library is distributed in the hope that it will be useful, 9 | but WITHOUT ANY WARRANTY; without even the implied warranty of 10 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 11 | Lesser General Public License for more details. 12 | You should have received a copy of the GNU Lesser General Public 13 | License along with this library; if not, write to the Free Software 14 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 15 | */ 16 | 17 | #include "ServoMotor.h" 18 | #include "Common.h" 19 | 20 | static int next_instance = 0; 21 | 22 | mc::ServoMotor::ServoMotor() { 23 | instance = next_instance; 24 | next_instance++; 25 | }; 26 | 27 | void mc::ServoMotor::setAngle(int angle) { 28 | setData(SET_PWM_DUTY_CYCLE_SERVO, instance, map(angle,0,180,7,28)); 29 | } 30 | 31 | void mc::ServoMotor::detach() { 32 | setData(SET_PWM_DUTY_CYCLE_SERVO, instance, -1); 33 | } 34 | 35 | void mc::ServoMotor::setFrequency(int frequency) { 36 | setData(SET_PWM_FREQUENCY_SERVO, instance, frequency); 37 | } 38 | -------------------------------------------------------------------------------- /src/DCMotor.h: -------------------------------------------------------------------------------- 1 | /* 2 | DCMotor.h - Library for Arduino Motor Carriers 3 | Copyright (c) 2018-2019 Arduino AG. All right reserved. 4 | This library is free software; you can redistribute it and/or 5 | modify it under the terms of the GNU Lesser General Public 6 | License as published by the Free Software Foundation; either 7 | version 2.1 of the License, or (at your option) any later version. 8 | This library is distributed in the hope that it will be useful, 9 | but WITHOUT ANY WARRANTY; without even the implied warranty of 10 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 11 | Lesser General Public License for more details. 12 | You should have received a copy of the GNU Lesser General Public 13 | License along with this library; if not, write to the Free Software 14 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 15 | */ 16 | 17 | #ifndef __DCMOTOR_H__ 18 | #define __DCMOTOR_H__ 19 | 20 | #include "Arduino.h" 21 | 22 | namespace mc { 23 | 24 | class DCMotor { 25 | public: 26 | DCMotor(); 27 | void setDuty(int duty); 28 | void setFrequency(int frequency); 29 | private: 30 | int instance; 31 | }; 32 | } 33 | 34 | namespace d21 { 35 | 36 | class DCMotor { 37 | public: 38 | DCMotor(); 39 | void setDuty(int duty); 40 | void setFrequency(int frequency); 41 | private: 42 | int instance; 43 | int duty = 0; 44 | int in1; 45 | int in2; 46 | }; 47 | } 48 | 49 | #endif 50 | -------------------------------------------------------------------------------- /extras/D11-Firmware/src/Encoder/examples/TwoKnobs/TwoKnobs.pde: -------------------------------------------------------------------------------- 1 | /* Encoder Library - TwoKnobs Example 2 | * http://www.pjrc.com/teensy/td_libs_Encoder.html 3 | * 4 | * This example code is in the public domain. 5 | */ 6 | 7 | #include 8 | 9 | // Change these pin numbers to the pins connected to your encoder. 10 | // Best Performance: both pins have interrupt capability 11 | // Good Performance: only the first pin has interrupt capability 12 | // Low Performance: neither pin has interrupt capability 13 | Encoder knobLeft(5, 6); 14 | Encoder knobRight(7, 8); 15 | // avoid using pins with LEDs attached 16 | 17 | void setup() { 18 | Serial.begin(9600); 19 | Serial.println("TwoKnobs Encoder Test:"); 20 | } 21 | 22 | long positionLeft = -999; 23 | long positionRight = -999; 24 | 25 | void loop() { 26 | long newLeft, newRight; 27 | newLeft = knobLeft.read(); 28 | newRight = knobRight.read(); 29 | if (newLeft != positionLeft || newRight != positionRight) { 30 | Serial.print("Left = "); 31 | Serial.print(newLeft); 32 | Serial.print(", Right = "); 33 | Serial.print(newRight); 34 | Serial.println(); 35 | positionLeft = newLeft; 36 | positionRight = newRight; 37 | } 38 | // if a character is sent from the serial monitor, 39 | // reset both back to zero. 40 | if (Serial.available()) { 41 | Serial.read(); 42 | Serial.println("Reset both knobs to zero"); 43 | knobLeft.write(0); 44 | knobRight.write(0); 45 | } 46 | } 47 | -------------------------------------------------------------------------------- /keywords.txt: -------------------------------------------------------------------------------- 1 | ################################################# 2 | # Syntax Coloring Map For ArduinoMotorCarrier 3 | ################################################# 4 | 5 | ####################################### 6 | # Datatypes (KEYWORD1) 7 | ####################################### 8 | 9 | servo1 KEYWORD1 10 | servo2 KEYWORD1 11 | servo3 KEYWORD1 12 | servo4 KEYWORD1 13 | M1 KEYWORD1 14 | M2 KEYWORD1 15 | M3 KEYWORD1 16 | M4 KEYWORD1 17 | encoder1 KEYWORD1 18 | encoder2 KEYWORD1 19 | pid1 KEYWORD1 20 | pid2 KEYWORD1 21 | battery KEYWORD1 22 | controller KEYWORD1 23 | 24 | ####################################### 25 | # Methods and Functions (KEYWORD2) 26 | ####################################### 27 | 28 | getRaw KEYWORD2 29 | getConverted KEYWORD2 30 | getFiltered KEYWORD2 31 | setDuty KEYWORD2 32 | setFrequency KEYWORD2 33 | getRawCount KEYWORD2 34 | getOverflowUnderflow KEYWORD2 35 | getCountPerSecond KEYWORD2 36 | resetCounter KEYWORD2 37 | setIrqOnCount KEYWORD2 38 | setIrqOnVelocity KEYWORD2 39 | getFWVersion KEYWORD2 40 | reboot KEYWORD2 41 | ping KEYWORD2 42 | begin KEYWORD2 43 | getTemperature KEYWORD2 44 | setGains KEYWORD2 45 | resetGains KEYWORD2 46 | setControlMode KEYWORD2 47 | setSetpoint KEYWORD2 48 | setMaxAcceleration KEYWORD2 49 | setMaxVelocity KEYWORD2 50 | setLimits KEYWORD2 51 | setAngle KEYWORD2 52 | 53 | ####################################### 54 | # Constants (LITERAL1) 55 | ####################################### 56 | 57 | CL_OPEN_LOOP LITERAL1 58 | CL_POSITION LITERAL1 59 | CL_VELOCITY LITERAL1 60 | TARGET_VELOCITY LITERAL1 61 | TARGET_POSITION LITERAL1 62 | -------------------------------------------------------------------------------- /src/ArduinoMotorCarrier.h: -------------------------------------------------------------------------------- 1 | /* 2 | ArduinoMotorCarrier.h - Library for Arduino Motor Carriers 3 | Copyright (c) 2018-2019 Arduino AG. All right reserved. 4 | This library is free software; you can redistribute it and/or 5 | modify it under the terms of the GNU Lesser General Public 6 | License as published by the Free Software Foundation; either 7 | version 2.1 of the License, or (at your option) any later version. 8 | This library is distributed in the hope that it will be useful, 9 | but WITHOUT ANY WARRANTY; without even the implied warranty of 10 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 11 | Lesser General Public License for more details. 12 | You should have received a copy of the GNU Lesser General Public 13 | License along with this library; if not, write to the Free Software 14 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 15 | */ 16 | 17 | #ifndef __ARDUINOMOTORSHIELD__ 18 | #define __ARDUINOMOTORSHIELD__ 19 | 20 | #include 21 | #include "Encoder.h" 22 | #include "Battery.h" 23 | #include "DCMotor.h" 24 | #include "ServoMotor.h" 25 | #include "MotorController.h" 26 | #include "Common.h" 27 | #include "PID.h" 28 | #include "src/FpF.hpp" 29 | 30 | extern mc::MotorController controller; 31 | 32 | extern mc::ServoMotor servo1; 33 | extern mc::ServoMotor servo2; 34 | extern mc::ServoMotor servo3; 35 | extern mc::ServoMotor servo4; 36 | 37 | extern mc::DCMotor M1; 38 | extern mc::DCMotor M2; 39 | extern d21::DCMotor M3; 40 | extern d21::DCMotor M4; 41 | 42 | extern mc::Encoder encoder1; 43 | extern mc::Encoder encoder2; 44 | 45 | extern mc::PID pid1; 46 | extern mc::PID pid2; 47 | 48 | extern mc::Battery battery; 49 | 50 | #endif 51 | -------------------------------------------------------------------------------- /extras/D11-Firmware/src/Encoder/examples/NoInterrupts/NoInterrupts.pde: -------------------------------------------------------------------------------- 1 | /* Encoder Library - NoInterrupts Example 2 | * http://www.pjrc.com/teensy/td_libs_Encoder.html 3 | * 4 | * This example code is in the public domain. 5 | */ 6 | 7 | // If you define ENCODER_DO_NOT_USE_INTERRUPTS *before* including 8 | // Encoder, the library will never use interrupts. This is mainly 9 | // useful to reduce the size of the library when you are using it 10 | // with pins that do not support interrupts. Without interrupts, 11 | // your program must call the read() function rapidly, or risk 12 | // missing changes in position. 13 | #define ENCODER_DO_NOT_USE_INTERRUPTS 14 | #include 15 | 16 | // Beware of Serial.print() speed. Without interrupts, if you 17 | // transmit too much data with Serial.print() it can slow your 18 | // reading from Encoder. Arduino 1.0 has improved transmit code. 19 | // Using the fastest baud rate also helps. Teensy has USB packet 20 | // buffering. But all boards can experience problems if you print 21 | // too much and fill up buffers. 22 | 23 | // Change these two numbers to the pins connected to your encoder. 24 | // With ENCODER_DO_NOT_USE_INTERRUPTS, no interrupts are ever 25 | // used, even if the pin has interrupt capability 26 | Encoder myEnc(5, 6); 27 | // avoid using pins with LEDs attached 28 | 29 | void setup() { 30 | Serial.begin(9600); 31 | Serial.println("Basic NoInterrupts Test:"); 32 | } 33 | 34 | long position = -999; 35 | 36 | void loop() { 37 | long newPos = myEnc.read(); 38 | if (newPos != position) { 39 | position = newPos; 40 | Serial.println(position); 41 | } 42 | // With any substantial delay added, Encoder can only track 43 | // very slow motion. You may uncomment this line to see 44 | // how badly a delay affects your encoder. 45 | //delay(50); 46 | } 47 | -------------------------------------------------------------------------------- /examples/NanoMotorCarrier/EncoderTest/EncoderTest.ino: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | //Variable to store the battery voltage 4 | static int batteryVoltage; 5 | 6 | //Variable to change the motor speed and direction 7 | static int duty = 0; 8 | 9 | void setup() 10 | { 11 | //Establishing the communication with the Motor Carrier 12 | if (controller.begin()) 13 | { 14 | Serial.print("Motor Carrier connected, firmware version "); 15 | Serial.println(controller.getFWVersion()); 16 | } 17 | else 18 | { 19 | Serial.println("Couldn't connect! Is the red LED blinking? You may need to update the firmware with FWUpdater sketch"); 20 | while (1); 21 | } 22 | 23 | //Serial port initialization 24 | Serial.begin(115200); 25 | while (!Serial); 26 | 27 | // Reboot the motor controller; brings every value back to default 28 | Serial.println("reboot"); 29 | controller.reboot(); 30 | delay(500); 31 | 32 | // Reset the encoder internal counter to zero (can be set to any initial value) 33 | Serial.println("reset counters"); 34 | encoder1.resetCounter(0); 35 | encoder2.resetCounter(0); 36 | 37 | M1.setDuty(30); 38 | M2.setDuty(30); 39 | M3.setDuty(30); 40 | M4.setDuty(30); 41 | } 42 | 43 | 44 | void loop() { 45 | 46 | //Chose the encoder to use:encoder1(default) or encoder2 47 | Serial.print("Encoder1 Pos [counts]: "); 48 | Serial.print(encoder1.getRawCount()); 49 | Serial.print(" Encoder1 vel [counts/sec]: "); 50 | Serial.println(encoder1.getCountPerSecond()); 51 | Serial.print("Encoder2 Pos [counts]: "); 52 | Serial.print(encoder2.getRawCount()); 53 | Serial.print(" Encoder2 vel [counts/sec]: "); 54 | Serial.println(encoder2.getCountPerSecond()); 55 | Serial.println(""); 56 | 57 | //Keep active the communication between Nano & Motor Carrier 58 | //Ping the SAMD11 59 | controller.ping(); 60 | //wait 61 | delay(50); 62 | } 63 | -------------------------------------------------------------------------------- /extras/D11-Firmware/Common.h: -------------------------------------------------------------------------------- 1 | enum Commands { 2 | GET_VERSION = 0x01, 3 | RESET, 4 | SET_PWM_DUTY_CYCLE_SERVO, 5 | SET_PWM_FREQUENCY_SERVO, 6 | SET_PWM_DUTY_CYCLE_DC_MOTOR, 7 | SET_PWM_FREQUENCY_DC_MOTOR, 8 | GET_RAW_COUNT_ENCODER, 9 | RESET_COUNT_ENCODER, 10 | GET_OVERFLOW_UNDERFLOW_STATUS_ENCODER, 11 | GET_COUNT_PER_SECOND_ENCODER, 12 | SET_INTERRUPT_ON_COUNT_ENCODER, 13 | SET_INTERRUPT_ON_VELOCITY_ENCODER, 14 | GET_RAW_ADC_BATTERY, 15 | GET_CONVERTED_ADC_BATTERY, 16 | GET_FILTERED_ADC_BATTERY, 17 | SET_PID_GAIN_CL_MOTOR, 18 | RESET_PID_GAIN_CL_MOTOR, 19 | SET_CONTROL_MODE_CL_MOTOR, 20 | SET_POSITION_SETPOINT_CL_MOTOR, 21 | SET_VELOCITY_SETPOINT_CL_MOTOR, 22 | SET_MAX_ACCELERATION_CL_MOTOR, 23 | SET_MAX_VELOCITY_CL_MOTOR, 24 | SET_MIN_MAX_DUTY_CYCLE_CL_MOTOR, 25 | PING, 26 | GET_INTERNAL_TEMP, 27 | CLEAR_IRQ, 28 | GET_FREE_RAM, 29 | GET_PID_VAL 30 | }; 31 | 32 | enum IRQCause { 33 | ENCODER_COUNTER_REACHED = 1, 34 | ENCODER_VELOCITY_REACHED, 35 | }; 36 | 37 | #define I2C_ADDRESS 0x66 38 | 39 | #define ADC_BATTERY A2 40 | #define IRQ_PIN 27 41 | #define LED_BUILTIN 3 42 | 43 | #define MOTOR_2_COUNTER TC1 44 | #define MOTOR_1_COUNTER TC2 45 | 46 | #define MOTOR_2_PIN_A 4 47 | #define MOTOR_2_PIN_B 5 48 | #define MOTOR_1_PIN_A 7 49 | #define MOTOR_1_PIN_B 6 50 | 51 | #include "config.h" 52 | #ifdef NANO_MOTOR_CARRIER 53 | #define ENCODER_1_PIN_A 8 54 | #define ENCODER_1_PIN_B 9 55 | #define ENCODER_2_PIN_A 11 56 | #define ENCODER_2_PIN_B 10 57 | #else 58 | #define ENCODER_1_PIN_A 9 59 | #define ENCODER_1_PIN_B 8 60 | #define ENCODER_2_PIN_A 10 61 | #define ENCODER_2_PIN_B 11 62 | #endif 63 | 64 | #define PWM_PIN_SERVO_COUNTER TCC0 65 | 66 | #define PWM_PIN_SERVO_1 17 67 | #define PWM_PIN_SERVO_2 23 68 | #define PWM_PIN_SERVO_3 16 69 | #define PWM_PIN_SERVO_4 22 70 | 71 | void requestAttention(int cause); 72 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # ArduinoMotorCarrier library 2 | 3 | [![Check Arduino status](https://github.com/arduino-libraries/ArduinoMotorCarrier/actions/workflows/check-arduino.yml/badge.svg)](https://github.com/arduino-libraries/ArduinoMotorCarrier/actions/workflows/check-arduino.yml) 4 | [![Compile Examples status](https://github.com/arduino-libraries/ArduinoMotorCarrier/actions/workflows/compile-examples.yml/badge.svg)](https://github.com/arduino-libraries/ArduinoMotorCarrier/actions/workflows/compile-examples.yml) 5 | [![Spell Check status](https://github.com/arduino-libraries/ArduinoMotorCarrier/actions/workflows/spell-check.yml/badge.svg)](https://github.com/arduino-libraries/ArduinoMotorCarrier/actions/workflows/spell-check.yml) 6 | 7 | This library is designed to use the MKR Motor Carrier. The shield supports micro servo motors, DC motors and DC motors with encoder. This library, therefore, has functions and creates objects to support such hardware. Objects are automatically created when "ArduinoMotorCarrier.h" is included, hence no need to initiate objects manually. 8 | 9 | For more information about this library please visit us at 10 | 11 | 12 | # License 13 | Copyright (c) Arduino LLC. All right reserved. 14 | 15 | This library is free software; you can redistribute it and/or modify it under the terms of the GNU Lesser General Public License as published by the Free Software Foundation; either version 2.1 of the License, or (at your option) any later version. 16 | 17 | This library is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more details. 18 | 19 | You should have received a copy of the GNU Lesser General Public License along with this library; if not, write to the Free Software Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 20 | -------------------------------------------------------------------------------- /src/Encoder.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Encoder.cpp - Library for Arduino Motor Carriers 3 | Copyright (c) 2018-2019 Arduino AG. All right reserved. 4 | This library is free software; you can redistribute it and/or 5 | modify it under the terms of the GNU Lesser General Public 6 | License as published by the Free Software Foundation; either 7 | version 2.1 of the License, or (at your option) any later version. 8 | This library is distributed in the hope that it will be useful, 9 | but WITHOUT ANY WARRANTY; without even the implied warranty of 10 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 11 | Lesser General Public License for more details. 12 | You should have received a copy of the GNU Lesser General Public 13 | License along with this library; if not, write to the Free Software 14 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 15 | */ 16 | 17 | #include "Encoder.h" 18 | #include "Wire.h" 19 | #include "Common.h" 20 | 21 | static int next_instance = 0; 22 | 23 | mc::Encoder::Encoder() { 24 | instance = next_instance; 25 | next_instance++; 26 | }; 27 | 28 | void mc::Encoder::resetCounter(long value) { 29 | setData(RESET_COUNT_ENCODER, instance, value); 30 | } 31 | 32 | int mc::Encoder::getRawCount() { 33 | int ret; 34 | getData(GET_RAW_COUNT_ENCODER, instance, (uint8_t*)&ret); 35 | return ret; 36 | } 37 | 38 | int mc::Encoder::getOverflowUnderflow() { 39 | uint8_t ret[2]; 40 | getData(GET_OVERFLOW_UNDERFLOW_STATUS_ENCODER, instance, (uint8_t*)ret); 41 | return ret[0] << 8 | ret[1]; 42 | } 43 | 44 | int mc::Encoder::getCountPerSecond() { 45 | int ret; 46 | getData(GET_COUNT_PER_SECOND_ENCODER, instance, (uint8_t*)&ret); 47 | return ret; 48 | } 49 | 50 | void mc::Encoder::setIrqOnCount(long value) { 51 | setData(SET_INTERRUPT_ON_COUNT_ENCODER, instance, value); 52 | } 53 | 54 | void mc::Encoder::setIrqOnVelocity(long value, uint8_t margin) { 55 | setData(SET_INTERRUPT_ON_VELOCITY_ENCODER, instance, (margin << 24 | value)); 56 | } 57 | -------------------------------------------------------------------------------- /examples/NanoMotorCarrier/ServoTest/ServoTest.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | //#include 4 | //#include 5 | 6 | 7 | void setup() 8 | { 9 | //Serial port initialization 10 | Serial.begin(115200); 11 | //while (!Serial); 12 | 13 | //Establishing the communication with the Motor Carrier 14 | if (controller.begin()) 15 | { 16 | Serial.print("Motor Carrier connected, firmware version "); 17 | Serial.println(controller.getFWVersion()); 18 | } 19 | else 20 | { 21 | Serial.println("Couldn't connect! Is the red LED blinking? You may need to update the firmware with FWUpdater sketch"); 22 | while (1); 23 | } 24 | 25 | // Reboot the motor controller; brings every value back to default 26 | Serial.println("reboot"); 27 | controller.reboot(); 28 | delay(500); 29 | 30 | M1.setDuty(0); 31 | M2.setDuty(0); 32 | M3.setDuty(0); 33 | M4.setDuty(0); 34 | 35 | if (!PMIC.enableBoostMode()) { 36 | Serial.println("Error enabling Boost Mode"); 37 | } 38 | } 39 | 40 | void loop() { 41 | 42 | //Servo sweep from 0 position to 180 43 | for (int i = 0; i < 180; i += 1) 44 | { 45 | //Choose which of the servo connectors you want to use: servo1(default), servo2, servo3 or servo4 46 | servo1.setAngle(i); 47 | servo2.setAngle(i); 48 | servo3.setAngle(i); 49 | servo4.setAngle(i); 50 | Serial.print("Servos position"); 51 | Serial.println(i); 52 | delay(20); 53 | } 54 | 55 | delay(200); 56 | 57 | //Servo sweep from 180 position to 0 58 | for (int i = 180; i > 0; i -= 1) 59 | { 60 | //Choose which of the servo connectors you want to use: servo1(default), servo2, servo3 or servo4 61 | servo1.setAngle(i); 62 | servo2.setAngle(i); 63 | servo3.setAngle(i); 64 | servo4.setAngle(i); 65 | Serial.print("Servos position: "); 66 | Serial.println(i); 67 | delay(20); 68 | } 69 | 70 | 71 | //Keep active the communication between MKR board & MKR Motor Carrier 72 | //Ping the SAMD11 73 | controller.ping(); 74 | //wait 75 | delay(50); 76 | } 77 | -------------------------------------------------------------------------------- /examples/MKRMotorCarrier/Test/Test.ino: -------------------------------------------------------------------------------- 1 | /* 2 | MKR Motor Carrier Test sketch 3 | This sketch demonstrates some APIs exposed by the ArduinoMotorCarrier library. 4 | For the complete list, visit the reference page on https://www.arduino.cc/en/Reference/MKRMotorCarrier 5 | This example code is in the public domain. 6 | */ 7 | 8 | #include "ArduinoMotorCarrier.h" 9 | 10 | void setup() { 11 | 12 | Serial.begin(115200); 13 | while (!Serial); 14 | 15 | // Start communication with the Motor Carrier 16 | if (controller.begin()) { 17 | Serial.print("MKR Motor Carrier connected, firmware version "); 18 | Serial.println(controller.getFWVersion()); 19 | } else { 20 | Serial.println("Couldn't connect! Is the red LED blinking? You may need to update the firmware with FWUpdater sketch"); 21 | while (1); 22 | } 23 | 24 | // Reboot the motor controller; brings every value back to default 25 | controller.reboot(); 26 | 27 | Serial.println(controller.getFreeRam()); 28 | 29 | // Reset the encoder internal counter to zero (can be set to any initial value) 30 | encoder1.resetCounter(0); 31 | 32 | // Start DC Motor 1 (labeled M1) with 90% of maximum speed, clockwise 33 | M1.setDuty(90); 34 | 35 | // Read the encoder connected to Motor1 until it reaches 36000 counts 36 | int motor1Pos = 0; 37 | while (motor1Pos < 36000) { 38 | motor1Pos = encoder1.getRawCount(); 39 | // Remember to call ping() here and there! 40 | // If you forget to do so the controller will think that the user sketch is not running and will freeze. 41 | // You can wait up to 3 seconds between pings before the reset kicks in 42 | controller.ping(); 43 | } 44 | 45 | // Switch motor control from DIRECT to PID-driven. 46 | // This way, you can program the motor to reach a certain position or velocity without any further intervention. 47 | // The PID can be carefully tuned if a particular profile is needed. 48 | pid1.setControlMode(CL_POSITION); 49 | pid1.setGains(25.0f, 0.0f, 3.0f); 50 | pid1.setMaxAcceleration(4000); 51 | pid1.setSetpoint(TARGET_POSITION, 5000); 52 | } 53 | 54 | void loop() { 55 | // Simply print the actual position while the PID is running, pinging the controller every loop() 56 | Serial.println(encoder1.getRawCount()); 57 | controller.ping(); 58 | delay(100); 59 | } 60 | -------------------------------------------------------------------------------- /extras/D11-Firmware/Encoder.cpp: -------------------------------------------------------------------------------- 1 | #include "EncoderWrapper.h" 2 | #include "Wire.h" 3 | #include "Events.h" 4 | #include "Common.h" 5 | 6 | static EncoderWrapper* obj[2] = {NULL, NULL}; 7 | 8 | #define VELOCITY_INTEGRATION_TIME (1000) 9 | 10 | void populateRegisters_wrapper(void* arg) { 11 | static long val[2]; 12 | static long lastTimestampedVal[2]; 13 | static long lastTimestamp; 14 | for (int i = 0; i < 2; i++) { 15 | int value = obj[i]->read(); 16 | 17 | if (obj[i]->irqCountEnabled && (abs(value - obj[i]->targetCount) < 10 )) { 18 | requestAttention(ENCODER_COUNTER_REACHED); 19 | } 20 | if (value - val[i] < -30000) { 21 | obj[i]->underflow = true; 22 | } 23 | if (value - val[i] > 30000) { 24 | obj[i]->overflow = true; 25 | } 26 | 27 | //for now just use the difference in count since the last period as this is more than sufficient for now for use as angular velocity 28 | int16_t diff = value - lastTimestampedVal[i]; 29 | obj[i]->velocity = Fix16(diff); 30 | lastTimestampedVal[i] = value; 31 | obj[i]->position = Fix16(value * 1.0f); 32 | 33 | // abs doesn't play well with Fix16 (missing an overload for minus) 34 | Fix16 ratioToTargetVelocityPercent = ((Fix16(obj[i]->velocity) - obj[i]->targetVelocity) * Fix16(100.0f)) / obj[i]->targetVelocity; 35 | 36 | if (obj[i]->irqVelocityEnabled && (ratioToTargetVelocityPercent < obj[i]->irqRatio) && (ratioToTargetVelocityPercent > (obj[i]->irqRatio * -1.0f) )) { 37 | requestAttention(ENCODER_VELOCITY_REACHED); 38 | } 39 | 40 | val[i] = value; 41 | lastTimestamp = millis(); 42 | } 43 | } 44 | 45 | EncoderWrapper::EncoderWrapper(int pinA, int pinB, int index) { 46 | enc = new Encoder(pinA, pinB); 47 | obj[index] = this; 48 | if (index == 0) { 49 | registerTimedEvent(populateRegisters_wrapper, this, 10); 50 | } 51 | } 52 | 53 | int EncoderWrapper::read() { 54 | return enc->read(); 55 | } 56 | 57 | void EncoderWrapper::resetCounter(long value) { 58 | enc->write(value); 59 | } 60 | 61 | void EncoderWrapper::getRawCount() { 62 | Wire.write(enc->read()); 63 | } 64 | 65 | void EncoderWrapper::getOverflowUnderflow() { 66 | Wire.write(overflow); 67 | Wire.write(underflow); 68 | } 69 | 70 | void EncoderWrapper::getCountPerSecond() { 71 | Wire.write(int32_t(velocity)); 72 | // Wire.write(velocity); 73 | } 74 | -------------------------------------------------------------------------------- /src/DCMotor.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | DCMotor.cpp - Library for Arduino Motor Carriers 3 | Copyright (c) 2018-2019 Arduino AG. All right reserved. 4 | This library is free software; you can redistribute it and/or 5 | modify it under the terms of the GNU Lesser General Public 6 | License as published by the Free Software Foundation; either 7 | version 2.1 of the License, or (at your option) any later version. 8 | This library is distributed in the hope that it will be useful, 9 | but WITHOUT ANY WARRANTY; without even the implied warranty of 10 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 11 | Lesser General Public License for more details. 12 | You should have received a copy of the GNU Lesser General Public 13 | License along with this library; if not, write to the Free Software 14 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 15 | */ 16 | 17 | #include "DCMotor.h" 18 | #include "Common.h" 19 | 20 | namespace mc { 21 | static int next_instance = 0; 22 | 23 | DCMotor::DCMotor() { 24 | instance = next_instance; 25 | next_instance++; 26 | }; 27 | 28 | void DCMotor::setDuty(int duty) { 29 | setData(SET_PWM_DUTY_CYCLE_DC_MOTOR, instance, duty); 30 | } 31 | 32 | void DCMotor::setFrequency(int frequency) { 33 | setData(SET_PWM_FREQUENCY_DC_MOTOR, instance, frequency); 34 | } 35 | 36 | } 37 | // D21 implementations 38 | 39 | namespace d21 { 40 | static int next_instance = 0; 41 | 42 | DCMotor::DCMotor() { 43 | instance = next_instance; 44 | next_instance++; 45 | if (instance == 0) { 46 | in1 = MOTOR_3_PIN_A; //2; 47 | in2 = MOTOR_3_PIN_B; //3; 48 | } else { 49 | in1 = MOTOR_4_PIN_A; //5; 50 | in2 = MOTOR_4_PIN_B; //4; 51 | } 52 | pinMode(in1, OUTPUT); 53 | pinMode(in2, OUTPUT); 54 | }; 55 | 56 | void DCMotor::setDuty(int duty) { 57 | //setData(SET_PWM_DUTY_CYCLE_DC_MOTOR, instance, duty); 58 | this->duty = duty; 59 | if (duty == 0) { 60 | analogWrite(in1, 0); 61 | analogWrite(in2, 0); 62 | return; 63 | } 64 | if (duty > 0) { 65 | analogWrite(in1, 0); 66 | analogWrite(in2, map(duty, 0, 100, 0, 255)); 67 | } else { 68 | analogWrite(in2, 0); 69 | analogWrite(in1, map(-duty, 0, 100, 0, 255)); 70 | } 71 | 72 | } 73 | 74 | void DCMotor::setFrequency(int frequency) { 75 | //setData(SET_PWM_FREQUENCY_DC_MOTOR, instance, frequency); 76 | } 77 | } 78 | -------------------------------------------------------------------------------- /extras/D11-Firmware/src/Encoder/utility/interrupt_config.h: -------------------------------------------------------------------------------- 1 | #if defined(__AVR__) 2 | 3 | #include 4 | #include 5 | 6 | #define attachInterrupt(num, func, mode) enableInterrupt(num) 7 | #if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) 8 | #define SCRAMBLE_INT_ORDER(num) ((num < 4) ? num + 2 : ((num < 6) ? num - 4 : num)) 9 | #define DESCRAMBLE_INT_ORDER(num) ((num < 2) ? num + 4 : ((num < 6) ? num - 2 : num)) 10 | #else 11 | #define SCRAMBLE_INT_ORDER(num) (num) 12 | #define DESCRAMBLE_INT_ORDER(num) (num) 13 | #endif 14 | 15 | static void enableInterrupt(uint8_t num) 16 | { 17 | switch (DESCRAMBLE_INT_ORDER(num)) { 18 | #if defined(EICRA) && defined(EIMSK) 19 | case 0: 20 | EICRA = (EICRA & 0xFC) | 0x01; 21 | EIMSK |= 0x01; 22 | return; 23 | case 1: 24 | EICRA = (EICRA & 0xF3) | 0x04; 25 | EIMSK |= 0x02; 26 | return; 27 | case 2: 28 | EICRA = (EICRA & 0xCF) | 0x10; 29 | EIMSK |= 0x04; 30 | return; 31 | case 3: 32 | EICRA = (EICRA & 0x3F) | 0x40; 33 | EIMSK |= 0x08; 34 | return; 35 | #elif defined(MCUCR) && defined(GICR) 36 | case 0: 37 | MCUCR = (MCUCR & ~((1 << ISC00) | (1 << ISC01))) | (mode << ISC00); 38 | GICR |= (1 << INT0); 39 | return; 40 | case 1: 41 | MCUCR = (MCUCR & ~((1 << ISC10) | (1 << ISC11))) | (mode << ISC10); 42 | GICR |= (1 << INT1); 43 | return; 44 | #elif defined(MCUCR) && defined(GIMSK) 45 | case 0: 46 | MCUCR = (MCUCR & ~((1 << ISC00) | (1 << ISC01))) | (mode << ISC00); 47 | GIMSK |= (1 << INT0); 48 | return; 49 | case 1: 50 | MCUCR = (MCUCR & ~((1 << ISC10) | (1 << ISC11))) | (mode << ISC10); 51 | GIMSK |= (1 << INT1); 52 | return; 53 | #endif 54 | #if defined(EICRB) && defined(EIMSK) 55 | case 4: 56 | EICRB = (EICRB & 0xFC) | 0x01; 57 | EIMSK |= 0x10; 58 | return; 59 | case 5: 60 | EICRB = (EICRB & 0xF3) | 0x04; 61 | EIMSK |= 0x20; 62 | return; 63 | case 6: 64 | EICRB = (EICRB & 0xCF) | 0x10; 65 | EIMSK |= 0x40; 66 | return; 67 | case 7: 68 | EICRB = (EICRB & 0x3F) | 0x40; 69 | EIMSK |= 0x80; 70 | return; 71 | #endif 72 | } 73 | } 74 | 75 | #elif defined(__PIC32MX__) 76 | 77 | #ifdef ENCODER_OPTIMIZE_INTERRUPTS 78 | #undef ENCODER_OPTIMIZE_INTERRUPTS 79 | #endif 80 | 81 | #else 82 | 83 | #ifdef ENCODER_OPTIMIZE_INTERRUPTS 84 | #undef ENCODER_OPTIMIZE_INTERRUPTS 85 | #endif 86 | 87 | #endif 88 | -------------------------------------------------------------------------------- /extras/D11-Firmware/PID.cpp: -------------------------------------------------------------------------------- 1 | #include "PID.h" 2 | #include "Events.h" 3 | 4 | static PIDWrapper* obj[2] = {NULL, NULL}; 5 | 6 | //extern DCMotor dcmotors[2];// necessary for workaround below 7 | 8 | void calculatePID_wrapper(void* arg) { 9 | static Fix16 prevvelocmd[2] = {0.0f, 0.0f}; 10 | 11 | for (int i = 0; i < 2; i++) { 12 | 13 | if (obj[i]->mode == CL_POSITION) { 14 | if (obj[i]->pid_pos->Compute()) { 15 | Fix16 curvelocmd = obj[i]->velocmd; 16 | //copy position PID output to velocity PID setpoint 17 | obj[i]->targetvelo = curvelocmd; 18 | //save curvelocmd for next iteration 19 | prevvelocmd[i] = curvelocmd; 20 | int dutyout = (int16_t)curvelocmd ; 21 | obj[i]->motor->setDuty(dutyout); //dutyout should be a value between -100 and 100. 22 | } 23 | } else { 24 | //CL_VELOCITY 25 | if (obj[i]->pid_velo->Compute()) { 26 | int dutyout = int32_t(obj[i]->actualDuty); 27 | obj[i]->motor->setDuty(dutyout); 28 | } 29 | } 30 | } 31 | } 32 | 33 | Fix16 KP_DEFAULT, KI_DEFAULT, KD_DEFAULT; 34 | 35 | PIDWrapper::PIDWrapper(Fix16& inputpos, Fix16& inputvelo, DCMotor* motor, int index, int periodms_velo , int periodms_pos) { 36 | 37 | KP_DEFAULT = Fix16(2.0); 38 | KI_DEFAULT = Fix16(2.0); 39 | KD_DEFAULT = Fix16(0.0); 40 | 41 | pid_pos = new PID(&inputpos, &velocmd, &targetpos, KP_DEFAULT, KI_DEFAULT, KD_DEFAULT, DIRECT); 42 | pid_velo = new PID(&inputvelo, &actualDuty, &targetvelo, KP_DEFAULT, KI_DEFAULT, KD_DEFAULT, DIRECT); 43 | pid_pos->SetSampleTime(periodms_pos); 44 | pid_velo->SetSampleTime(periodms_velo); 45 | pid_pos->SetOutputLimits(Fix16(-30.0), Fix16(30.0)); //position pid can only command +/- max_velo 46 | pid_velo->SetOutputLimits(Fix16(-90.0), Fix16(90.0)); //velocity pid can only command +/- 100 PWM duty cycle 47 | 48 | stop(); 49 | 50 | this->motor = motor; 51 | this->motor->pid = this; 52 | obj[index] = this; 53 | if (index == 0) { 54 | // recalculate every millisecond 55 | registerTimedEvent(calculatePID_wrapper, this, 0); 56 | } 57 | } 58 | 59 | void PIDWrapper::resetGains() { 60 | cl_control prev_mode = this->mode; 61 | this->mode = CL_VELOCITY; 62 | setGains(KP_DEFAULT, KI_DEFAULT, KD_DEFAULT); 63 | this->mode = CL_POSITION; 64 | setGains(KP_DEFAULT, KI_DEFAULT, KD_DEFAULT); 65 | this->mode = prev_mode; 66 | run(); 67 | }; 68 | -------------------------------------------------------------------------------- /src/src/libfixmath/fix16_sqrt.c: -------------------------------------------------------------------------------- 1 | #include "fix16.h" 2 | 3 | /* The square root algorithm is quite directly from 4 | * http://en.wikipedia.org/wiki/Methods_of_computing_square_roots#Binary_numeral_system_.28base_2.29 5 | * An important difference is that it is split to two parts 6 | * in order to use only 32-bit operations. 7 | * 8 | * Note that for negative numbers we return -sqrt(-inValue). 9 | * Not sure if someone relies on this behaviour, but not going 10 | * to break it for now. It doesn't slow the code much overall. 11 | */ 12 | fix16_t fix16_sqrt(fix16_t inValue) 13 | { 14 | uint8_t neg = (inValue < 0); 15 | uint32_t num = (neg ? -inValue : inValue); 16 | uint32_t result = 0; 17 | uint32_t bit; 18 | uint8_t n; 19 | 20 | // Many numbers will be less than 15, so 21 | // this gives a good balance between time spent 22 | // in if vs. time spent in the while loop 23 | // when searching for the starting value. 24 | if (num & 0xFFF00000) 25 | bit = (uint32_t)1 << 30; 26 | else 27 | bit = (uint32_t)1 << 18; 28 | 29 | while (bit > num) bit >>= 2; 30 | 31 | // The main part is executed twice, in order to avoid 32 | // using 64 bit values in computations. 33 | for (n = 0; n < 2; n++) 34 | { 35 | // First we get the top 24 bits of the answer. 36 | while (bit) 37 | { 38 | if (num >= result + bit) 39 | { 40 | num -= result + bit; 41 | result = (result >> 1) + bit; 42 | } 43 | else 44 | { 45 | result = (result >> 1); 46 | } 47 | bit >>= 2; 48 | } 49 | 50 | if (n == 0) 51 | { 52 | // Then process it again to get the lowest 8 bits. 53 | if (num > 65535) 54 | { 55 | // The remainder 'num' is too large to be shifted left 56 | // by 16, so we have to add 1 to result manually and 57 | // adjust 'num' accordingly. 58 | // num = a - (result + 0.5)^2 59 | // = num + result^2 - (result + 0.5)^2 60 | // = num - result - 0.5 61 | num -= result; 62 | num = (num << 16) - 0x8000; 63 | result = (result << 16) + 0x8000; 64 | } 65 | else 66 | { 67 | num <<= 16; 68 | result <<= 16; 69 | } 70 | 71 | bit = 1 << 14; 72 | } 73 | } 74 | 75 | #ifndef FIXMATH_NO_ROUNDING 76 | // Finally, if next bit would have been 1, round the result upwards. 77 | if (num > result) 78 | { 79 | result++; 80 | } 81 | #endif 82 | 83 | return (neg ? -(fix16_t)result : (fix16_t)result); 84 | } 85 | -------------------------------------------------------------------------------- /examples/MKRMotorCarrier/Servo_test/Servo_test.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #define INTERRUPT_PIN 6 3 | 4 | //Variable to store the battery voltage 5 | int batteryVoltage; 6 | 7 | void setup() 8 | { 9 | //Serial port initialization 10 | Serial.begin(115200); 11 | while (!Serial); 12 | 13 | //Establishing the communication with the Motor Carrier 14 | if (controller.begin()) 15 | { 16 | Serial.print("MKR Motor Carrier connected, firmware version "); 17 | Serial.println(controller.getFWVersion()); 18 | } 19 | else 20 | { 21 | Serial.println("Couldn't connect! Is the red LED blinking? You may need to update the firmware with FWUpdater sketch"); 22 | while (1); 23 | } 24 | 25 | // Reboot the motor controller; brings every value back to default 26 | Serial.println("reboot"); 27 | controller.reboot(); 28 | delay(500); 29 | 30 | //Take the battery status 31 | float batteryVoltage = (float)battery.getConverted(); 32 | Serial.print("Battery voltage: "); 33 | Serial.print(batteryVoltage); 34 | Serial.print("V, Raw "); 35 | Serial.println(battery.getRaw()); 36 | } 37 | 38 | 39 | void loop() { 40 | 41 | //Take the battery status 42 | float batteryVoltage = (float)battery.getConverted(); 43 | 44 | //Reset to the default values if the battery level is lower than 11 V 45 | if (batteryVoltage < 11) 46 | { 47 | Serial.println(" "); 48 | Serial.println("WARNING: LOW BATTERY"); 49 | Serial.println("ALL SYSTEMS DOWN"); 50 | while (batteryVoltage < 11) { 51 | batteryVoltage = (float)battery.getConverted(); 52 | } 53 | } 54 | else 55 | { 56 | //Servo sweep from 0 position to 180 57 | for (int i=0; i<180; i+=5) 58 | { 59 | //Choose which of the servo connectors you want to use: servo1(default), servo2, servo3 or servo4 60 | servo1.setAngle(i); 61 | Serial.print("Servo position"); 62 | Serial.println(i); 63 | delay(50); 64 | } 65 | 66 | delay(200); 67 | 68 | //Servo sweep from 180 position to 0 69 | for (int i=180; i>0; i-=5) 70 | { 71 | //Choose which of the servo connectors you want to use: servo1(default), servo2, servo3 or servo4 72 | servo1.setAngle(i); 73 | Serial.print("Servo position"); 74 | Serial.println(i); 75 | delay(50); 76 | } 77 | } 78 | 79 | //Keep active the communication between MKR board & MKR Motor Carrier 80 | //Ping the SAMD11 81 | controller.ping(); 82 | //wait 83 | delay(50); 84 | } 85 | -------------------------------------------------------------------------------- /.github/workflows/compile-firmware.yml: -------------------------------------------------------------------------------- 1 | name: Compile Firmware 2 | 3 | # See: https://docs.github.com/en/free-pro-team@latest/actions/reference/events-that-trigger-workflows 4 | on: 5 | push: 6 | paths: 7 | - ".github/workflows/compile-firmware.yml" 8 | - "extras/D11-Firmware/**" 9 | pull_request: 10 | paths: 11 | - ".github/workflows/compile-firmware.yml" 12 | - "extras/D11-Firmware/**" 13 | schedule: 14 | # Run every Tuesday at 8 AM UTC to catch breakage caused by changes to external resources (libraries, platforms). 15 | - cron: "0 8 * * TUE" 16 | workflow_dispatch: 17 | repository_dispatch: 18 | 19 | jobs: 20 | build: 21 | name: ${{ matrix.board.fqbn }} 22 | runs-on: ubuntu-latest 23 | 24 | env: 25 | SKETCHES_REPORTS_PATH: sketches-reports 26 | 27 | strategy: 28 | fail-fast: false 29 | 30 | matrix: 31 | board: 32 | - fqbn: arduino:samd:mkrmotorshield:bootloader=0kb,pinmap=complete,lto=disabled 33 | platforms: | 34 | # Install MattairTech_Arduino:samd via Boards Manager for the toolchain 35 | - name: MattairTech_Arduino:samd 36 | source-url: https://www.mattairtech.com/software/arduino/package_MattairTech_index.json 37 | # This needs to match with the version of MattairTech_Arduino:samd the Arduino fork is based on in order to get the right tool versions 38 | version: 1.6.16 39 | # Install the platform with Motor Carrier support 40 | - name: arduino:samd 41 | source-url: https://github.com/arduino/ArduinoCore-samd.git 42 | version: mkrmotorcarrier 43 | 44 | steps: 45 | - name: Checkout repository 46 | uses: actions/checkout@v6 47 | 48 | - name: Compile firmware 49 | uses: arduino/compile-sketches@v1 50 | with: 51 | github-token: ${{ secrets.GITHUB_TOKEN }} 52 | fqbn: ${{ matrix.board.fqbn }} 53 | platforms: ${{ matrix.board.platforms }} 54 | libraries: | 55 | - 56 | sketch-paths: | 57 | - extras/D11-Firmware/D11-Firmware.ino 58 | enable-deltas-report: true 59 | sketches-report-path: ${{ env.SKETCHES_REPORTS_PATH }} 60 | 61 | - name: Save sketches report as workflow artifact 62 | uses: actions/upload-artifact@v6 63 | with: 64 | if-no-files-found: error 65 | path: ${{ env.SKETCHES_REPORTS_PATH }} 66 | name: ${{ env.SKETCHES_REPORTS_PATH }} 67 | -------------------------------------------------------------------------------- /.github/workflows/compile-bootloader.yml: -------------------------------------------------------------------------------- 1 | name: Compile Bootloader 2 | 3 | # See: https://docs.github.com/en/free-pro-team@latest/actions/reference/events-that-trigger-workflows 4 | on: 5 | push: 6 | paths: 7 | - ".github/workflows/compile-bootloader.yml" 8 | - "extras/D11-Bootloader/**" 9 | pull_request: 10 | paths: 11 | - ".github/workflows/compile-bootloader.yml" 12 | - "extras/D11-Bootloader/**" 13 | schedule: 14 | # Run every Tuesday at 8 AM UTC to catch breakage caused by changes to external resources (libraries, platforms). 15 | - cron: "0 8 * * TUE" 16 | workflow_dispatch: 17 | repository_dispatch: 18 | 19 | jobs: 20 | build: 21 | name: ${{ matrix.board.fqbn }} 22 | runs-on: ubuntu-latest 23 | 24 | env: 25 | SKETCHES_REPORTS_PATH: sketches-reports 26 | 27 | strategy: 28 | fail-fast: false 29 | 30 | matrix: 31 | board: 32 | - fqbn: arduino:samd:mkrmotorshield:bootloader=0kb,pinmap=complete,lto=disabled 33 | platforms: | 34 | # Install MattairTech_Arduino:samd via Boards Manager for the toolchain 35 | - name: MattairTech_Arduino:samd 36 | source-url: https://www.mattairtech.com/software/arduino/package_MattairTech_index.json 37 | # This needs to match with the version of MattairTech_Arduino:samd the Arduino fork is based on in order to get the right tool versions 38 | version: 1.6.16 39 | # Install the platform with Motor Carrier support 40 | - name: arduino:samd 41 | source-url: https://github.com/arduino/ArduinoCore-samd.git 42 | version: mkrmotorcarrier 43 | 44 | steps: 45 | - name: Checkout repository 46 | uses: actions/checkout@v6 47 | 48 | - name: Compile bootloader 49 | uses: arduino/compile-sketches@v1 50 | with: 51 | github-token: ${{ secrets.GITHUB_TOKEN }} 52 | fqbn: ${{ matrix.board.fqbn }} 53 | platforms: ${{ matrix.board.platforms }} 54 | libraries: | 55 | - name: Sodaq_wdt 56 | sketch-paths: | 57 | - extras/D11-Bootloader 58 | enable-deltas-report: true 59 | sketches-report-path: ${{ env.SKETCHES_REPORTS_PATH }} 60 | 61 | - name: Save sketches report as workflow artifact 62 | uses: actions/upload-artifact@v6 63 | with: 64 | if-no-files-found: error 65 | path: ${{ env.SKETCHES_REPORTS_PATH }} 66 | name: ${{ env.SKETCHES_REPORTS_PATH }} 67 | -------------------------------------------------------------------------------- /examples/MKRMotorCarrier/Motor_test/Motor_test.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #define INTERRUPT_PIN 6 3 | 4 | //Variable to store the battery voltage 5 | static int batteryVoltage; 6 | 7 | //Variable to change the motor speed and direction 8 | static int duty = 0; 9 | 10 | void setup() 11 | { 12 | //Serial port initialization 13 | Serial.begin(115200); 14 | while (!Serial); 15 | 16 | //Establishing the communication with the Motor Carrier 17 | if (controller.begin()) 18 | { 19 | Serial.print("MKR Motor Connected connected, firmware version "); 20 | Serial.println(controller.getFWVersion()); 21 | } 22 | else 23 | { 24 | Serial.println("Couldn't connect! Is the red LED blinking? You may need to update the firmware with FWUpdater sketch"); 25 | while (1); 26 | } 27 | 28 | // Reboot the motor controller; brings every value back to default 29 | Serial.println("reboot"); 30 | controller.reboot(); 31 | delay(500); 32 | 33 | //Take the battery status 34 | float batteryVoltage = (float)battery.getConverted(); 35 | Serial.print("Battery voltage: "); 36 | Serial.print(batteryVoltage); 37 | Serial.print("V, Raw "); 38 | Serial.println(battery.getRaw()); 39 | } 40 | 41 | 42 | void loop() { 43 | 44 | //Take the battery status 45 | float batteryVoltage = (float)battery.getConverted(); 46 | 47 | //Reset to the default values if the battery level is lower than 11 V 48 | if (batteryVoltage < 11) 49 | { 50 | Serial.println(" "); 51 | Serial.println("WARNING: LOW BATTERY"); 52 | Serial.println("ALL SYSTEMS DOWN"); 53 | M1.setDuty(0); 54 | M2.setDuty(0); 55 | M3.setDuty(0); 56 | M4.setDuty(0); 57 | while (batteryVoltage < 11) 58 | { 59 | batteryVoltage = (float)battery.getConverted(); 60 | } 61 | } 62 | else 63 | { 64 | //Motor test 65 | for (duty=-100; duty<100; duty+=5) 66 | { 67 | Serial.print("Motor Duty: "); 68 | Serial.println(duty); 69 | M1.setDuty(duty); 70 | M2.setDuty(duty); 71 | M3.setDuty(duty); 72 | M4.setDuty(duty); 73 | delay(50); 74 | } 75 | for (duty=100; duty>-100; duty-=5) 76 | { 77 | Serial.print("Motor Duty: "); 78 | Serial.println(duty); 79 | M1.setDuty(duty); 80 | M2.setDuty(duty); 81 | M3.setDuty(duty); 82 | M4.setDuty(duty); 83 | delay(50); 84 | } 85 | 86 | //Keep active the communication between MKR board & MKR Motor Carrier 87 | //Ping the SAMD11 88 | controller.ping(); 89 | //wait 90 | delay(50); 91 | } 92 | } 93 | -------------------------------------------------------------------------------- /examples/MKRMotorCarrier/Motor_test_encoder/Motor_test_encoder.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #define INTERRUPT_PIN 6 3 | 4 | //Variable to store the battery voltage 5 | static int batteryVoltage; 6 | 7 | //Variable to change the motor speed and direction 8 | static int duty = -20; 9 | 10 | void setup() 11 | { 12 | //Serial port initialization 13 | Serial.begin(115200); 14 | while (!Serial); 15 | 16 | //Establishing the communication with the Motor Carrier 17 | if (controller.begin()) 18 | { 19 | Serial.print("Motor Carrier connected, firmware version "); 20 | Serial.println(controller.getFWVersion()); 21 | } 22 | else 23 | { 24 | Serial.println("Couldn't connect! Is the red LED blinking? You may need to update the firmware with FWUpdater sketch"); 25 | while (1); 26 | } 27 | 28 | // Reboot the motor controller; brings every value back to default 29 | Serial.println("reboot"); 30 | controller.reboot(); 31 | delay(500); 32 | 33 | // Reset the encoder internal counter to zero (can be set to any initial value) 34 | Serial.println("reset counters"); 35 | encoder1.resetCounter(0); 36 | encoder2.resetCounter(0); 37 | 38 | //Take the battery status 39 | float batteryVoltage = (float)battery.getConverted(); 40 | Serial.print("Battery voltage: "); 41 | Serial.print(batteryVoltage); 42 | Serial.print("V, Raw "); 43 | Serial.println(battery.getRaw()); 44 | } 45 | 46 | 47 | void loop() { 48 | 49 | //Take the battery status 50 | float batteryVoltage = (float)battery.getConverted(); 51 | 52 | //Reset to the default values if the battery level is lower than 11 V 53 | if (batteryVoltage < 11) 54 | { 55 | Serial.println(" "); 56 | Serial.println("WARNING: LOW BATTERY"); 57 | Serial.println("ALL SYSTEMS DOWN"); 58 | M1.setDuty(0); 59 | M2.setDuty(0); 60 | M3.setDuty(0); 61 | M4.setDuty(0); 62 | while (batteryVoltage < 11) 63 | { 64 | batteryVoltage = (float)battery.getConverted(); 65 | } 66 | } 67 | else 68 | { 69 | //Chose the motor to use:M1(default), M2, M3 or M4 70 | Serial.print("M1 Duty: "); 71 | Serial.println(duty); 72 | M1.setDuty(duty); 73 | 74 | //Chose the encoder to use:encoder1(default) or encoder2 75 | Serial.print("Encoder1 Pos [counts]: "); 76 | Serial.println(encoder1.getRawCount()); 77 | Serial.print("Encoder1 vel [counts/sec]: "); 78 | Serial.println(encoder1.getCountPerSecond()); 79 | } 80 | 81 | //Keep active the communication between MKR board & MKR Motor Carrier 82 | //Ping the SAMD11 83 | controller.ping(); 84 | //wait 85 | delay(50); 86 | } 87 | -------------------------------------------------------------------------------- /examples/NanoMotorCarrier/DCMotorTest/DCMotorTest.ino: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | //Variable to store the battery voltage 4 | static int batteryVoltage; 5 | 6 | //Variable to change the motor speed and direction 7 | static int duty = 0; 8 | 9 | void setup() 10 | { 11 | //Serial port initialization 12 | Serial.begin(115200); 13 | //while (!Serial); 14 | 15 | //Establishing the communication with the Motor Carrier 16 | if (controller.begin()) 17 | { 18 | Serial.print("Motor Carrier connected, firmware version "); 19 | Serial.println(controller.getFWVersion()); 20 | } 21 | else 22 | { 23 | Serial.println("Couldn't connect! Is the red LED blinking? You may need to update the firmware with FWUpdater sketch"); 24 | while (1); 25 | } 26 | 27 | // Reboot the motor controller; brings every value back to default 28 | Serial.println("reboot"); 29 | controller.reboot(); 30 | delay(500); 31 | 32 | int dutyInit = 0; // at 50 it works as expected, at 60 shift sides and is too small duty to move, at 70 is very big duty. 33 | M1.setDuty(dutyInit); 34 | M2.setDuty(dutyInit); 35 | M3.setDuty(dutyInit); 36 | M4.setDuty(dutyInit); 37 | Serial.print("Duty init: "); 38 | Serial.println(dutyInit); 39 | // int duty2 = dutyInit * 16777215 / 100; 40 | // Serial.print("Conversion formula: "); 41 | // Serial.println(duty2); 42 | //while (1); //WHILE 1!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! REMOVE!!!! 43 | } 44 | 45 | 46 | void loop() { 47 | 48 | //Take the battery status 49 | //float batteryVoltage = (float)battery.getConverted(); 50 | 51 | //Reset to the default values if the battery level is lower than 11 V 52 | // if (batteryVoltage < 11) 53 | // { 54 | // Serial.println(" "); 55 | // Serial.println("WARNING: LOW BATTERY"); 56 | // Serial.println("ALL SYSTEMS DOWN"); 57 | // M1.setDuty(0); 58 | // M2.setDuty(0); 59 | // M3.setDuty(0); 60 | // M4.setDuty(0); 61 | // while (batteryVoltage < 11) 62 | // { 63 | // batteryVoltage = (float)battery.getConverted(); 64 | // } 65 | // } 66 | // else 67 | // { 68 | 69 | //Motor test 70 | for (duty = -100; duty < 100; duty += 1) 71 | { 72 | Serial.print("Motor Duty: "); 73 | Serial.println(duty); 74 | M1.setDuty(duty); 75 | M2.setDuty(duty); 76 | M3.setDuty(duty); 77 | M4.setDuty(duty); 78 | delay(10); 79 | } 80 | for (duty = 100; duty > -100; duty -= 1) 81 | { 82 | Serial.print("Motor Duty: "); 83 | Serial.println(duty); 84 | M1.setDuty(duty); 85 | M2.setDuty(duty); 86 | M3.setDuty(duty); 87 | M4.setDuty(duty); 88 | delay(10); 89 | } 90 | 91 | //Keep active the communication between Nano & Motor Carrier 92 | //Ping the SAMD11 93 | controller.ping(); 94 | //wait 95 | delay(50); 96 | } 97 | -------------------------------------------------------------------------------- /src/Common.h: -------------------------------------------------------------------------------- 1 | /* 2 | Common.h - Library for Arduino Motor Carriers 3 | Copyright (c) 2018-2019 Arduino AG. All right reserved. 4 | This library is free software; you can redistribute it and/or 5 | modify it under the terms of the GNU Lesser General Public 6 | License as published by the Free Software Foundation; either 7 | version 2.1 of the License, or (at your option) any later version. 8 | This library is distributed in the hope that it will be useful, 9 | but WITHOUT ANY WARRANTY; without even the implied warranty of 10 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 11 | Lesser General Public License for more details. 12 | You should have received a copy of the GNU Lesser General Public 13 | License along with this library; if not, write to the Free Software 14 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 15 | */ 16 | 17 | #if defined(ARDUINO_PORTENTA_H7_M7) || defined(ARDUINO_GENERIC_STM32H747_M4) 18 | #define RESET _RESET 19 | #endif 20 | 21 | enum Commands { 22 | GET_VERSION = 0x01, 23 | RESET, 24 | SET_PWM_DUTY_CYCLE_SERVO, 25 | SET_PWM_FREQUENCY_SERVO, 26 | SET_PWM_DUTY_CYCLE_DC_MOTOR, 27 | SET_PWM_FREQUENCY_DC_MOTOR, 28 | GET_RAW_COUNT_ENCODER, 29 | RESET_COUNT_ENCODER, 30 | GET_OVERFLOW_UNDERFLOW_STATUS_ENCODER, 31 | GET_COUNT_PER_SECOND_ENCODER, 32 | SET_INTERRUPT_ON_COUNT_ENCODER, 33 | SET_INTERRUPT_ON_VELOCITY_ENCODER, 34 | GET_RAW_ADC_BATTERY, 35 | GET_CONVERTED_ADC_BATTERY, 36 | GET_FILTERED_ADC_BATTERY, 37 | SET_PID_GAIN_CL_MOTOR, 38 | RESET_PID_GAIN_CL_MOTOR, 39 | SET_CONTROL_MODE_CL_MOTOR, 40 | SET_POSITION_SETPOINT_CL_MOTOR, 41 | SET_VELOCITY_SETPOINT_CL_MOTOR, 42 | SET_MAX_ACCELERATION_CL_MOTOR, 43 | SET_MAX_VELOCITY_CL_MOTOR, 44 | SET_MIN_MAX_DUTY_CYCLE_CL_MOTOR, 45 | PING, 46 | GET_INTERNAL_TEMP, 47 | CLEAR_IRQ, 48 | GET_FREE_RAM, 49 | GET_PID_VAL 50 | }; 51 | 52 | enum IRQCause { 53 | ENCODER_COUNTER_REACHED = 1, 54 | ENCODER_VELOCITY_REACHED, 55 | }; 56 | 57 | #define I2C_ADDRESS 0x66 58 | #define IRQ_PIN 6 59 | 60 | #ifdef ARDUINO_SAMD_NANO_33_IOT 61 | #define MOTOR_3_PIN_A 2 62 | #define MOTOR_3_PIN_B 3 63 | #define MOTOR_4_PIN_A 5 64 | #define MOTOR_4_PIN_B 4 65 | #else 66 | #define MOTOR_3_PIN_A 3 67 | #define MOTOR_3_PIN_B 2 68 | #define MOTOR_4_PIN_A 4 69 | #define MOTOR_4_PIN_B 5 70 | #endif 71 | 72 | 73 | #ifdef ARDUINO_SAMD_NANO_33_IOT 74 | #define IN1 A7 75 | #define IN2 A2 76 | #define IN3 A6 77 | #define IN4 A3 78 | #else 79 | #define IN1 A6 80 | #define IN2 A1 81 | #define IN3 A5 82 | #define IN4 A2 83 | #endif 84 | 85 | #include "src/FpF.hpp" 86 | #define Fix16 mn::MFixedPoint::FpF32<8> 87 | 88 | namespace mc { 89 | int getData(Commands cmd, uint8_t target, uint8_t* buf); 90 | void setData(Commands cmd, uint8_t target, int data); 91 | void setDataPIDGains(Commands cmd, uint8_t target, Fix16 P, Fix16 I, Fix16 D); 92 | int getDataPIDGains(Commands cmd, uint8_t target, uint8_t* buf, int dataSize); 93 | int getData(Commands cmd, uint8_t* buf); 94 | } 95 | -------------------------------------------------------------------------------- /extras/D11-Firmware/PID.h: -------------------------------------------------------------------------------- 1 | #ifndef __PID_H__ 2 | #define __PID_H__ 3 | 4 | #include "src/FpF.hpp" 5 | #include "src/PID/PID_v1.h" 6 | #include "DCMotor.h" 7 | 8 | #define Fix16 mn::MFixedPoint::FpF32<8> 9 | 10 | typedef enum { 11 | CL_OPEN_LOOP = 0, 12 | CL_POSITION, 13 | CL_VELOCITY, 14 | } cl_control; 15 | 16 | typedef enum { 17 | TARGET_VELOCITY = 0, 18 | TARGET_POSITION 19 | } cl_target; 20 | 21 | class PIDWrapper { 22 | 23 | public: 24 | PIDWrapper(Fix16& inputpos, Fix16& inputvelo, DCMotor* motor, int index, int periodms_pos, int periodms_velo); 25 | 26 | void setGains(Fix16 kp, Fix16 ki, Fix16 kd) { 27 | if (this->mode == CL_VELOCITY) { 28 | pid_velo->SetTunings(kp, ki, kd); 29 | } 30 | if (this->mode == CL_POSITION) { 31 | pid_pos->SetTunings(kp, ki, kd); 32 | } 33 | run(); 34 | }; 35 | 36 | void resetGains(); 37 | 38 | 39 | void getGains(Fix16* gains) { 40 | if (this->mode == CL_VELOCITY) { 41 | gains[0] = pid_velo->GetKp(); 42 | gains[1] = pid_velo->GetKi(); 43 | gains[2] = pid_velo->GetKd(); 44 | } 45 | if (this->mode == CL_POSITION) { 46 | gains[0] = pid_pos->GetKp(); 47 | gains[1] = pid_pos->GetKi(); 48 | gains[2] = pid_pos->GetKd(); 49 | } 50 | }; 51 | 52 | void setControlMode(cl_control mode) { 53 | this->mode = mode; 54 | run(); 55 | }; 56 | 57 | void setSetpoint(cl_target control_target, Fix16 target) { 58 | if (control_target == TARGET_VELOCITY) { 59 | this->targetvelo = target; 60 | } else if (control_target == TARGET_POSITION) { 61 | this->targetpos = target; 62 | } 63 | run(); 64 | }; 65 | 66 | void setMaxAcceleration(Fix16 maxAccel) { 67 | this->maxAcceleration = maxAccel; 68 | run(); 69 | }; 70 | 71 | void setMaxVelocity(Fix16 maxVelocity) { 72 | this->maxVelocity = maxVelocity; 73 | run(); 74 | }; 75 | 76 | void setLimits(int16_t minDuty, int16_t maxDuty) { 77 | if (mode == CL_POSITION) { 78 | pid_pos->SetOutputLimits(Fix16(minDuty), Fix16(maxDuty)); 79 | } 80 | if (mode == CL_VELOCITY) { 81 | pid_velo->SetOutputLimits(Fix16(minDuty), Fix16(maxDuty)); 82 | } 83 | run(); 84 | }; 85 | 86 | void run() { 87 | pid_velo->SetMode(AUTOMATIC); 88 | pid_pos->SetMode(AUTOMATIC); 89 | }; 90 | 91 | void stop() { 92 | pid_velo->SetMode(MANUAL); 93 | pid_pos->SetMode(MANUAL); 94 | }; 95 | 96 | cl_control mode = CL_VELOCITY; 97 | Fix16 targetpos = 0.0; 98 | Fix16 targetvelo = 0.0; 99 | Fix16 maxAcceleration; 100 | Fix16 maxVelocity; 101 | int maxDuty = 100; 102 | int minDuty = 0; 103 | Fix16 actualDuty; 104 | Fix16 velocmd = 0.0f; 105 | PID* pid_velo; 106 | PID* pid_pos; 107 | DCMotor* motor; 108 | bool disabled = true; 109 | }; 110 | 111 | #endif 112 | -------------------------------------------------------------------------------- /src/ArduinoMotorCarrier.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | ArduinoMotorCarrier.cpp - Library for Arduino Motor Carriers 3 | Copyright (c) 2018-2019 Arduino AG. All right reserved. 4 | This library is free software; you can redistribute it and/or 5 | modify it under the terms of the GNU Lesser General Public 6 | License as published by the Free Software Foundation; either 7 | version 2.1 of the License, or (at your option) any later version. 8 | This library is distributed in the hope that it will be useful, 9 | but WITHOUT ANY WARRANTY; without even the implied warranty of 10 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 11 | Lesser General Public License for more details. 12 | You should have received a copy of the GNU Lesser General Public 13 | License along with this library; if not, write to the Free Software 14 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 15 | */ 16 | 17 | #include "ArduinoMotorCarrier.h" 18 | 19 | namespace mc { 20 | //Set Data (gains) with Fix16 format 21 | void setDataPIDGains(Commands cmd, uint8_t target, Fix16 P, Fix16 I, Fix16 D) { 22 | Wire.beginTransmission(I2C_ADDRESS); 23 | Wire.write((uint8_t)cmd); 24 | Wire.write((uint8_t)target); 25 | Wire.write((uint8_t*)&P, 4); 26 | Wire.write((uint8_t*)&I, 4); 27 | Wire.write((uint8_t*)&D, 4); 28 | Wire.endTransmission(); 29 | } 30 | 31 | void setData(Commands cmd, uint8_t target, int data) { 32 | Wire.beginTransmission(I2C_ADDRESS); 33 | Wire.write((uint8_t)cmd); 34 | Wire.write((uint8_t)target); 35 | Wire.write((uint8_t*)&data, 4); 36 | Wire.endTransmission(); 37 | } 38 | 39 | int getDataPIDGains(Commands cmd, uint8_t target, uint8_t* buf, int dataSize) { 40 | Wire.beginTransmission(I2C_ADDRESS); 41 | Wire.write((uint8_t)cmd); 42 | Wire.write((uint8_t)target); 43 | Wire.endTransmission(); 44 | 45 | int i = 0; 46 | Wire.requestFrom(I2C_ADDRESS, dataSize + 1); //one extra for the irq_status 47 | uint8_t status = Wire.read(); 48 | if (status != 0) controller.irq_status = status; 49 | 50 | while (Wire.available()) { 51 | buf[i++] = (uint8_t)Wire.read(); 52 | } 53 | return i; 54 | } 55 | 56 | int getData(Commands cmd, uint8_t target, uint8_t* buf) { 57 | Wire.beginTransmission(I2C_ADDRESS); 58 | Wire.write((uint8_t)cmd); 59 | Wire.write((uint8_t)target); 60 | Wire.endTransmission(); 61 | 62 | Wire.requestFrom(I2C_ADDRESS, 5); 63 | uint8_t status = Wire.read(); 64 | if (status != 0) { 65 | controller.irq_status = status; 66 | } 67 | 68 | int i = 0; 69 | while (Wire.available()) { 70 | buf[i++] = (uint8_t)Wire.read(); 71 | } 72 | return i; 73 | } 74 | 75 | int getData(Commands cmd, uint8_t* buf) { 76 | return getData(cmd, 0, buf); 77 | } 78 | } 79 | 80 | mc::MotorController controller; 81 | 82 | mc::ServoMotor servo1; 83 | mc::ServoMotor servo2; 84 | mc::ServoMotor servo3; 85 | mc::ServoMotor servo4; 86 | 87 | mc::DCMotor M1; 88 | mc::DCMotor M2; 89 | d21::DCMotor M3; 90 | d21::DCMotor M4; 91 | 92 | mc::Encoder encoder1; 93 | mc::Encoder encoder2; 94 | 95 | mc::PID pid1; 96 | mc::PID pid2; 97 | 98 | mc::Battery battery; 99 | -------------------------------------------------------------------------------- /src/src/libfixmath/fix16_str.c: -------------------------------------------------------------------------------- 1 | #include "fix16.h" 2 | #include 3 | #include 4 | 5 | static const uint32_t scales[8] = { 6 | /* 5 decimals is enough for full fix16_t precision */ 7 | 1, 10, 100, 1000, 10000, 100000, 100000, 100000 8 | }; 9 | 10 | static char *itoa_loop(char *buf, uint32_t scale, uint32_t value, bool skip) 11 | { 12 | while (scale) 13 | { 14 | unsigned digit = (value / scale); 15 | 16 | if (!skip || digit || scale == 1) 17 | { 18 | skip = false; 19 | *buf++ = '0' + digit; 20 | value %= scale; 21 | } 22 | 23 | scale /= 10; 24 | } 25 | return buf; 26 | } 27 | 28 | void fix16_to_str(fix16_t value, char *buf, int decimals) 29 | { 30 | uint32_t uvalue = (value >= 0) ? value : -value; 31 | if (value < 0) 32 | *buf++ = '-'; 33 | 34 | /* Separate the integer and decimal parts of the value */ 35 | unsigned intpart = uvalue >> 16; 36 | uint32_t fracpart = uvalue & 0xFFFF; 37 | uint32_t scale = scales[decimals & 7]; 38 | fracpart = fix16_mul(fracpart, scale); 39 | 40 | if (fracpart >= scale) 41 | { 42 | /* Handle carry from decimal part */ 43 | intpart++; 44 | fracpart -= scale; 45 | } 46 | 47 | /* Format integer part */ 48 | buf = itoa_loop(buf, 10000, intpart, true); 49 | 50 | /* Format decimal part (if any) */ 51 | if (scale != 1) 52 | { 53 | *buf++ = '.'; 54 | buf = itoa_loop(buf, scale / 10, fracpart, false); 55 | } 56 | 57 | *buf = '\0'; 58 | } 59 | 60 | fix16_t fix16_from_str(const char *buf) 61 | { 62 | while (isspace(*buf)) 63 | buf++; 64 | 65 | /* Decode the sign */ 66 | bool negative = (*buf == '-'); 67 | if (*buf == '+' || *buf == '-') 68 | buf++; 69 | 70 | /* Decode the integer part */ 71 | uint32_t intpart = 0; 72 | int count = 0; 73 | while (isdigit(*buf)) 74 | { 75 | intpart *= 10; 76 | intpart += *buf++ - '0'; 77 | count++; 78 | } 79 | 80 | if (count == 0 || count > 5 81 | || intpart > 32768 || (!negative && intpart > 32767)) 82 | return fix16_overflow; 83 | 84 | fix16_t value = intpart << 16; 85 | 86 | /* Decode the decimal part */ 87 | if (*buf == '.' || *buf == ',') 88 | { 89 | buf++; 90 | 91 | uint32_t fracpart = 0; 92 | uint32_t scale = 1; 93 | while (isdigit(*buf) && scale < 100000) 94 | { 95 | scale *= 10; 96 | fracpart *= 10; 97 | fracpart += *buf++ - '0'; 98 | } 99 | 100 | value += fix16_div(fracpart, scale); 101 | } 102 | 103 | /* Verify that there is no garbage left over */ 104 | while (*buf != '\0') 105 | { 106 | if (!isdigit(*buf) && !isspace(*buf)) 107 | return fix16_overflow; 108 | 109 | buf++; 110 | } 111 | 112 | return negative ? -value : value; 113 | } 114 | 115 | -------------------------------------------------------------------------------- /extras/D11-Bootloader/D11-Bootloader.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #include "reset.h" 3 | #include "Sodaq_wdt.h" 4 | 5 | #define I2C_ADDRESS 0x09 6 | #define LED_BUILTIN 0 7 | 8 | extern uint16_t PAGE_SIZE; 9 | extern uint32_t dest_addr; 10 | extern volatile uint32_t app_start_address; 11 | 12 | extern uint32_t *flash_ptr; 13 | volatile uint8_t buffer[64]; 14 | volatile int buf_available = 0; 15 | 16 | inline void setup_ptrs() __attribute__((always_inline)); 17 | inline void erase_all() __attribute__((always_inline)); 18 | inline void nvm_write_buffer(uint32_t destination_address, const uint8_t *buffer, uint16_t length) __attribute__((always_inline)); 19 | inline void nvm_erase_row(const uint32_t row_address, uint32_t PAGE_SIZE) __attribute__((always_inline)); 20 | 21 | volatile bool stay_in_bootloader = false; 22 | 23 | void requestEvent() { 24 | Wire.write(dest_addr); 25 | } 26 | 27 | static volatile uint8_t command; 28 | static volatile uint8_t last_crc; 29 | 30 | void receiveEvent(int howMany) { 31 | 32 | command = Wire.read(); 33 | if (command == 'r') { 34 | stay_in_bootloader = true; 35 | while (Wire.available()) { 36 | Wire.read(); 37 | } 38 | return; 39 | } 40 | 41 | if (command == 'x') { 42 | digitalWrite(LED_BUILTIN, HIGH); 43 | stay_in_bootloader = false; 44 | } 45 | 46 | if (command == 'w') { 47 | last_crc = (uint8_t)Wire.read(); 48 | while (Wire.available() && buf_available < 64) { 49 | buffer[buf_available++] = (uint8_t)Wire.read(); 50 | } 51 | return; 52 | } 53 | 54 | // empty the buffer in case of spurious data 55 | while (Wire.available()) { 56 | Wire.read(); 57 | } 58 | } 59 | 60 | void setup() { 61 | // put your setup code here, to run once: 62 | Wire.begin(I2C_ADDRESS); 63 | Wire.onRequest(requestEvent); 64 | Wire.onReceive(receiveEvent); 65 | pinMode(LED_BUILTIN, OUTPUT); 66 | pinMode(3, INPUT_PULLUP); 67 | 68 | setup_ptrs(); 69 | sodaq_wdt_enable(WDT_PERIOD_2X); 70 | 71 | int start = millis(); 72 | 73 | if (system_get_reset_cause() == SYSTEM_RESET_CAUSE_WDT) { 74 | stay_in_bootloader = true; 75 | } 76 | 77 | while (millis() - start < 100) { 78 | if (stay_in_bootloader) { 79 | erase_all(); 80 | break; 81 | } 82 | } 83 | 84 | if (*flash_ptr != 0xFFFFFFFF && !stay_in_bootloader) { 85 | digitalWrite(LED_BUILTIN, HIGH); 86 | boot_app(); 87 | } 88 | } 89 | 90 | int status = HIGH; 91 | int next = 0; 92 | 93 | void loop() { 94 | 95 | if ((millis() % 1000 > 500) && ((millis() / 1000) % 2 != status)) { 96 | digitalWrite(LED_BUILTIN, status); 97 | status = (millis() / 1000) % 2; 98 | } 99 | 100 | if (sodaq_wdt_flag) { 101 | sodaq_wdt_flag = false; 102 | sodaq_wdt_reset(); 103 | } 104 | 105 | if (stay_in_bootloader == false) { 106 | NVIC_SystemReset(); 107 | } 108 | 109 | if (buf_available == 64) { 110 | 111 | uint8_t crc = 0; 112 | 113 | for (int i = 0; i < 64; i++) { 114 | crc ^= buffer[i]; 115 | } 116 | 117 | if (crc != last_crc) { 118 | buf_available = 0; 119 | return; 120 | } 121 | 122 | noInterrupts(); 123 | nvm_write_buffer(dest_addr, (const uint8_t*)buffer, PAGE_SIZE); 124 | dest_addr += PAGE_SIZE; 125 | buf_available = 0; 126 | interrupts(); 127 | } 128 | } 129 | -------------------------------------------------------------------------------- /examples/NanoMotorCarrier/IMU_Test/IMU_Test.ino: -------------------------------------------------------------------------------- 1 | /* 2 | *************************************************************************** 3 | 4 | Euler_Streaming.pde - part of sample SW for using BNO055 with Arduino 5 | 6 | (C) All rights reserved by ROBERT BOSCH GMBH 7 | 8 | Copyright (C) 2014 Bosch Sensortec GmbH 9 | 10 | This program is free software: you can redistribute it and/or modify 11 | it under the terms of the GNU General Public License as published by 12 | the Free Software Foundation, either version 3 of the License, or 13 | (at your option) any later version. 14 | 15 | This program is distributed in the hope that it will be useful, 16 | but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | GNU General Public License for more details. 19 | 20 | You should have received a copy of the GNU General Public License 21 | along with this program. If not, see . 22 | 23 | **************************************************************************/ 24 | /* Date: 2014/01/07 25 | Revision: 1.2 26 | 27 | */ 28 | 29 | #include "BNO055_support.h" //Contains the bridge code between the API and Arduino 30 | #include 31 | 32 | //The device address is set to BNO055_I2C_ADDR2 in this example. You can change this in the BNO055.h file in the code segment shown below. 33 | // /* bno055 I2C Address */ 34 | // #define BNO055_I2C_ADDR1 0x28 35 | // #define BNO055_I2C_ADDR2 0x29 36 | // #define BNO055_I2C_ADDR BNO055_I2C_ADDR2 37 | 38 | //Pin assignments as tested on the Arduino Due. 39 | //Vdd,Vddio : 3.3V 40 | //GND : GND 41 | //SDA/SCL : SDA/SCL 42 | //PSO/PS1 : GND/GND (I2C mode) 43 | 44 | //This structure contains the details of the BNO055 device that is connected. (Updated after initialization) 45 | struct bno055_t myBNO; 46 | struct bno055_euler myEulerData; //Structure to hold the Euler data 47 | 48 | unsigned long lastTime = 0; 49 | 50 | void setup() //This code is executed once 51 | { 52 | //Initialize I2C communication 53 | Wire.begin(); 54 | 55 | //Initialization of the BNO055 56 | BNO_Init(&myBNO); //Assigning the structure to hold information about the device 57 | 58 | //Configuration to NDoF mode 59 | bno055_set_operation_mode(OPERATION_MODE_NDOF); 60 | 61 | delay(1); 62 | 63 | //Initialize the Serial Port to view information on the Serial Monitor 64 | Serial.begin(115200); 65 | } 66 | 67 | void loop() //This code is looped forever 68 | { 69 | if ((millis() - lastTime) >= 100) //To stream at 10 Hz without using additional timers 70 | { 71 | lastTime = millis(); 72 | 73 | bno055_read_euler_hrp(&myEulerData); //Update Euler data into the structure 74 | 75 | Serial.print("Time Stamp: "); //To read out the Time Stamp 76 | Serial.println(lastTime); 77 | 78 | Serial.print("Heading(Yaw): "); //To read out the Heading (Yaw) 79 | Serial.println(float(myEulerData.h) / 16.00); //Convert to degrees 80 | 81 | Serial.print("Roll: "); //To read out the Roll 82 | Serial.println(float(myEulerData.r) / 16.00); //Convert to degrees 83 | 84 | Serial.print("Pitch: "); //To read out the Pitch 85 | Serial.println(float(myEulerData.p) / 16.00); //Convert to degrees 86 | 87 | Serial.println(); //Extra line to differentiate between packets 88 | } 89 | } 90 | -------------------------------------------------------------------------------- /src/MotorController.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | MotorCOntroller.cpp - Library for Arduino Motor Carriers 3 | Copyright (c) 2018-2019 Arduino AG. All right reserved. 4 | This library is free software; you can redistribute it and/or 5 | modify it under the terms of the GNU Lesser General Public 6 | License as published by the Free Software Foundation; either 7 | version 2.1 of the License, or (at your option) any later version. 8 | This library is distributed in the hope that it will be useful, 9 | but WITHOUT ANY WARRANTY; without even the implied warranty of 10 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 11 | Lesser General Public License for more details. 12 | You should have received a copy of the GNU Lesser General Public 13 | License along with this library; if not, write to the Free Software 14 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 15 | */ 16 | 17 | #include "MotorController.h" 18 | #include "Common.h" 19 | #include "Wire.h" 20 | 21 | #define PMIC_ADDRESS 0x6B 22 | #define PMIC_REG00 0x00 23 | #define PMIC_REG01 0x01 24 | #define PMIC_REG02 0x02 25 | #define PMIC_REG04 0x04 26 | #define PMIC_REG05 0x05 27 | 28 | String mc::MotorController::getFWVersion() { 29 | char ret[5]; 30 | getData(GET_VERSION, (uint8_t*)ret); 31 | ret[4] = '\0'; 32 | return String(ret); 33 | } 34 | 35 | void mc::MotorController::reboot() { 36 | setData(RESET, 0, 0); 37 | delay(500); 38 | } 39 | 40 | void mc::MotorController::ping() { 41 | setData(PING, 0, 0); 42 | } 43 | 44 | float mc::MotorController::getTemperature() { 45 | int ret; 46 | getData(GET_INTERNAL_TEMP, (uint8_t*)&ret); 47 | return (float)ret / 1000.0f; 48 | } 49 | 50 | int mc::MotorController::begin() { 51 | Wire.begin(); 52 | 53 | // PMIC initialization raw APIs: are used to initialize the 54 | // PMIC when used with Nano 33 IoT 55 | #ifdef ARDUINO_SAMD_NANO_33_IOT 56 | Serial.println("Board: Nano 33 IoT"); 57 | enable_battery_charging(); 58 | Serial.println("Charging enabled...."); 59 | #endif 60 | String version = getFWVersion(); 61 | if (version.c_str()[0] == '0') { 62 | return 1; 63 | } 64 | return 0; 65 | }; 66 | 67 | uint8_t mc::MotorController::getIrqStatus() { 68 | int ret; 69 | getData(CLEAR_IRQ, (uint8_t*)&ret); 70 | return irq_status; 71 | } 72 | 73 | int mc::MotorController::getFreeRam() { 74 | int ret; 75 | getData(GET_FREE_RAM, (uint8_t*)&ret); 76 | return ret; 77 | } 78 | 79 | void mc::MotorController::enable_battery_charging() { 80 | Wire.beginTransmission(PMIC_ADDRESS); 81 | Wire.write(PMIC_REG00); 82 | Wire.write(0x06); // min sys voltage 3.88 V + max input current 2.0 A 83 | Wire.endTransmission(); 84 | Wire.beginTransmission(PMIC_ADDRESS); 85 | Wire.write(PMIC_REG01); 86 | Wire.write(0x1B); // Charge Battery + Minimum System Voltage 3.5 V 87 | Wire.endTransmission(); 88 | Wire.beginTransmission(PMIC_ADDRESS); 89 | Wire.write(PMIC_REG02); 90 | Wire.write(0x00); // Charge current 512 mA 91 | Wire.endTransmission(); 92 | Wire.beginTransmission(PMIC_ADDRESS); 93 | Wire.write(PMIC_REG04); 94 | Wire.write(0x9E); // Charge Voltage Limit 4.128 V 95 | Wire.endTransmission(); 96 | Wire.beginTransmission(PMIC_ADDRESS); 97 | Wire.write(PMIC_REG05); 98 | Wire.write(0x8A); // Enable Battery Charge termination + disable watchdog 99 | Wire.endTransmission(); 100 | } 101 | -------------------------------------------------------------------------------- /examples/Flasher/Flasher.ino: -------------------------------------------------------------------------------- 1 | /* 2 | STANDALONE FIRMWARE UPDATE FOR Arduino Motor Carrier 3 | 4 | To generate a new firmware, compile D11-Firmware with target MKRMotorShield, 4KB bootloader, LTO enabled, pinmap complete 5 | and execute 6 | 7 | echo -n "const " > fw.h && xxd -i D11-Firmware.ino.bin >> fw.h 8 | */ 9 | 10 | #include "Wire.h" 11 | #include "ArduinoMotorCarrier.h" 12 | 13 | #ifdef ARDUINO_SAMD_NANO_33_IOT 14 | #include "fw_nano.h" 15 | #else 16 | #include "fw_mkr.h" 17 | #endif 18 | 19 | #define I2C_ADDRESS 0x09 20 | 21 | void setDataRunning(int cmd, uint8_t target, int data) { 22 | Wire.beginTransmission(0x66); 23 | Wire.write((uint8_t)cmd); 24 | Wire.write((uint8_t)target); 25 | Wire.write(data); 26 | Wire.endTransmission(); 27 | } 28 | 29 | 30 | void setup() { 31 | // put your setup code here, to run once: 32 | Wire.begin(); 33 | Wire.setClock(1000000); 34 | 35 | Serial.begin(115200); 36 | while (!Serial); 37 | pinMode(6, OUTPUT); 38 | digitalWrite(6, HIGH); 39 | 40 | int prev_address = -1; 41 | 42 | Wire.beginTransmission(0x66); 43 | Wire.write((uint8_t)GET_VERSION); 44 | Wire.write((uint8_t)0); 45 | Wire.endTransmission(); 46 | 47 | Wire.requestFrom(0x66, 5); 48 | Wire.read(); 49 | 50 | String version = ""; 51 | 52 | while (Wire.available()) { 53 | version += (char)Wire.read(); 54 | } 55 | 56 | if (version.c_str()[0] == '0') { 57 | Serial.println("Reset D11"); 58 | setDataRunning(RESET, 0, 0); 59 | delay(10); 60 | } else { 61 | // TODO: on NanoMotorCarrier we have the change to forcefully reset the D11; do it now if it is unresponsive 62 | 63 | } 64 | 65 | // reset running D11 66 | Serial.println("Erase flash"); 67 | 68 | Wire.beginTransmission(I2C_ADDRESS); 69 | Wire.write('r'); 70 | Wire.endTransmission(); 71 | 72 | delay(500); 73 | 74 | Serial.println("Starting flash"); 75 | 76 | int address = 0; 77 | while (address < (D11_Firmware_ino_bin_len + 0x1000)) { 78 | int retry = 0; 79 | do { 80 | Wire.requestFrom(I2C_ADDRESS, 4); 81 | uint8_t buf[4]; 82 | int k = 0; 83 | while (Wire.available()) { 84 | buf[k++] = Wire.read(); 85 | } 86 | address = *(uint32_t*)buf; 87 | delay(10); 88 | } while (prev_address == address && retry++ < 5); 89 | prev_address = address; 90 | Serial.println(address); 91 | 92 | uint8_t crc = 0; 93 | for (int j = 0; j < 64; j++) { 94 | crc ^= D11_Firmware_ino_bin[address - 0x1000 + j]; 95 | } 96 | 97 | Serial.println(crc, HEX); 98 | 99 | Wire.beginTransmission(I2C_ADDRESS); 100 | Wire.write('w'); 101 | Wire.write(crc); 102 | Wire.write(&D11_Firmware_ino_bin[address - 0x1000], 64); 103 | Wire.endTransmission(); 104 | } 105 | 106 | Serial.println("Booting FW"); 107 | Wire.beginTransmission(I2C_ADDRESS); 108 | Wire.write('x'); 109 | Wire.endTransmission(); 110 | 111 | delay(1000); 112 | 113 | Wire.beginTransmission(0x66); 114 | Wire.write((uint8_t)GET_VERSION); 115 | Wire.write(0); 116 | Wire.endTransmission(); 117 | 118 | Wire.requestFrom(0x66, 5); 119 | Wire.read(); 120 | 121 | Serial.print("New version: "); 122 | while (Wire.available()) { 123 | Serial.print((char)Wire.read()); 124 | } 125 | Serial.println(); 126 | } 127 | 128 | void loop() { 129 | // put your main code here, to run repeatedly: 130 | 131 | } 132 | -------------------------------------------------------------------------------- /extras/D11-Firmware/src/Encoder/utility/direct_pin_read.h: -------------------------------------------------------------------------------- 1 | #ifndef direct_pin_read_h_ 2 | #define direct_pin_read_h_ 3 | 4 | #if defined(__AVR__) || (defined(__arm__) && defined(CORE_TEENSY)) 5 | 6 | #define IO_REG_TYPE uint8_t 7 | #define PIN_TO_BASEREG(pin) (portInputRegister(digitalPinToPort(pin))) 8 | #define PIN_TO_BITMASK(pin) (digitalPinToBitMask(pin)) 9 | #define DIRECT_PIN_READ(base, mask) (((*(base)) & (mask)) ? 1 : 0) 10 | 11 | #elif defined(__SAM3X8E__) // || defined(ESP8266) 12 | 13 | #define IO_REG_TYPE uint32_t 14 | #define PIN_TO_BASEREG(pin) (portInputRegister(digitalPinToPort(pin))) 15 | #define PIN_TO_BITMASK(pin) (digitalPinToBitMask(pin)) 16 | #define DIRECT_PIN_READ(base, mask) (((*(base)) & (mask)) ? 1 : 0) 17 | 18 | #elif defined(__PIC32MX__) 19 | 20 | #define IO_REG_TYPE uint32_t 21 | #define PIN_TO_BASEREG(pin) (portModeRegister(digitalPinToPort(pin))) 22 | #define PIN_TO_BITMASK(pin) (digitalPinToBitMask(pin)) 23 | #define DIRECT_PIN_READ(base, mask) (((*(base+4)) & (mask)) ? 1 : 0) 24 | 25 | /* ESP8266 v2.0.0 Arduino workaround for bug https://github.com/esp8266/Arduino/issues/1110 */ 26 | #elif defined(ESP8266) 27 | 28 | #define IO_REG_TYPE uint32_t 29 | #define PIN_TO_BASEREG(pin) ((volatile uint32_t *)(0x60000000+(0x318))) 30 | #define PIN_TO_BITMASK(pin) (digitalPinToBitMask(pin)) 31 | #define DIRECT_PIN_READ(base, mask) (((*(base)) & (mask)) ? 1 : 0) 32 | 33 | /* ESP32 Arduino (https://github.com/espressif/arduino-esp32) */ 34 | #elif defined(ESP32) 35 | 36 | #define IO_REG_TYPE uint32_t 37 | #define PIN_TO_BASEREG(pin) (portInputRegister(digitalPinToPort(pin))) 38 | #define PIN_TO_BITMASK(pin) (digitalPinToBitMask(pin)) 39 | #define DIRECT_PIN_READ(base, mask) (((*(base)) & (mask)) ? 1 : 0) 40 | 41 | #elif defined(ARDUINO_ARCH_SAMD) 42 | 43 | #define IO_REG_TYPE uint32_t 44 | #define PIN_TO_BASEREG(pin) portModeRegister(digitalPinToPort(pin)) 45 | #define PIN_TO_BITMASK(pin) (digitalPinToBitMask(pin)) 46 | #define DIRECT_PIN_READ(base, mask) (((*((base)+8)) & (mask)) ? 1 : 0) 47 | 48 | #elif defined(RBL_NRF51822) 49 | 50 | #define IO_REG_TYPE uint32_t 51 | #define PIN_TO_BASEREG(pin) (0) 52 | #define PIN_TO_BITMASK(pin) (pin) 53 | #define DIRECT_PIN_READ(base, pin) nrf_gpio_pin_read(pin) 54 | 55 | #elif defined(__arc__) /* Arduino101/Genuino101 specifics */ 56 | 57 | #include "scss_registers.h" 58 | #include "portable.h" 59 | #include "avr/pgmspace.h" 60 | #define GPIO_ID(pin) (g_APinDescription[pin].ulGPIOId) 61 | #define GPIO_TYPE(pin) (g_APinDescription[pin].ulGPIOType) 62 | #define GPIO_BASE(pin) (g_APinDescription[pin].ulGPIOBase) 63 | #define EXT_PORT_OFFSET_SS 0x0A 64 | #define EXT_PORT_OFFSET_SOC 0x50 65 | #define PIN_TO_BASEREG(pin) ((volatile uint32_t *)g_APinDescription[pin].ulGPIOBase) 66 | #define PIN_TO_BITMASK(pin) pin 67 | #define IO_REG_TYPE uint32_t 68 | static inline __attribute__((always_inline)) 69 | IO_REG_TYPE directRead(volatile IO_REG_TYPE *base, IO_REG_TYPE pin) 70 | { 71 | IO_REG_TYPE ret; 72 | if (SS_GPIO == GPIO_TYPE(pin)) { 73 | ret = READ_ARC_REG(((IO_REG_TYPE)base + EXT_PORT_OFFSET_SS)); 74 | } else { 75 | ret = MMIO_REG_VAL_FROM_BASE((IO_REG_TYPE)base, EXT_PORT_OFFSET_SOC); 76 | } 77 | return ((ret >> GPIO_ID(pin)) & 0x01); 78 | } 79 | #define DIRECT_PIN_READ(base, pin) directRead(base, pin) 80 | 81 | #endif 82 | 83 | #endif 84 | -------------------------------------------------------------------------------- /src/PID.h: -------------------------------------------------------------------------------- 1 | #ifndef __PID_H__ 2 | #define __PID_H__ 3 | 4 | #include "Arduino.h" 5 | #include "DCMotor.h" 6 | //-------------------------------- ---------------------- 7 | #include "src/FpF.hpp" 8 | #define Fix16 mn::MFixedPoint::FpF32<8> 9 | //------------------------------------------------------ 10 | 11 | typedef enum { 12 | CL_OPEN_LOOP = 0, 13 | CL_POSITION, 14 | CL_VELOCITY, 15 | } cl_control; 16 | 17 | typedef enum { 18 | TARGET_VELOCITY = 0, 19 | TARGET_POSITION 20 | } cl_target; 21 | 22 | namespace mc { 23 | 24 | class PID { 25 | 26 | public: 27 | PID(); 28 | 29 | void setGains(Fix16 kp, Fix16 ki, Fix16 kd); 30 | void resetGains(); 31 | void setControlMode(cl_control mode); 32 | void setSetpoint(cl_target control_target, int target); 33 | void setMaxAcceleration(int maxAccel); 34 | void setMaxVelocity(int maxVelocity); 35 | void setLimits(int16_t minDuty, int16_t maxDuty); 36 | Fix16 getPgain(); 37 | Fix16 getIgain(); 38 | Fix16 getDgain(); 39 | 40 | private: 41 | int instance; 42 | }; 43 | } 44 | 45 | #include "src/PID/PID_v1.h" 46 | #include "Encoder.h" 47 | 48 | namespace d21 { 49 | class PID { 50 | 51 | public: 52 | PID(mc::Encoder& encoder, DCMotor& motor, int index, int periodms_velo , int periodms_pos); 53 | 54 | void setGains(float kp, float ki, float kd); 55 | void setGains(int16_t kp, int16_t ki, int16_t kd) { 56 | setGains((float)kp, (float)ki, (float)kd); 57 | } 58 | void resetGains(); 59 | void setControlMode(cl_control mode); 60 | void setSetpoint(cl_target control_target, int16_t target); 61 | void setMaxAcceleration(int16_t maxAccel); 62 | void setMaxVelocity(int16_t maxVelocity); 63 | void setLimits(int16_t minDuty, int16_t maxDuty); 64 | 65 | void run() { 66 | pid_velo->SetMode(AUTOMATIC); 67 | pid_pos->SetMode(AUTOMATIC); 68 | }; 69 | 70 | /* ATTENTION: d21::PID::update() should be called very often */ 71 | void update() { 72 | 73 | inputpos = (float)encoder->getRawCount(); 74 | inputvelo = (float)encoder->getCountPerSecond(); 75 | 76 | static float prevvelocmd = 0.0f; 77 | if (mode == CL_POSITION) { 78 | if (pid_pos->Compute()) { 79 | //slew limit velocity command with max accel 80 | float curvelocmd = velocmd; 81 | if ((prevvelocmd - curvelocmd) > maxAcceleration) curvelocmd = prevvelocmd - maxAcceleration;//limit decel 82 | if ((curvelocmd - prevvelocmd) > maxAcceleration) curvelocmd = prevvelocmd + maxAcceleration;//limit accel 83 | //copy position PID output to velocity PID setpoint 84 | targetvelo = curvelocmd; 85 | //save curvelocmd for next iteration 86 | prevvelocmd = curvelocmd; 87 | } 88 | } 89 | 90 | if (pid_velo->Compute()) { 91 | int dutyout = (int)actualDuty; 92 | //obj[i]->motor->setDuty(dutyout); not working so using line below instead 93 | 94 | //deadzone compensation 95 | if (dutyout > 0) dutyout += 13; else dutyout -= 13; 96 | motor->setDuty(dutyout); 97 | } 98 | }; 99 | 100 | void stop() { 101 | pid_velo->SetMode(MANUAL); 102 | pid_pos->SetMode(MANUAL); 103 | }; 104 | 105 | private: 106 | int instance; 107 | cl_control mode = CL_VELOCITY; 108 | float targetpos = 0.0f; 109 | float targetvelo = 0.0f; 110 | float inputpos = 0.0f; 111 | float inputvelo = 0.0f; 112 | float maxAcceleration; 113 | float maxVelocity; 114 | int maxDuty = 100; 115 | int minDuty = 0; 116 | float actualDuty; 117 | float velocmd = 0.0f; 118 | std::PID* pid_velo; 119 | std::PID* pid_pos; 120 | DCMotor* motor; 121 | mc::Encoder* encoder; 122 | }; 123 | } 124 | 125 | #endif 126 | -------------------------------------------------------------------------------- /extras/D11-Firmware/adc.ino: -------------------------------------------------------------------------------- 1 | #if 0 2 | 3 | #define WAIT_ADC_SYNC while (ADC->STATUS.bit.SYNCBUSY) {} 4 | #define WAIT_ADC_RESET while (ADC->CTRLA.bit.SWRST) {} 5 | 6 | void adc_reset() { 7 | WAIT_ADC_SYNC 8 | ADC->CTRLA.bit.ENABLE = 0; //disable DAC 9 | WAIT_ADC_SYNC 10 | ADC->CTRLA.bit.SWRST = 1; //reset ADC 11 | WAIT_ADC_SYNC 12 | WAIT_ADC_RESET 13 | } 14 | 15 | void adc_init() { 16 | //make sure the digital interface clock is enabled (should be already after reset) 17 | PM->APBCMASK.reg |= PM_APBCMASK_ADC; 18 | 19 | // Enable ADC conversion clock, take from GCLK0 20 | GCLK->CLKCTRL.reg = GCLK_CLKCTRL_ID( GCM_ADC ) | 21 | GCLK_CLKCTRL_GEN_GCLK0 | 22 | GCLK_CLKCTRL_CLKEN ; 23 | 24 | //reset ADC to defaults 25 | adc_reset(); 26 | 27 | //Control B: Prescaler (/256), resolution selection (16 bit), correction enable (no), freerun mode (no) 28 | ADC->CTRLB.reg = 0x610; 29 | WAIT_ADC_SYNC 30 | 31 | //sample time in half clock cycles will be set before conversion, set to max (slow but high impedance) 32 | ADC->SAMPCTRL.reg = 0x3f; 33 | WAIT_ADC_SYNC 34 | 35 | //enable accumulation / averaging: 1024 samples to 16 bit resolution 36 | ADC->AVGCTRL.reg = 0x0a; 37 | WAIT_ADC_SYNC 38 | 39 | adc_read(0x18, true); //dummy read a generic channel, discard result 40 | } 41 | 42 | uint32_t adc_read(uint8_t channel, bool onevolt) { 43 | 44 | if (onevolt) { //one volt full scale: use internal 1.0V reference, gain 1 45 | ADC->INPUTCTRL.bit.GAIN = ADC_INPUTCTRL_GAIN_1X_Val; 46 | ADC->REFCTRL.bit.REFSEL = ADC_REFCTRL_REFSEL_INT1V_Val; 47 | WAIT_ADC_SYNC 48 | ADC->INPUTCTRL.reg = channel | 0x1800; //select channel as MUXPOS, MUXNEG = internal GND 49 | WAIT_ADC_SYNC 50 | } else { //3.3V full: use half supply as reference, set gain to 0.5 51 | ADC->INPUTCTRL.bit.GAIN = ADC_INPUTCTRL_GAIN_DIV2_Val; 52 | ADC->REFCTRL.bit.REFSEL = ADC_REFCTRL_REFSEL_INTVCC1_Val; 53 | WAIT_ADC_SYNC 54 | ADC->INPUTCTRL.reg = channel | 0x1900; //select channel as MUXPOS, MUXNEG = IO GND 55 | WAIT_ADC_SYNC 56 | } 57 | 58 | ADC->SAMPCTRL.reg = 0x3f; //make sure we're sampling slowly 59 | WAIT_ADC_SYNC 60 | 61 | ADC->CTRLA.bit.ENABLE = 1; //enable ADC 62 | WAIT_ADC_SYNC 63 | 64 | ADC->SWTRIG.bit.START = 1; // Start ADC conversion 65 | while (!(ADC->INTFLAG.bit.RESRDY)) {} //wait until ADC conversion is done 66 | WAIT_ADC_SYNC 67 | uint32_t val = ADC->RESULT.reg; 68 | 69 | ADC->INTFLAG.reg = ADC_INTFLAG_RESRDY; // clear result ready flag 70 | WAIT_ADC_SYNC 71 | 72 | ADC->CTRLA.bit.ENABLE = 0; //disable ADC 73 | WAIT_ADC_SYNC 74 | 75 | return val; 76 | } 77 | 78 | 79 | #define ADC_TEMP_CHANNEL 0x18 80 | 81 | void temp_init() { 82 | SYSCTRL->VREF.reg |= SYSCTRL_VREF_TSEN; //enable temperature sensor, route to ADC 83 | adc_init(); 84 | } 85 | 86 | uint32_t temp_read_raw() { //returns 16 bit. 87 | return adc_read(ADC_TEMP_CHANNEL, true); 88 | } 89 | 90 | int32_t temp_raw_to_mdeg(uint32_t raw) { 91 | int32_t adc = raw; 92 | //use device factory calibration values for temperature conversion (simplified) 93 | uint32_t* tmpLogBase = (uint32_t*)0x00806030; 94 | uint32_t tmpLog0 = tmpLogBase[0]; 95 | uint32_t tmpLog1 = tmpLogBase[1]; 96 | uint8_t roomInt = tmpLog0 & 0xff; 97 | uint8_t roomFrac = (tmpLog0 >> 8) & 0x0f; 98 | uint8_t hotInt = (tmpLog0 >> 12) & 0xff; 99 | uint8_t hotFrac = (tmpLog0 >> 20) & 0x0f; 100 | int32_t roomADC = ((tmpLog1 >> 8) & 0xfff) << 4; 101 | int32_t hotADC = ((tmpLog1 >> 20) & 0xfff) << 4; 102 | int32_t roomMdeg = 1000 * roomInt + 100 * roomFrac; 103 | int32_t hotMdeg = 1000 * hotInt + 100 * hotFrac; 104 | int32_t mdeg = roomMdeg + ((hotMdeg-roomMdeg) * (adc-roomADC)) / (hotADC-roomADC); 105 | return mdeg; 106 | } 107 | 108 | #endif 109 | -------------------------------------------------------------------------------- /examples/NanoMotorCarrier/PID_Position_test/PID_Position_test.ino: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | //Variable to store the battery voltage 4 | static int batteryVoltage; 5 | 6 | //Variable to change the motor speed and direction 7 | static int duty = 0; 8 | 9 | int target; 10 | 11 | void setup() 12 | { 13 | //Serial port initialization 14 | Serial.begin(115200); 15 | while (!Serial); 16 | 17 | //Establishing the communication with the Motor Carrier 18 | if (controller.begin()) 19 | { 20 | Serial.print("Motor Carrier connected, firmware version "); 21 | Serial.println(controller.getFWVersion()); 22 | } 23 | else 24 | { 25 | Serial.println("Couldn't connect! Is the red LED blinking? You may need to update the firmware with FWUpdater sketch"); 26 | while (1); 27 | } 28 | 29 | // Reboot the motor controller; brings every value back to default 30 | Serial.println("reboot"); 31 | controller.reboot(); 32 | delay(500); 33 | 34 | //Take the battery status 35 | float batteryVoltage = (float)battery.getConverted(); 36 | Serial.print("Battery voltage: "); 37 | Serial.print(batteryVoltage); 38 | Serial.print("V, Raw "); 39 | Serial.println(battery.getRaw()); 40 | 41 | int dutyInit = 0; // at 50 it works as expected, at 60 shift sides and is too small duty to move, at 70 is very big duty. 42 | M1.setDuty(dutyInit); 43 | M2.setDuty(dutyInit); 44 | M3.setDuty(dutyInit); 45 | M4.setDuty(dutyInit); 46 | Serial.print("Duty: "); 47 | Serial.println(dutyInit); 48 | 49 | /************* PID 1 ***********************/ 50 | 51 | // pid1.setControlMode(CL_POSITION); 52 | // 53 | // //pid1.resetGains(); 54 | // //pid1.setLimits(-100,100); 55 | // pid1.setGains(0.01f, 0.017f, 0.0f); //Proportional(change) Integral(change) Derivative 56 | // Serial.print("P Gain: "); 57 | // Serial.println((float)pid1.getPgain()); 58 | // Serial.print("I Gain: "); 59 | // Serial.println((float)pid1.getIgain()); 60 | // Serial.print("D Gain: "); 61 | // Serial.println((float)pid1.getDgain(), 7); 62 | // Serial.println(""); 63 | // 64 | // encoder1.resetCounter(0); 65 | // Serial.print("encoder1: "); 66 | // Serial.println(encoder1.getRawCount()); 67 | // target = 1000; 68 | // pid1.setSetpoint(TARGET_POSITION, target); 69 | 70 | /************* PID 2 ***********************/ 71 | 72 | pid2.setControlMode(CL_POSITION); 73 | 74 | //pid1.resetGains(); 75 | //pid1.setLimits(-100,100); 76 | pid2.setGains(0.1f, 0.0f, 0.0f); //Proportional(change) Integral(change) Derivative 77 | Serial.print("P Gain: "); 78 | Serial.println((float)pid2.getPgain()); 79 | Serial.print("I Gain: "); 80 | Serial.println((float)pid2.getIgain()); 81 | Serial.print("D Gain: "); 82 | Serial.println((float)pid2.getDgain(), 7); 83 | Serial.println(""); 84 | 85 | encoder2.resetCounter(0); 86 | Serial.print("encoder2: "); 87 | Serial.println(encoder2.getRawCount()); 88 | target = 1000; 89 | pid2.setSetpoint(TARGET_POSITION, target); 90 | 91 | } 92 | 93 | void loop() { 94 | 95 | /************* PID 1 ***********************/ 96 | 97 | // Serial.print("encoder1: "); 98 | // Serial.print(encoder1.getRawCount()); 99 | // Serial.print(" target: "); 100 | // Serial.println(target); 101 | // if (encoder1.getRawCount() == target) { 102 | // target += 1000; 103 | // Serial.print("Target reached: Setting new target.."); 104 | // pid1.setSetpoint(TARGET_POSITION, target); 105 | // //delay(5000); 106 | // } 107 | 108 | /************* PID 2 ***********************/ 109 | 110 | Serial.print("encoder2: "); 111 | Serial.print(encoder2.getRawCount()); 112 | Serial.print(" target: "); 113 | Serial.println(target); 114 | if (encoder2.getRawCount() == target) { 115 | target += 1000; 116 | Serial.print("Target reached: Setting new target.."); 117 | pid2.setSetpoint(TARGET_POSITION, target); 118 | //delay(5000); 119 | } 120 | 121 | //--------------------------------------- 122 | controller.ping(); 123 | //wait 124 | delay(50); 125 | } 126 | -------------------------------------------------------------------------------- /src/src/PID/PID_v1.h: -------------------------------------------------------------------------------- 1 | #ifndef PID_v1_h 2 | #define PID_v1_h 3 | #define LIBRARY_VERSION 1.2.1 4 | 5 | namespace std { 6 | 7 | class PID 8 | { 9 | 10 | 11 | public: 12 | 13 | //Constants used in some of the functions below 14 | #define AUTOMATIC 1 15 | #define MANUAL 0 16 | #define DIRECT 0 17 | #define REVERSE 1 18 | #define P_ON_M 0 19 | #define P_ON_E 1 20 | 21 | //commonly used functions ************************************************************************** 22 | PID(float*, float*, float*, // * constructor. links the PID to the Input, Output, and 23 | float, float, float, int, int);// Setpoint. Initial tuning parameters are also set here. 24 | // (overload for specifying proportional mode) 25 | 26 | PID(float*, float*, float*, // * constructor. links the PID to the Input, Output, and 27 | float, float, float, int); // Setpoint. Initial tuning parameters are also set here 28 | 29 | void SetMode(int Mode); // * sets PID to either Manual (0) or Auto (non-0) 30 | 31 | bool Compute(); // * performs the PID calculation. it should be 32 | // called every time loop() cycles. ON/OFF and 33 | // calculation frequency can be set using SetMode 34 | // SetSampleTime respectively 35 | 36 | void SetOutputLimits(float, float); // * clamps the output to a specific range. 0-255 by default, but 37 | // it's likely the user will want to change this depending on 38 | // the application 39 | 40 | 41 | 42 | //available but not commonly used functions ******************************************************** 43 | void SetTunings(float, float, // * While most users will set the tunings once in the 44 | float); // constructor, this function gives the user the option 45 | // of changing tunings during runtime for Adaptive control 46 | void SetTunings(float, float, // * overload for specifying proportional mode 47 | float, int); 48 | 49 | void SetControllerDirection(int); // * Sets the Direction, or "Action" of the controller. DIRECT 50 | // means the output will increase when error is positive. REVERSE 51 | // means the opposite. it's very unlikely that this will be needed 52 | // once it is set in the constructor. 53 | void SetSampleTime(int); // * sets the frequency, in Milliseconds, with which 54 | // the PID calculation is performed. default is 100 55 | 56 | 57 | 58 | //Display functions **************************************************************** 59 | float GetKp(); // These functions query the pid for interal values. 60 | float GetKi(); // they were created mainly for the pid front-end, 61 | float GetKd(); // where it's important to know what is actually 62 | int GetMode(); // inside the PID. 63 | int GetDirection(); // 64 | 65 | private: 66 | void Initialize(); 67 | 68 | float dispKp; // * we'll hold on to the tuning parameters in user-entered 69 | float dispKi; // format for display purposes 70 | float dispKd; // 71 | 72 | float kp; // * (P)roportional Tuning Parameter 73 | float ki; // * (I)ntegral Tuning Parameter 74 | float kd; // * (D)erivative Tuning Parameter 75 | 76 | int controllerDirection; 77 | int pOn; 78 | 79 | float *myInput; // * Pointers to the Input, Output, and Setpoint variables 80 | float *myOutput; // This creates a hard link between the variables and the 81 | float *mySetpoint; // PID, freeing the user from having to constantly tell us 82 | // what these values are. with pointers we'll just know. 83 | 84 | unsigned long lastTime; 85 | float outputSum, lastInput; 86 | 87 | unsigned long SampleTime; 88 | float outMin, outMax; 89 | bool inAuto, pOnE; 90 | }; 91 | 92 | } 93 | #endif 94 | 95 | -------------------------------------------------------------------------------- /extras/D11-Firmware/src/PID/PID_v1.h: -------------------------------------------------------------------------------- 1 | #ifndef PID_v1_h 2 | #define PID_v1_h 3 | #define LIBRARY_VERSION 1.2.1 4 | 5 | #include "../FpF.hpp" 6 | 7 | #define Fix16 mn::MFixedPoint::FpF32<8> 8 | 9 | class PID 10 | { 11 | 12 | 13 | public: 14 | 15 | //Constants used in some of the functions below 16 | #define AUTOMATIC 1 17 | #define MANUAL 0 18 | #define DIRECT 0 19 | #define REVERSE 1 20 | #define P_ON_M 0 21 | #define P_ON_E 1 22 | 23 | //commonly used functions ************************************************************************** 24 | PID(Fix16*, Fix16*, Fix16*, // * constructor. links the PID to the Input, Output, and 25 | Fix16, Fix16, Fix16, int, int);// Setpoint. Initial tuning parameters are also set here. 26 | // (overload for specifying proportional mode) 27 | 28 | PID(Fix16*, Fix16*, Fix16*, // * constructor. links the PID to the Input, Output, and 29 | Fix16, Fix16, Fix16, int); // Setpoint. Initial tuning parameters are also set here 30 | 31 | void SetMode(int Mode); // * sets PID to either Manual (0) or Auto (non-0) 32 | 33 | bool Compute(); // * performs the PID calculation. it should be 34 | // called every time loop() cycles. ON/OFF and 35 | // calculation frequency can be set using SetMode 36 | // SetSampleTime respectively 37 | 38 | void SetOutputLimits(Fix16, Fix16); // * clamps the output to a specific range. 0-255 by default, but 39 | // it's likely the user will want to change this depending on 40 | // the application 41 | 42 | 43 | 44 | //available but not commonly used functions ******************************************************** 45 | void SetTunings(Fix16, Fix16, // * While most users will set the tunings once in the 46 | Fix16); // constructor, this function gives the user the option 47 | // of changing tunings during runtime for Adaptive control 48 | void SetTunings(Fix16, Fix16, // * overload for specifying proportional mode 49 | Fix16, int); 50 | 51 | void SetControllerDirection(int); // * Sets the Direction, or "Action" of the controller. DIRECT 52 | // means the output will increase when error is positive. REVERSE 53 | // means the opposite. it's very unlikely that this will be needed 54 | // once it is set in the constructor. 55 | void SetSampleTime(int); // * sets the frequency, in Milliseconds, with which 56 | // the PID calculation is performed. default is 100 57 | 58 | 59 | 60 | //Display functions **************************************************************** 61 | Fix16 GetKp(); // These functions query the pid for interal values. 62 | Fix16 GetKi(); // they were created mainly for the pid front-end, 63 | Fix16 GetKd(); // where it's important to know what is actually 64 | int GetMode(); // inside the PID. 65 | int GetDirection(); // 66 | 67 | private: 68 | void Initialize(); 69 | 70 | Fix16 dispKp; // * we'll hold on to the tuning parameters in user-entered 71 | Fix16 dispKi; // format for display purposes 72 | Fix16 dispKd; // 73 | 74 | Fix16 kp; // * (P)roportional Tuning Parameter 75 | Fix16 ki; // * (I)ntegral Tuning Parameter 76 | Fix16 kd; // * (D)erivative Tuning Parameter 77 | 78 | int controllerDirection; 79 | int pOn; 80 | 81 | Fix16 *myInput; // * Pointers to the Input, Output, and Setpoint variables 82 | Fix16 *myOutput; // This creates a hard link between the variables and the 83 | Fix16 *mySetpoint; // PID, freeing the user from having to constantly tell us 84 | // what these values are. with pointers we'll just know. 85 | 86 | unsigned long lastTime; 87 | Fix16 outputSum, lastInput; 88 | Fix16 lastError; 89 | Fix16 iError; //integral error 90 | 91 | unsigned long SampleTime; 92 | Fix16 outMin, outMax; 93 | bool inAuto, pOnE; 94 | }; 95 | #endif 96 | 97 | -------------------------------------------------------------------------------- /extras/D11-Bootloader/nvm.ino: -------------------------------------------------------------------------------- 1 | /* Application starts from 1 kB memory - Bootloader size is 1 kB 2 | Change the address if higher boot size is needed 3 | Good site for quick conversions: 4 | http://www.binaryhexconverter.com/hex-to-decimal-converter */ 5 | 6 | #include "nvm.h" 7 | 8 | /** If \c false, a page write command will be issued automatically when the 9 | page buffer is full. */ 10 | bool manual_page_write; 11 | 12 | uint8_t data_8 = 1; 13 | uint32_t file_size, i, dest_addr; 14 | uint32_t volatile app_start_address; 15 | uint8_t volatile data_from_flash; 16 | uint32_t *flash_ptr; 17 | uint8_t *flash_byte_ptr; 18 | 19 | /* Flash page size is 64 bytes */ 20 | uint16_t PAGE_SIZE = (8 << NVMCTRL->PARAM.bit.PSZ); //used to read and write to flash. 21 | 22 | void erase_all() { 23 | /* erase all */ 24 | for (int i = APP_START; i < FLASH_SIZE; i = i + PAGE_SIZE) 25 | { 26 | nvm_erase_row(i, PAGE_SIZE); 27 | } 28 | } 29 | 30 | #define SKETCH_START (uint32_t*)(APP_START) 31 | 32 | void boot_app() { 33 | // jump to the sketch 34 | __set_MSP(*SKETCH_START); 35 | 36 | //Reset vector table address 37 | SCB->VTOR = ((uint32_t)(SKETCH_START) & SCB_VTOR_TBLOFF_Msk); 38 | 39 | // address of Reset_Handler is written by the linker at the beginning of the .text section (see linker script) 40 | uint32_t resetHandlerAddress = (uint32_t) * (SKETCH_START + 1); 41 | // jump to reset handler 42 | asm("bx %0"::"r"(resetHandlerAddress)); 43 | } 44 | 45 | void setup_ptrs() 46 | { 47 | //set values, for flash pointers. 48 | dest_addr = APP_START; 49 | flash_ptr = (uint32_t*)APP_START; 50 | app_start_address = *flash_ptr; 51 | flash_byte_ptr = (uint8_t*)APP_START; 52 | } 53 | 54 | void nvm_erase_row(const uint32_t row_address, uint32_t PAGE_SIZE) 55 | { 56 | #if 0 57 | /* Check if the address to erase is not aligned to the start of a row */ 58 | if (row_address > ((uint32_t)_nvm_dev.page_size * _nvm_dev.number_of_pages)) 59 | { 60 | return 0; 61 | } 62 | 63 | /* Get a pointer to the module hardware instance */ 64 | if (row_address & ((_nvm_dev.page_size * NVMCTRL_ROW_PAGES) - 1)) 65 | { 66 | return 0; 67 | } 68 | #endif 69 | 70 | /* Check if the module is busy */ 71 | while (!NVMCTRL->INTFLAG.bit.READY); 72 | 73 | /* Clear error flags */ 74 | NVMCTRL->STATUS.reg &= ~NVMCTRL_STATUS_MASK; 75 | 76 | while (!(NVMCTRL->INTFLAG.bit.READY)); 77 | 78 | /* Set address and command */ 79 | NVMCTRL->ADDR.reg = (uintptr_t)&NVM_MEMORY[row_address / 4]; 80 | NVMCTRL->CTRLA.reg = NVMCTRL_CTRLA_CMDEX_KEY | NVMCTRL_CTRLA_CMD_ER; 81 | while (!(NVMCTRL->INTFLAG.bit.READY)); 82 | 83 | } 84 | 85 | void nvm_write_buffer(uint32_t destination_address, const uint8_t *buffer, uint16_t length) 86 | { 87 | #if 0 88 | 89 | /* Check if the destination address is valid */ 90 | if (destination_address > 91 | ((uint32_t)_nvm_dev.page_size * _nvm_dev.number_of_pages)) { 92 | return 0; 93 | } 94 | 95 | /* Check if the write address not aligned to the start of a page */ 96 | if (destination_address & (_nvm_dev.page_size - 1)) { 97 | return 0; 98 | } 99 | 100 | /* Check if the write length is longer than a NVM page */ 101 | if (length > _nvm_dev.page_size) { 102 | return 0; 103 | } 104 | #endif 105 | 106 | /* Check if the module is busy */ 107 | while (!NVMCTRL->INTFLAG.bit.READY); 108 | 109 | //set auto page writes 110 | NVMCTRL->CTRLB.bit.MANW = 0; 111 | 112 | /* Erase the page buffer before buffering new data */ 113 | NVMCTRL->CTRLA.reg = NVMCTRL_CTRLA_CMD_PBC | NVMCTRL_CTRLA_CMDEX_KEY; 114 | 115 | /* Check if the module is busy */ 116 | while (!NVMCTRL->INTFLAG.bit.READY); 117 | 118 | /* Clear error flags */ 119 | NVMCTRL->STATUS.reg &= ~NVMCTRL_STATUS_MASK; 120 | 121 | uint32_t nvm_address = destination_address / 2; 122 | 123 | /* NVM _must_ be accessed as a series of 16-bit words, perform manual copy 124 | to ensure alignment */ 125 | for (uint16_t k = 0; k < length; k += 2) 126 | { 127 | uint16_t data; 128 | 129 | /* Copy first byte of the 16-bit chunk to the temporary buffer */ 130 | data = buffer[k]; 131 | 132 | /* If we are not at the end of a write request with an odd byte count, 133 | store the next byte of data as well */ 134 | if (k < (length - 1)) { 135 | data |= (buffer[k + 1] << 8); 136 | } 137 | /* Store next 16-bit chunk to the NVM memory space */ 138 | NVM_MEMORY[nvm_address++] = data; 139 | NVMCTRL->CTRLA.reg = NVMCTRL_CTRLA_CMDEX_KEY | NVMCTRL_CTRLA_CMD_WP; 140 | 141 | } 142 | 143 | /* If automatic page write mode is enable, then perform a manual NVM 144 | write when the length of data to be programmed is less than page size 145 | */ 146 | if ((manual_page_write == 0) && (length < NVMCTRL_PAGE_SIZE)) { 147 | NVMCTRL->CTRLA.reg = NVMCTRL_CTRLA_CMDEX_KEY | NVMCTRL_CTRLA_CMD_WP; 148 | } 149 | 150 | while (!NVMCTRL->INTFLAG.bit.READY); 151 | } 152 | -------------------------------------------------------------------------------- /.github/workflows/compile-examples.yml: -------------------------------------------------------------------------------- 1 | name: Compile Examples 2 | 3 | # See: https://docs.github.com/en/free-pro-team@latest/actions/reference/events-that-trigger-workflows 4 | on: 5 | push: 6 | paths: 7 | - ".github/workflows/compile-examples.yml" 8 | - "examples/**" 9 | - "src/**" 10 | pull_request: 11 | paths: 12 | - ".github/workflows/compile-examples.yml" 13 | - "examples/**" 14 | - "src/**" 15 | schedule: 16 | # Run every Tuesday at 8 AM UTC to catch breakage caused by changes to external resources (libraries, platforms). 17 | - cron: "0 8 * * TUE" 18 | workflow_dispatch: 19 | repository_dispatch: 20 | 21 | jobs: 22 | build: 23 | name: ${{ matrix.board.fqbn }} 24 | runs-on: ubuntu-latest 25 | 26 | env: 27 | SKETCHES_REPORTS_PATH: sketches-reports 28 | 29 | strategy: 30 | fail-fast: false 31 | 32 | matrix: 33 | board: 34 | - fqbn: arduino:samd:nano_33_iot 35 | platforms: | 36 | - name: arduino:samd 37 | type: nano 38 | artifact-name-suffix: arduino-samd-nano_33_iot 39 | - fqbn: arduino:samd:mkr1000 40 | platforms: | 41 | - name: arduino:samd 42 | type: mkr 43 | artifact-name-suffix: arduino-samd-mkr1000 44 | - fqbn: arduino:samd:mkrzero 45 | platforms: | 46 | - name: arduino:samd 47 | type: mkr 48 | artifact-name-suffix: arduino-samd-mkrzero 49 | - fqbn: arduino:samd:mkrwifi1010 50 | platforms: | 51 | - name: arduino:samd 52 | type: mkr 53 | artifact-name-suffix: arduino-samd-mkrwifi1010 54 | - fqbn: arduino:samd:mkrfox1200 55 | platforms: | 56 | - name: arduino:samd 57 | type: mkr 58 | artifact-name-suffix: arduino-samd-mkrfox1200 59 | - fqbn: arduino:samd:mkrwan1300 60 | platforms: | 61 | - name: arduino:samd 62 | type: mkr 63 | artifact-name-suffix: arduino-samd-mkrwan1300 64 | - fqbn: arduino:samd:mkrwan1310 65 | platforms: | 66 | - name: arduino:samd 67 | type: mkr 68 | artifact-name-suffix: arduino-samd-mkrwan1310 69 | - fqbn: arduino:samd:mkrgsm1400 70 | platforms: | 71 | - name: arduino:samd 72 | type: mkr 73 | artifact-name-suffix: arduino-samd-mkrgsm1400 74 | - fqbn: arduino:samd:mkrnb1500 75 | platforms: | 76 | - name: arduino:samd 77 | type: mkr 78 | artifact-name-suffix: arduino-samd-mkrnb1500 79 | - fqbn: arduino:samd:mkrvidor4000 80 | platforms: | 81 | - name: arduino:samd 82 | type: mkr 83 | artifact-name-suffix: arduino-samd-mkrvidor4000 84 | - fqbn: arduino:mbed_portenta:envie_m7:target_core=cm4 85 | platforms: | 86 | - name: arduino:mbed_portenta 87 | type: mkr 88 | - fqbn: arduino:mbed_portenta:envie_m7 89 | platforms: | 90 | - name: arduino:mbed_portenta 91 | type: mkr 92 | artifact-name-suffix: arduino-mbed_portenta-envie_m7 93 | 94 | # make board type-specific customizations to the matrix jobs 95 | include: 96 | - board: 97 | type: nano 98 | libraries: | 99 | - name: BNO055 100 | - name: Arduino_BQ24195 101 | sketch-paths: | 102 | - examples/NanoMotorCarrier 103 | - board: 104 | type: mkr 105 | libraries: | 106 | - 107 | sketch-paths: | 108 | - examples/MKRMotorCarrier 109 | 110 | 111 | steps: 112 | - name: Checkout repository 113 | uses: actions/checkout@v6 114 | 115 | - name: Compile examples 116 | uses: arduino/compile-sketches@v1 117 | with: 118 | github-token: ${{ secrets.GITHUB_TOKEN }} 119 | fqbn: ${{ matrix.board.fqbn }} 120 | platforms: ${{ matrix.board.platforms }} 121 | libraries: | 122 | # Install the library from the local path. 123 | - source-path: ./ 124 | ${{ matrix.libraries }} 125 | sketch-paths: | 126 | - examples/Flasher 127 | 128 | ${{ matrix.sketch-paths }} 129 | enable-deltas-report: true 130 | sketches-report-path: ${{ env.SKETCHES_REPORTS_PATH }} 131 | 132 | - name: Save sketches report as workflow artifact 133 | uses: actions/upload-artifact@v6 134 | with: 135 | if-no-files-found: error 136 | path: ${{ env.SKETCHES_REPORTS_PATH }} 137 | name: sketches-report-${{ matrix.board.artifact-name-suffix }} 138 | -------------------------------------------------------------------------------- /src/PID.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | PID.cpp - Library for Arduino Motor Carriers 3 | Copyright (c) 2018-2019 Arduino AG. All right reserved. 4 | This library is free software; you can redistribute it and/or 5 | modify it under the terms of the GNU Lesser General Public 6 | License as published by the Free Software Foundation; either 7 | version 2.1 of the License, or (at your option) any later version. 8 | This library is distributed in the hope that it will be useful, 9 | but WITHOUT ANY WARRANTY; without even the implied warranty of 10 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 11 | Lesser General Public License for more details. 12 | You should have received a copy of the GNU Lesser General Public 13 | License along with this library; if not, write to the Free Software 14 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 15 | */ 16 | 17 | #include "PID.h" 18 | #include "Common.h" 19 | 20 | namespace mc { 21 | static int next_instance = 0; 22 | 23 | typedef struct { 24 | Fix16 P; 25 | Fix16 I; 26 | Fix16 D; 27 | } PIDGains; 28 | 29 | union { 30 | Fix16 rxFloat = Fix16(0.0); 31 | uint8_t rxArray[4]; 32 | } PIDGain; 33 | 34 | PID::PID() { 35 | instance = next_instance; 36 | next_instance++; 37 | } 38 | 39 | void PID::setGains(Fix16 kp, Fix16 ki, Fix16 kd) { 40 | setDataPIDGains(SET_PID_GAIN_CL_MOTOR, instance, kp, ki, kd); 41 | } 42 | 43 | void PID::resetGains() { 44 | setData(RESET_PID_GAIN_CL_MOTOR, instance, 0); 45 | } 46 | 47 | void PID::setControlMode(cl_control mode) { 48 | setData(SET_CONTROL_MODE_CL_MOTOR, instance, mode); 49 | } 50 | 51 | void PID::setSetpoint(cl_target control_target, int target) { 52 | if (control_target == TARGET_POSITION) { 53 | setData(SET_POSITION_SETPOINT_CL_MOTOR, instance, target); 54 | } 55 | if (control_target == TARGET_VELOCITY) { 56 | if (!target) { 57 | setData(SET_PWM_DUTY_CYCLE_DC_MOTOR, instance, target); //Fix target = 0 issue in PID_VELOCITY mode. 58 | } else { 59 | setData(SET_VELOCITY_SETPOINT_CL_MOTOR, instance, target); 60 | } 61 | } 62 | } 63 | 64 | void PID::setMaxAcceleration(int maxAccel) { 65 | setData(SET_MAX_ACCELERATION_CL_MOTOR, instance, maxAccel); 66 | } 67 | 68 | void PID::setMaxVelocity(int maxVelocity) { 69 | setData(SET_MAX_VELOCITY_CL_MOTOR, instance, maxVelocity); 70 | } 71 | 72 | void PID::setLimits(int16_t minDuty, int16_t maxDuty) { 73 | setData(SET_MIN_MAX_DUTY_CYCLE_CL_MOTOR, instance, (minDuty << 16 | maxDuty)); 74 | } 75 | 76 | Fix16 mc::PID::getPgain() { 77 | PIDGains pidGains; 78 | int stat = getDataPIDGains(GET_PID_VAL, instance, (uint8_t*)&pidGains ,sizeof(pidGains)); 79 | return pidGains.P; 80 | } 81 | 82 | Fix16 mc::PID::getIgain() { 83 | PIDGains pidGains; 84 | int stat = getDataPIDGains(GET_PID_VAL, instance, (uint8_t*)&pidGains ,sizeof(pidGains)); 85 | return pidGains.I; 86 | } 87 | Fix16 mc::PID::getDgain() { 88 | PIDGains pidGains; 89 | int stat = getDataPIDGains(GET_PID_VAL, instance, (uint8_t*)&pidGains ,sizeof(pidGains)); 90 | return pidGains.D; 91 | } 92 | 93 | } 94 | 95 | namespace d21 { 96 | static int next_instance = 0; 97 | 98 | static float KP_DEFAULT = 5000.0f; 99 | static float KI_DEFAULT = 100.0f; 100 | static float KD_DEFAULT = 0.0f; 101 | 102 | PID::PID(mc::Encoder& encoder, DCMotor& motor, int index, int periodms_velo , int periodms_pos) { 103 | instance = next_instance; 104 | next_instance++; 105 | pid_pos = new std::PID(&inputpos, &velocmd, &targetpos, KP_DEFAULT, KI_DEFAULT, KD_DEFAULT, DIRECT); 106 | pid_velo = new std::PID(&inputvelo, &actualDuty, &targetvelo, KP_DEFAULT, KI_DEFAULT, KD_DEFAULT, DIRECT); 107 | pid_pos->SetSampleTime(periodms_pos); 108 | pid_velo->SetSampleTime(periodms_velo); 109 | pid_pos->SetOutputLimits((short) - 30, (short)30); //position pid can only command +/- max_velo 110 | pid_velo->SetOutputLimits((short) - 100, (short)100); //velocity pid can only command +/- 100 PWM duty cycle 111 | this->motor = &motor; 112 | this->encoder = &encoder; 113 | } 114 | 115 | void PID::setGains(float kp, float ki, float kd) { 116 | pid_pos->SetTunings(kp, ki, kd); 117 | pid_velo->SetTunings(kp, ki, kd); 118 | } 119 | 120 | void PID::resetGains() { 121 | pid_pos->SetTunings(KP_DEFAULT, KI_DEFAULT, KD_DEFAULT); 122 | pid_velo->SetTunings(KP_DEFAULT, KI_DEFAULT, KD_DEFAULT); 123 | } 124 | 125 | void PID::setControlMode(cl_control mode) { 126 | this->mode = mode; 127 | } 128 | 129 | void PID::setSetpoint(cl_target control_target, int16_t target) { 130 | if (control_target == TARGET_VELOCITY) { 131 | this->targetvelo = target; 132 | } else if (control_target == TARGET_POSITION) { 133 | this->targetpos = target; 134 | } 135 | } 136 | 137 | void PID::setMaxAcceleration(int16_t maxAccel) { 138 | this->maxAcceleration = maxAccel; 139 | } 140 | 141 | void PID::setMaxVelocity(int16_t maxVelocity) { 142 | this->maxVelocity = maxVelocity; 143 | } 144 | 145 | void PID::setLimits(int16_t minDuty, int16_t maxDuty) { 146 | this->maxDuty = maxDuty; 147 | this->minDuty = minDuty; 148 | } 149 | } 150 | -------------------------------------------------------------------------------- /.github/workflows/sync-labels.yml: -------------------------------------------------------------------------------- 1 | # Source: https://github.com/arduino/tooling-project-assets/blob/main/workflow-templates/sync-labels.md 2 | name: Sync Labels 3 | 4 | # See: https://docs.github.com/en/actions/reference/events-that-trigger-workflows 5 | on: 6 | push: 7 | paths: 8 | - ".github/workflows/sync-labels.ya?ml" 9 | - ".github/label-configuration-files/*.ya?ml" 10 | pull_request: 11 | paths: 12 | - ".github/workflows/sync-labels.ya?ml" 13 | - ".github/label-configuration-files/*.ya?ml" 14 | schedule: 15 | # Run daily at 8 AM UTC to sync with changes to shared label configurations. 16 | - cron: "0 8 * * *" 17 | workflow_dispatch: 18 | repository_dispatch: 19 | 20 | env: 21 | CONFIGURATIONS_FOLDER: .github/label-configuration-files 22 | CONFIGURATIONS_ARTIFACT: label-configuration-files 23 | 24 | jobs: 25 | check: 26 | runs-on: ubuntu-latest 27 | 28 | steps: 29 | - name: Checkout repository 30 | uses: actions/checkout@v6 31 | 32 | - name: Download JSON schema for labels configuration file 33 | id: download-schema 34 | uses: carlosperate/download-file-action@v2 35 | with: 36 | file-url: https://raw.githubusercontent.com/arduino/tooling-project-assets/main/workflow-templates/assets/sync-labels/arduino-tooling-gh-label-configuration-schema.json 37 | location: ${{ runner.temp }}/label-configuration-schema 38 | 39 | - name: Install JSON schema validator 40 | run: | 41 | sudo npm install \ 42 | --global \ 43 | ajv-cli \ 44 | ajv-formats 45 | 46 | - name: Validate local labels configuration 47 | run: | 48 | # See: https://github.com/ajv-validator/ajv-cli#readme 49 | ajv validate \ 50 | --all-errors \ 51 | -c ajv-formats \ 52 | -s "${{ steps.download-schema.outputs.file-path }}" \ 53 | -d "${{ env.CONFIGURATIONS_FOLDER }}/*.{yml,yaml}" 54 | 55 | download: 56 | needs: check 57 | runs-on: ubuntu-latest 58 | 59 | strategy: 60 | matrix: 61 | filename: 62 | # Filenames of the shared configurations to apply to the repository in addition to the local configuration. 63 | # https://github.com/arduino/tooling-project-assets/blob/main/workflow-templates/assets/sync-labels 64 | - universal.yml 65 | 66 | steps: 67 | - name: Download 68 | uses: carlosperate/download-file-action@v2 69 | with: 70 | file-url: https://raw.githubusercontent.com/arduino/tooling-project-assets/main/workflow-templates/assets/sync-labels/${{ matrix.filename }} 71 | 72 | - name: Pass configuration files to next job via workflow artifact 73 | uses: actions/upload-artifact@v6 74 | with: 75 | path: | 76 | *.yaml 77 | *.yml 78 | if-no-files-found: error 79 | name: ${{ env.CONFIGURATIONS_ARTIFACT }} 80 | 81 | sync: 82 | needs: download 83 | runs-on: ubuntu-latest 84 | 85 | steps: 86 | - name: Set environment variables 87 | run: | 88 | # See: https://docs.github.com/en/actions/reference/workflow-commands-for-github-actions#setting-an-environment-variable 89 | echo "MERGED_CONFIGURATION_PATH=${{ runner.temp }}/labels.yml" >> "$GITHUB_ENV" 90 | 91 | - name: Determine whether to dry run 92 | id: dry-run 93 | if: > 94 | github.event_name == 'pull_request' || 95 | ( 96 | ( 97 | github.event_name == 'push' || 98 | github.event_name == 'workflow_dispatch' 99 | ) && 100 | github.ref != format('refs/heads/{0}', github.event.repository.default_branch) 101 | ) 102 | run: | 103 | # Use of this flag in the github-label-sync command will cause it to only check the validity of the 104 | # configuration. 105 | echo "::set-output name=flag::--dry-run" 106 | 107 | - name: Checkout repository 108 | uses: actions/checkout@v6 109 | 110 | - name: Download configuration files artifact 111 | uses: actions/download-artifact@v7 112 | with: 113 | name: ${{ env.CONFIGURATIONS_ARTIFACT }} 114 | path: ${{ env.CONFIGURATIONS_FOLDER }} 115 | 116 | - name: Remove unneeded artifact 117 | uses: geekyeggo/delete-artifact@v5 118 | with: 119 | name: ${{ env.CONFIGURATIONS_ARTIFACT }} 120 | 121 | - name: Merge label configuration files 122 | run: | 123 | # Merge all configuration files 124 | shopt -s extglob 125 | cat "${{ env.CONFIGURATIONS_FOLDER }}"/*.@(yml|yaml) > "${{ env.MERGED_CONFIGURATION_PATH }}" 126 | 127 | - name: Install github-label-sync 128 | run: sudo npm install --global github-label-sync 129 | 130 | - name: Sync labels 131 | env: 132 | GITHUB_ACCESS_TOKEN: ${{ secrets.GITHUB_TOKEN }} 133 | run: | 134 | # See: https://github.com/Financial-Times/github-label-sync 135 | github-label-sync \ 136 | --labels "${{ env.MERGED_CONFIGURATION_PATH }}" \ 137 | ${{ steps.dry-run.outputs.flag }} \ 138 | ${{ github.repository }} 139 | -------------------------------------------------------------------------------- /extras/D11-Firmware/src/Encoder/examples/SpeedTest/SpeedTest.pde: -------------------------------------------------------------------------------- 1 | /* Encoder Library - SpeedTest - for measuring maximum Encoder speed 2 | * http://www.pjrc.com/teensy/td_libs_Encoder.html 3 | * 4 | * This example code is in the public domain. 5 | */ 6 | 7 | 8 | // This SpeedTest example provides a simple way to verify how much 9 | // CPU time Encoder is consuming. Connect a DC voltmeter to the 10 | // output pin and measure the voltage while the encoder is stopped 11 | // or running at a very slow speed. Even though the pin is rapidly 12 | // pulsing, a DC voltmeter will show the average voltage. Due to 13 | // software timing, it will read a number much less than a steady 14 | // logic high, but this number will give you a baseline reading 15 | // for output with minimal interrupt overhead. Then increase the 16 | // encoder speed. The voltage will decrease as the processor spends 17 | // more time in Encoder's interrupt routines counting the pulses 18 | // and less time pulsing the output pin. When the voltage is 19 | // close to zero and will not decrease any farther, you have reached 20 | // the absolute speed limit. Or, if using a mechanical system where 21 | // you reach a speed limit imposed by your motors or other hardware, 22 | // the amount this voltage has decreased, compared to the baseline, 23 | // should give you a good approximation of the portion of available 24 | // CPU time Encoder is consuming at your maximum speed. 25 | 26 | // Encoder requires low latency interrupt response. Available CPU 27 | // time does NOT necessarily prove or guarantee correct performance. 28 | // If another library, like NewSoftSerial, is disabling interrupts 29 | // for lengthy periods of time, Encoder can be prevented from 30 | // properly counting the intput signals while interrupt are disabled. 31 | 32 | 33 | // This optional setting causes Encoder to use more optimized code, 34 | // but the downside is a conflict if any other part of your sketch 35 | // or any other library you're using requires attachInterrupt(). 36 | // It must be defined before Encoder.h is included. 37 | //#define ENCODER_OPTIMIZE_INTERRUPTS 38 | 39 | #include 40 | #include "pins_arduino.h" 41 | 42 | // Change these two numbers to the pins connected to your encoder 43 | // or shift register circuit which emulates a quadrature encoder 44 | // case 1: both pins are interrupts 45 | // case 2: only first pin used as interrupt 46 | Encoder myEnc(5, 6); 47 | 48 | // Connect a DC voltmeter to this pin. 49 | const int outputPin = 12; 50 | 51 | /* This simple circuit, using a Dual Flip-Flop chip, can emulate 52 | quadrature encoder signals. The clock can come from a fancy 53 | function generator or a cheap 555 timer chip. The clock 54 | frequency can be measured with another board running FreqCount 55 | http://www.pjrc.com/teensy/td_libs_FreqCount.html 56 | 57 | +5V 58 | | Quadrature Encoder Signal Emulator 59 | Clock | 60 | Input o----*-------------------------- ---------------------------o Output1 61 | | |14 | | 62 | | _______|_______ | | _______________ 63 | | | CD4013 | | | | CD4013 | 64 | | 5 | | 1 | | 9 | | 13 65 | ---------| D Q |-----|----*----| D Q |------o Output2 66 | | | | | | | | 67 | | | 3 | | | 11 | | 68 | | ----|> Clk | ---------|> Clk | 69 | | | | | | 70 | | 6 | | 8 | | 71 | | ----| S | ----| S | 72 | | | | | | | | 73 | | | 4 | _ | 2 | 10 | _ | 12 74 | | *----| R Q |--- *----| R Q |---- 75 | | | | | | | | | 76 | | | |_______________| | |_______________| | 77 | | | | | | 78 | | | | 7 | | 79 | | | | | | 80 | -------------------------------------------------------------- 81 | | | | 82 | | | | 83 | ----- ----- ----- 84 | --- --- --- 85 | - - - 86 | */ 87 | 88 | 89 | void setup() { 90 | pinMode(outputPin, OUTPUT); 91 | } 92 | 93 | #if defined(__AVR__) || defined(TEENSYDUINO) 94 | #define REGTYPE unsigned char 95 | #else 96 | #define REGTYPE unsigned long 97 | #endif 98 | 99 | void loop() { 100 | volatile int count = 0; 101 | volatile REGTYPE *reg = portOutputRegister(digitalPinToPort(outputPin)); 102 | REGTYPE mask = digitalPinToBitMask(outputPin); 103 | 104 | while (1) { 105 | myEnc.read(); // Read the encoder while interrupts are enabled. 106 | noInterrupts(); 107 | *reg |= mask; // Pulse the pin high, while interrupts are disabled. 108 | count = count + 1; 109 | *reg &= ~mask; 110 | interrupts(); 111 | } 112 | } 113 | 114 | -------------------------------------------------------------------------------- /docs/api.md: -------------------------------------------------------------------------------- 1 | # ArduinoMotorCarrier library 2 | 3 | ## Methods 4 | 5 | ### `BATTERY` 6 | 7 | Returns information about the battery connected to the carrier. 8 | 9 | #### Syntax 10 | 11 | ``` 12 | battery.getRaw() 13 | battery.getConverted() 14 | battery.getFiltered() 15 | ``` 16 | 17 | #### Returns 18 | 19 | * _getRaw()_: returns the raw ADC read from the battery as an integer. 20 | * _getConverted()_: returns the battery voltage converted to volts as a floating point. 21 | * _getFiltered()_: returns the battery voltage converted to volts and filtered in the last 10 seconds. 22 | 23 | 24 | #### Example 25 | 26 | ``` 27 | 28 | #include 29 | 30 | //Variable to store the battery voltage 31 | float batteryVoltage; 32 | ... 33 | void loop() { 34 | batteryVoltage = battery.getRaw()/236.0; //236 for Nano, 77 for MKR. 35 | Serial.print("Battery voltage: "); 36 | Serial.print(batteryVoltage,3); 37 | Serial.print("V, Raw "); 38 | Serial.println(battery.getRaw()); 39 | delay(5000); //wait for a few seconds 40 | } 41 | ``` 42 | 43 | ### `SERVO` 44 | 45 | Represent the servomotors available on the MotorCarrier. There are 4 headers for servomotors. 46 | 47 | #### Syntax 48 | 49 | ``` 50 | servo1.setAngle(int) 51 | servo2.setAngle(int) 52 | servo3.setAngle(int) 53 | servo4.setAngle(int) 54 | ``` 55 | 56 | #### Function 57 | 58 | * _setAngle(int)_: Set the rotation angle (from 0 to 180) 59 | 60 | 61 | ### `MOTOR` 62 | 63 | There is 4 objects pointing to each possible motor: M1, M2, M3 and M4. 64 | 65 | #### Syntax 66 | 67 | ```` 68 | M1.setDuty(int) 69 | M2.setDuty(int) 70 | M3.setDuty(int) 71 | M4.setDuty(int) 72 | ```` 73 | 74 | #### Functions 75 | 76 | * _setDuty(int)_: Set the duty cycle of the dc motor (from -100 to +100) , 0 means stop. 77 | 78 | 79 | ### `ENCODER` 80 | 81 | Represents the 2 quadrature encoders embedded in the carrier board. The two objects are encoder1 and encoder2. 82 | 83 | #### Syntax 84 | 85 | ```` 86 | encoder1.getRawCount() 87 | encoder2.resetCounter(); 88 | ```` 89 | 90 | #### Functions 91 | 92 | * _getRawCount()_: Returns the number of counts from start as an integer. 93 | * _resetCounter(int)_: Resets counter to a certain value. 94 | 95 | 96 | ### `CONTROLLER` 97 | 98 | Represents the motor shield controller and exposes some high level functions. It also configures the battery charger (only on the NanoMotorCarrier) to start charging the battery. 99 | 100 | #### Syntax 101 | 102 | ``` 103 | controller.getFWVersion() 104 | controller.reboot() 105 | ``` 106 | ### `PID` 107 | 108 | Allow setting Motor1 or Motor2 to a specific speed or position. There are two PID virtual objects in the controller: pid1 and pid2. pid1 acts on M1 and encoder1. pid2 acts on M2 and encoder2. It is advisable to control the motors using these functions. 109 | 110 | #### Syntax 111 | 112 | ``` 113 | pid1.setGains(float P, float I, float D) 114 | ``` 115 | 116 | #### Functions 117 | 118 | * _setGains(float P, float I, float D)_: Set PID gains. 119 | * _resetGains()_: Reset PID gains to factory default settings. 120 | * _setControlMode(cl_control)_: Set control mode to either `CL_VELOCITY` or `CL_POSITION`. 121 | * _setSetpoint(cl_mode, int target)_: Set a specific velocity or position in one of the motors. Define cl_mode as `TARGET_POSITION` or `TARGET_VELOCITY` and the desired value in target. 122 | 123 | #### Example 124 | 125 | Example for PID position control. 126 | 127 | ``` 128 | #include 129 | #define INTERRUPT_PIN 6 130 | 131 | //Variable to change the motor speed and direction 132 | static int duty = 0; 133 | 134 | int target; 135 | float P; 136 | float I; 137 | float D; 138 | 139 | void setup() 140 | { 141 | //Serial port initialization 142 | Serial.begin(115200); 143 | while (!Serial); 144 | 145 | //Establishing the communication with the motor shield 146 | if (controller.begin()) 147 | { 148 | Serial.print("MKR Motor Shield connected, firmware version "); 149 | Serial.println(controller.getFWVersion()); 150 | } 151 | else 152 | { 153 | Serial.println("Couldn't connect! Is the red led blinking? You may need to update the firmware with FWUpdater sketch"); 154 | while (1); 155 | } 156 | 157 | // Reboot the motor controller; brings every value back to default 158 | Serial.println("reboot"); 159 | controller.reboot(); 160 | delay(500); 161 | 162 | int dutyInit = 0; 163 | M1.setDuty(dutyInit); 164 | M2.setDuty(dutyInit); 165 | M3.setDuty(dutyInit); 166 | M4.setDuty(dutyInit); 167 | Serial.print("Duty: "); 168 | Serial.println(dutyInit); 169 | 170 | P = 0.07f;//0.07 //0.2 171 | I = 0.0f; 172 | D = 0.007f; 173 | 174 | /************* PID 1 ***********************/ 175 | 176 | pid1.setControlMode(CL_POSITION); 177 | 178 | //pid1.resetGains(); 179 | pid1.setGains(P, I, D); //Proportional(change) Integral(change) Derivative 180 | Serial.print("P Gain: "); 181 | Serial.println((float)pid1.getPgain()); 182 | Serial.print("I Gain: "); 183 | Serial.println((float)pid1.getIgain()); 184 | Serial.print("D Gain: "); 185 | Serial.println((float)pid1.getDgain(), 7); 186 | Serial.println(""); 187 | 188 | encoder1.resetCounter(0); 189 | Serial.print("encoder1: "); 190 | Serial.println(encoder1.getRawCount()); 191 | target = 5000; 192 | pid1.setSetpoint(TARGET_POSITION, target); 193 | } 194 | 195 | void loop() { 196 | 197 | Serial.print("encoder1: "); 198 | Serial.print(encoder1.getRawCount()); 199 | Serial.print(" target: "); 200 | Serial.println(target); 201 | if (encoder1.getRawCount() == target) { 202 | target += 1000; 203 | Serial.print("Target reached: Setting new target.."); 204 | pid1.setSetpoint(TARGET_POSITION, target); 205 | //delay(5000); 206 | } 207 | 208 | delay(50); 209 | } 210 | ``` 211 | -------------------------------------------------------------------------------- /src/src/libfixmath/int64.h: -------------------------------------------------------------------------------- 1 | #ifndef __libfixmath_int64_h__ 2 | #define __libfixmath_int64_h__ 3 | 4 | #ifdef __cplusplus 5 | extern "C" 6 | { 7 | #endif 8 | 9 | #ifndef FIXMATH_NO_64BIT 10 | static inline int64_t int64_const(int32_t hi, uint32_t lo) { return (((int64_t)hi << 32) | lo); } 11 | static inline int64_t int64_from_int32(int32_t x) { return (int64_t)x; } 12 | static inline int32_t int64_hi(int64_t x) { return (x >> 32); } 13 | static inline uint32_t int64_lo(int64_t x) { return (x & ((1ULL << 32) - 1)); } 14 | 15 | static inline int64_t int64_add(int64_t x, int64_t y) { return (x + y); } 16 | static inline int64_t int64_neg(int64_t x) { return (-x); } 17 | static inline int64_t int64_sub(int64_t x, int64_t y) { return (x - y); } 18 | static inline int64_t int64_shift(int64_t x, int8_t y) { return (y < 0 ? (x >> -y) : (x << y)); } 19 | 20 | static inline int64_t int64_mul_i32_i32(int32_t x, int32_t y) { return (x * y); } 21 | static inline int64_t int64_mul_i64_i32(int64_t x, int32_t y) { return (x * y); } 22 | 23 | static inline int64_t int64_div_i64_i32(int64_t x, int32_t y) { return (x / y); } 24 | 25 | static inline int int64_cmp_eq(int64_t x, int64_t y) { return (x == y); } 26 | static inline int int64_cmp_ne(int64_t x, int64_t y) { return (x != y); } 27 | static inline int int64_cmp_gt(int64_t x, int64_t y) { return (x > y); } 28 | static inline int int64_cmp_ge(int64_t x, int64_t y) { return (x >= y); } 29 | static inline int int64_cmp_lt(int64_t x, int64_t y) { return (x < y); } 30 | static inline int int64_cmp_le(int64_t x, int64_t y) { return (x <= y); } 31 | #else 32 | 33 | typedef struct { 34 | int32_t hi; 35 | uint32_t lo; 36 | } __int64_t; 37 | 38 | static inline __int64_t int64_const(int32_t hi, uint32_t lo) { return (__int64_t){ hi, lo }; } 39 | static inline __int64_t int64_from_int32(int32_t x) { return (__int64_t){ (x < 0 ? -1 : 0), x }; } 40 | static inline int32_t int64_hi(__int64_t x) { return x.hi; } 41 | static inline uint32_t int64_lo(__int64_t x) { return x.lo; } 42 | 43 | static inline int int64_cmp_eq(__int64_t x, __int64_t y) { return ((x.hi == y.hi) && (x.lo == y.lo)); } 44 | static inline int int64_cmp_ne(__int64_t x, __int64_t y) { return ((x.hi != y.hi) || (x.lo != y.lo)); } 45 | static inline int int64_cmp_gt(__int64_t x, __int64_t y) { return ((x.hi > y.hi) || ((x.hi == y.hi) && (x.lo > y.lo))); } 46 | static inline int int64_cmp_ge(__int64_t x, __int64_t y) { return ((x.hi > y.hi) || ((x.hi == y.hi) && (x.lo >= y.lo))); } 47 | static inline int int64_cmp_lt(__int64_t x, __int64_t y) { return ((x.hi < y.hi) || ((x.hi == y.hi) && (x.lo < y.lo))); } 48 | static inline int int64_cmp_le(__int64_t x, __int64_t y) { return ((x.hi < y.hi) || ((x.hi == y.hi) && (x.lo <= y.lo))); } 49 | 50 | static inline __int64_t int64_add(__int64_t x, __int64_t y) { 51 | __int64_t ret; 52 | ret.hi = x.hi + y.hi; 53 | ret.lo = x.lo + y.lo; 54 | if((ret.lo < x.lo) || (ret.hi < y.hi)) 55 | ret.hi++; 56 | return ret; 57 | } 58 | 59 | static inline __int64_t int64_neg(__int64_t x) { 60 | __int64_t ret; 61 | ret.hi = ~x.hi; 62 | ret.lo = ~x.lo + 1; 63 | if(ret.lo == 0) 64 | ret.hi++; 65 | return ret; 66 | } 67 | 68 | static inline __int64_t int64_sub(__int64_t x, __int64_t y) { 69 | return int64_add(x, int64_neg(y)); 70 | } 71 | 72 | static inline __int64_t int64_shift(__int64_t x, int8_t y) { 73 | __int64_t ret; 74 | if(y > 0) { 75 | if(y >= 32) 76 | return (__int64_t){ 0, 0 }; 77 | ret.hi = (x.hi << y) | (x.lo >> (32 - y)); 78 | ret.lo = (x.lo << y); 79 | } else { 80 | y = -y; 81 | if(y >= 32) 82 | return (__int64_t){ 0, 0 }; 83 | ret.lo = (x.lo >> y) | (x.hi << (32 - y)); 84 | ret.hi = (x.hi >> y); 85 | } 86 | return ret; 87 | } 88 | 89 | static inline __int64_t int64_mul_i32_i32(int32_t x, int32_t y) { 90 | int16_t hi[2] = { (x >> 16), (y >> 16) }; 91 | uint16_t lo[2] = { (x & 0xFFFF), (y & 0xFFFF) }; 92 | 93 | int32_t r_hi = hi[0] * hi[1]; 94 | int32_t r_md = (hi[0] * lo[1]) + (hi[1] * lo[0]); 95 | uint32_t r_lo = lo[0] * lo[1]; 96 | 97 | r_hi += (r_md >> 16); 98 | r_lo += (r_md << 16); 99 | 100 | return (__int64_t){ r_hi, r_lo }; 101 | } 102 | 103 | static inline __int64_t int64_mul_i64_i32(__int64_t x, int32_t y) { 104 | int neg = ((x.hi ^ y) < 0); 105 | if(x.hi < 0) 106 | x = int64_neg(x); 107 | if(y < 0) 108 | y = -y; 109 | 110 | uint32_t _x[4] = { (x.hi >> 16), (x.hi & 0xFFFF), (x.lo >> 16), (x.lo & 0xFFFF) }; 111 | uint32_t _y[2] = { (y >> 16), (y & 0xFFFF) }; 112 | 113 | uint32_t r[4]; 114 | r[0] = (_x[0] * _y[0]); 115 | r[1] = (_x[1] * _y[0]) + (_x[0] * _y[1]); 116 | r[2] = (_x[1] * _y[1]) + (_x[2] * _y[0]); 117 | r[3] = (_x[2] * _y[0]) + (_x[1] * _y[1]); 118 | 119 | __int64_t ret; 120 | ret.lo = r[0] + (r[1] << 16); 121 | ret.hi = (r[3] << 16) + r[2] + (r[1] >> 16); 122 | return (neg ? int64_neg(ret) : ret); 123 | } 124 | 125 | static inline __int64_t int64_div_i64_i32(__int64_t x, int32_t y) { 126 | int neg = ((x.hi ^ y) < 0); 127 | if(x.hi < 0) 128 | x = int64_neg(x); 129 | if(y < 0) 130 | y = -y; 131 | 132 | __int64_t ret = { (x.hi / y) , (x.lo / y) }; 133 | x.hi = x.hi % y; 134 | x.lo = x.lo % y; 135 | 136 | __int64_t _y = int64_from_int32(y); 137 | 138 | __int64_t i; 139 | for(i = int64_from_int32(1); int64_cmp_lt(_y, x); _y = int64_shift(_y, 1), i = int64_shift(i, 1)); 140 | 141 | while(x.hi) { 142 | _y = int64_shift(_y, -1); 143 | i = int64_shift(i, -1); 144 | if(int64_cmp_ge(x, _y)) { 145 | x = int64_sub(x, _y); 146 | ret = int64_add(ret, i); 147 | } 148 | } 149 | 150 | ret = int64_add(ret, int64_from_int32(x.lo / y)); 151 | return (neg ? int64_neg(ret) : ret); 152 | } 153 | 154 | #define int64_t __int64_t 155 | 156 | #endif 157 | 158 | #ifdef __cplusplus 159 | } 160 | #endif 161 | 162 | #endif 163 | -------------------------------------------------------------------------------- /src/src/libfixmath/fix16_exp.c: -------------------------------------------------------------------------------- 1 | #include "fix16.h" 2 | #include 3 | 4 | #ifndef FIXMATH_NO_CACHE 5 | static fix16_t _fix16_exp_cache_index[4096] = { 0 }; 6 | static fix16_t _fix16_exp_cache_value[4096] = { 0 }; 7 | #endif 8 | 9 | 10 | 11 | fix16_t fix16_exp(fix16_t inValue) { 12 | if(inValue == 0 ) return fix16_one; 13 | if(inValue == fix16_one) return fix16_e; 14 | if(inValue >= 681391 ) return fix16_maximum; 15 | if(inValue <= -772243 ) return 0; 16 | 17 | #ifndef FIXMATH_NO_CACHE 18 | fix16_t tempIndex = (inValue ^ (inValue >> 16)); 19 | tempIndex = (inValue ^ (inValue >> 4)) & 0x0FFF; 20 | if(_fix16_exp_cache_index[tempIndex] == inValue) 21 | return _fix16_exp_cache_value[tempIndex]; 22 | #endif 23 | 24 | /* The algorithm is based on the power series for exp(x): 25 | * http://en.wikipedia.org/wiki/Exponential_function#Formal_definition 26 | * 27 | * From term n, we get term n+1 by multiplying with x/n. 28 | * When the sum term drops to zero, we can stop summing. 29 | */ 30 | 31 | // The power-series converges much faster on positive values 32 | // and exp(-x) = 1/exp(x). 33 | bool neg = (inValue < 0); 34 | if (neg) inValue = -inValue; 35 | 36 | fix16_t result = inValue + fix16_one; 37 | fix16_t term = inValue; 38 | 39 | uint_fast8_t i; 40 | for (i = 2; i < 30; i++) 41 | { 42 | term = fix16_mul(term, fix16_div(inValue, fix16_from_int(i))); 43 | result += term; 44 | 45 | if ((term < 500) && ((i > 15) || (term < 20))) 46 | break; 47 | } 48 | 49 | if (neg) result = fix16_div(fix16_one, result); 50 | 51 | #ifndef FIXMATH_NO_CACHE 52 | _fix16_exp_cache_index[tempIndex] = inValue; 53 | _fix16_exp_cache_value[tempIndex] = result; 54 | #endif 55 | 56 | return result; 57 | } 58 | 59 | 60 | 61 | fix16_t fix16_log(fix16_t inValue) 62 | { 63 | fix16_t guess = fix16_from_int(2); 64 | fix16_t delta; 65 | int scaling = 0; 66 | int count = 0; 67 | 68 | if (inValue <= 0) 69 | return fix16_minimum; 70 | 71 | // Bring the value to the most accurate range (1 < x < 100) 72 | const fix16_t e_to_fourth = 3578144; 73 | while (inValue > fix16_from_int(100)) 74 | { 75 | inValue = fix16_div(inValue, e_to_fourth); 76 | scaling += 4; 77 | } 78 | 79 | while (inValue < fix16_one) 80 | { 81 | inValue = fix16_mul(inValue, e_to_fourth); 82 | scaling -= 4; 83 | } 84 | 85 | do 86 | { 87 | // Solving e(x) = y using Newton's method 88 | // f(x) = e(x) - y 89 | // f'(x) = e(x) 90 | fix16_t e = fix16_exp(guess); 91 | delta = fix16_div(inValue - e, e); 92 | 93 | // It's unlikely that logarithm is very large, so avoid overshooting. 94 | if (delta > fix16_from_int(3)) 95 | delta = fix16_from_int(3); 96 | 97 | guess += delta; 98 | } while ((count++ < 10) 99 | && ((delta > 1) || (delta < -1))); 100 | 101 | return guess + fix16_from_int(scaling); 102 | } 103 | 104 | 105 | 106 | static inline fix16_t fix16_rs(fix16_t x) 107 | { 108 | #ifdef FIXMATH_NO_ROUNDING 109 | return (x >> 1); 110 | #else 111 | fix16_t y = (x >> 1) + (x & 1); 112 | return y; 113 | #endif 114 | } 115 | 116 | /** 117 | * This assumes that the input value is >= 1. 118 | * 119 | * Note that this is only ever called with inValue >= 1 (because it has a wrapper to check. 120 | * As such, the result is always less than the input. 121 | */ 122 | static fix16_t fix16__log2_inner(fix16_t x) 123 | { 124 | fix16_t result = 0; 125 | 126 | while(x >= fix16_from_int(2)) 127 | { 128 | result++; 129 | x = fix16_rs(x); 130 | } 131 | 132 | if(x == 0) return (result << 16); 133 | 134 | uint_fast8_t i; 135 | for(i = 16; i > 0; i--) 136 | { 137 | x = fix16_mul(x, x); 138 | result <<= 1; 139 | if(x >= fix16_from_int(2)) 140 | { 141 | result |= 1; 142 | x = fix16_rs(x); 143 | } 144 | } 145 | #ifndef FIXMATH_NO_ROUNDING 146 | x = fix16_mul(x, x); 147 | if(x >= fix16_from_int(2)) result++; 148 | #endif 149 | 150 | return result; 151 | } 152 | 153 | 154 | 155 | /** 156 | * calculates the log base 2 of input. 157 | * Note that negative inputs are invalid! (will return fix16_overflow, since there are no exceptions) 158 | * 159 | * i.e. 2 to the power output = input. 160 | * It's equivalent to the log or ln functions, except it uses base 2 instead of base 10 or base e. 161 | * This is useful as binary things like this are easy for binary devices, like modern microprocessros, to calculate. 162 | * 163 | * This can be used as a helper function to calculate powers with non-integer powers and/or bases. 164 | */ 165 | fix16_t fix16_log2(fix16_t x) 166 | { 167 | // Note that a negative x gives a non-real result. 168 | // If x == 0, the limit of log2(x) as x -> 0 = -infinity. 169 | // log2(-ve) gives a complex result. 170 | if (x <= 0) return fix16_overflow; 171 | 172 | // If the input is less than one, the result is -log2(1.0 / in) 173 | if (x < fix16_one) 174 | { 175 | // Note that the inverse of this would overflow. 176 | // This is the exact answer for log2(1.0 / 65536) 177 | if (x == 1) return fix16_from_int(-16); 178 | 179 | fix16_t inverse = fix16_div(fix16_one, x); 180 | return -fix16__log2_inner(inverse); 181 | } 182 | 183 | // If input >= 1, just proceed as normal. 184 | // Note that x == fix16_one is a special case, where the answer is 0. 185 | return fix16__log2_inner(x); 186 | } 187 | 188 | /** 189 | * This is a wrapper for fix16_log2 which implements saturation arithmetic. 190 | */ 191 | fix16_t fix16_slog2(fix16_t x) 192 | { 193 | fix16_t retval = fix16_log2(x); 194 | // The only overflow possible is when the input is negative. 195 | if(retval == fix16_overflow) 196 | return fix16_minimum; 197 | return retval; 198 | } 199 | -------------------------------------------------------------------------------- /src/src/libfixmath/fix16_trig.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include "fix16.h" 3 | 4 | #if defined(FIXMATH_SIN_LUT) 5 | #include "fix16_trig_sin_lut.h" 6 | #elif !defined(FIXMATH_NO_CACHE) 7 | static fix16_t _fix16_sin_cache_index[4096] = { 0 }; 8 | static fix16_t _fix16_sin_cache_value[4096] = { 0 }; 9 | #endif 10 | 11 | #ifndef FIXMATH_NO_CACHE 12 | static fix16_t _fix16_atan_cache_index[2][4096] = { { 0 }, { 0 } }; 13 | static fix16_t _fix16_atan_cache_value[4096] = { 0 }; 14 | #endif 15 | 16 | 17 | fix16_t fix16_sin_parabola(fix16_t inAngle) 18 | { 19 | fix16_t abs_inAngle, abs_retval, retval; 20 | fix16_t mask; 21 | 22 | /* Absolute function */ 23 | mask = (inAngle >> (sizeof(fix16_t)*CHAR_BIT-1)); 24 | abs_inAngle = (inAngle + mask) ^ mask; 25 | 26 | /* On 0->PI, sin looks like x² that is : 27 | - centered on PI/2, 28 | - equals 1 on PI/2, 29 | - equals 0 on 0 and PI 30 | that means : 4/PI * x - 4/PI² * x² 31 | Use abs(x) to handle (-PI) -> 0 zone. 32 | */ 33 | retval = fix16_mul(FOUR_DIV_PI, inAngle) + fix16_mul( fix16_mul(_FOUR_DIV_PI2, inAngle), abs_inAngle ); 34 | /* At this point, retval equals sin(inAngle) on important points ( -PI, -PI/2, 0, PI/2, PI), 35 | but is not very precise between these points 36 | */ 37 | #ifndef FIXMATH_FAST_SIN 38 | /* Absolute value of retval */ 39 | mask = (retval >> (sizeof(fix16_t)*CHAR_BIT-1)); 40 | abs_retval = (retval + mask) ^ mask; 41 | /* So improve its precision by adding some x^4 component to retval */ 42 | retval += fix16_mul(X4_CORRECTION_COMPONENT, fix16_mul(retval, abs_retval) - retval ); 43 | #endif 44 | return retval; 45 | } 46 | 47 | fix16_t fix16_sin(fix16_t inAngle) 48 | { 49 | fix16_t tempAngle = inAngle % (fix16_pi << 1); 50 | 51 | #ifdef FIXMATH_SIN_LUT 52 | if(tempAngle < 0) 53 | tempAngle += (fix16_pi << 1); 54 | 55 | fix16_t tempOut; 56 | if(tempAngle >= fix16_pi) { 57 | tempAngle -= fix16_pi; 58 | if(tempAngle >= (fix16_pi >> 1)) 59 | tempAngle = fix16_pi - tempAngle; 60 | tempOut = -(tempAngle >= _fix16_sin_lut_count ? fix16_one : _fix16_sin_lut[tempAngle]); 61 | } else { 62 | if(tempAngle >= (fix16_pi >> 1)) 63 | tempAngle = fix16_pi - tempAngle; 64 | tempOut = (tempAngle >= _fix16_sin_lut_count ? fix16_one : _fix16_sin_lut[tempAngle]); 65 | } 66 | #else 67 | if(tempAngle > fix16_pi) 68 | tempAngle -= (fix16_pi << 1); 69 | else if(tempAngle < -fix16_pi) 70 | tempAngle += (fix16_pi << 1); 71 | 72 | #ifndef FIXMATH_NO_CACHE 73 | fix16_t tempIndex = ((inAngle >> 5) & 0x00000FFF); 74 | if(_fix16_sin_cache_index[tempIndex] == inAngle) 75 | return _fix16_sin_cache_value[tempIndex]; 76 | #endif 77 | 78 | fix16_t tempAngleSq = fix16_mul(tempAngle, tempAngle); 79 | 80 | #ifndef FIXMATH_FAST_SIN // Most accurate version, accurate to ~2.1% 81 | fix16_t tempOut = tempAngle; 82 | tempAngle = fix16_mul(tempAngle, tempAngleSq); 83 | tempOut -= (tempAngle / 6); 84 | tempAngle = fix16_mul(tempAngle, tempAngleSq); 85 | tempOut += (tempAngle / 120); 86 | tempAngle = fix16_mul(tempAngle, tempAngleSq); 87 | tempOut -= (tempAngle / 5040); 88 | tempAngle = fix16_mul(tempAngle, tempAngleSq); 89 | tempOut += (tempAngle / 362880); 90 | tempAngle = fix16_mul(tempAngle, tempAngleSq); 91 | tempOut -= (tempAngle / 39916800); 92 | #else // Fast implementation, runs at 159% the speed of above 'accurate' version with an slightly lower accuracy of ~2.3% 93 | fix16_t tempOut; 94 | tempOut = fix16_mul(-13, tempAngleSq) + 546; 95 | tempOut = fix16_mul(tempOut, tempAngleSq) - 10923; 96 | tempOut = fix16_mul(tempOut, tempAngleSq) + 65536; 97 | tempOut = fix16_mul(tempOut, tempAngle); 98 | #endif 99 | 100 | #ifndef FIXMATH_NO_CACHE 101 | _fix16_sin_cache_index[tempIndex] = inAngle; 102 | _fix16_sin_cache_value[tempIndex] = tempOut; 103 | #endif 104 | #endif 105 | 106 | return tempOut; 107 | } 108 | 109 | fix16_t fix16_cos(fix16_t inAngle) 110 | { 111 | return fix16_sin(inAngle + (fix16_pi >> 1)); 112 | } 113 | 114 | fix16_t fix16_tan(fix16_t inAngle) 115 | { 116 | return fix16_sdiv(fix16_sin(inAngle), fix16_cos(inAngle)); 117 | } 118 | 119 | fix16_t fix16_asin(fix16_t x) 120 | { 121 | if((x > fix16_one) 122 | || (x < -fix16_one)) 123 | return 0; 124 | 125 | fix16_t out; 126 | out = (fix16_one - fix16_mul(x, x)); 127 | out = fix16_div(x, fix16_sqrt(out)); 128 | out = fix16_atan(out); 129 | return out; 130 | } 131 | 132 | fix16_t fix16_acos(fix16_t x) 133 | { 134 | return ((fix16_pi >> 1) - fix16_asin(x)); 135 | } 136 | 137 | fix16_t fix16_atan2(fix16_t inY , fix16_t inX) 138 | { 139 | fix16_t abs_inY, mask, angle, r, r_3; 140 | 141 | #ifndef FIXMATH_NO_CACHE 142 | uintptr_t hash = (inX ^ inY); 143 | hash ^= hash >> 20; 144 | hash &= 0x0FFF; 145 | if((_fix16_atan_cache_index[0][hash] == inX) && (_fix16_atan_cache_index[1][hash] == inY)) 146 | return _fix16_atan_cache_value[hash]; 147 | #endif 148 | 149 | /* Absolute inY */ 150 | mask = (inY >> (sizeof(fix16_t)*CHAR_BIT-1)); 151 | abs_inY = (inY + mask) ^ mask; 152 | 153 | if (inX >= 0) 154 | { 155 | r = fix16_div( (inX - abs_inY), (inX + abs_inY)); 156 | r_3 = fix16_mul(fix16_mul(r, r),r); 157 | angle = fix16_mul(0x00003240 , r_3) - fix16_mul(0x0000FB50,r) + PI_DIV_4; 158 | } else { 159 | r = fix16_div( (inX + abs_inY), (abs_inY - inX)); 160 | r_3 = fix16_mul(fix16_mul(r, r),r); 161 | angle = fix16_mul(0x00003240 , r_3) 162 | - fix16_mul(0x0000FB50,r) 163 | + THREE_PI_DIV_4; 164 | } 165 | if (inY < 0) 166 | { 167 | angle = -angle; 168 | } 169 | 170 | #ifndef FIXMATH_NO_CACHE 171 | _fix16_atan_cache_index[0][hash] = inX; 172 | _fix16_atan_cache_index[1][hash] = inY; 173 | _fix16_atan_cache_value[hash] = angle; 174 | #endif 175 | 176 | return angle; 177 | } 178 | 179 | fix16_t fix16_atan(fix16_t x) 180 | { 181 | return fix16_atan2(x, fix16_one); 182 | } 183 | -------------------------------------------------------------------------------- /extras/D11-Firmware/D11-Firmware.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #include "EncoderWrapper.h" 3 | #include "Battery.h" 4 | #include "DCMotor.h" 5 | #include "ServoMotor.h" 6 | #include "Common.h" 7 | #include "Events.h" 8 | #include "PID.h" 9 | #include "FreeRAM.h" 10 | 11 | #define Fix16 mn::MFixedPoint::FpF32<8> 12 | 13 | // grab core from https://github.com/arduino/ArduinoCore-samd/tree/mkrmotorcarrier 14 | // compile me with target arduino:samd:mkrmotorshield:bootloader=0kb,pinmap=complete,lto=disabled during development 15 | // compile me with target arduino:samd:mkrmotorshield:bootloader=4kb,pinmap=complete,lto=enabled for release 16 | 17 | const char* FW_VERSION = "0.20"; 18 | 19 | DCMotor* dcmotors[2]; 20 | ServoMotor* servos[4]; 21 | EncoderWrapper* encoders[2]; 22 | PIDWrapper* pid_control[2]; 23 | Battery* battery; 24 | 25 | void led_on() { 26 | digitalWrite(LED_BUILTIN, HIGH); 27 | } 28 | 29 | typedef struct { 30 | Fix16 P; 31 | Fix16 I; 32 | Fix16 D; 33 | } PIDGains; 34 | 35 | union PIDData { 36 | Fix16 txFloat; 37 | uint8_t txArray[4]; 38 | } PIDGain; 39 | 40 | void setup() { 41 | 42 | WDT->CTRL.reg &= ~WDT_CTRL_ENABLE; 43 | while (WDT->STATUS.reg & WDT_STATUS_SYNCBUSY); 44 | 45 | //temp_init(); 46 | battery = new Battery(ADC_BATTERY); 47 | 48 | dcmotors[0] = new DCMotor(MOTOR_1_COUNTER, MOTOR_1_PIN_A, MOTOR_1_PIN_B); 49 | dcmotors[1] = new DCMotor(MOTOR_2_COUNTER, MOTOR_2_PIN_A, MOTOR_2_PIN_B), 50 | 51 | servos[0] = new ServoMotor(PWM_PIN_SERVO_1); 52 | servos[1] = new ServoMotor(PWM_PIN_SERVO_2); 53 | servos[2] = new ServoMotor(PWM_PIN_SERVO_3); 54 | servos[3] = new ServoMotor(PWM_PIN_SERVO_4); 55 | 56 | encoders[0] = new EncoderWrapper(ENCODER_2_PIN_A, ENCODER_2_PIN_B, 1); 57 | encoders[1] = new EncoderWrapper(ENCODER_1_PIN_A, ENCODER_1_PIN_B, 0); 58 | 59 | pid_control[0] = new PIDWrapper(encoders[0]->position, encoders[0]->velocity, dcmotors[0], 0, 10, 100); //10 ms period velo, 100 ms period pos 60 | pid_control[1] = new PIDWrapper(encoders[1]->position, encoders[1]->velocity, dcmotors[1], 1, 10, 100); 61 | 62 | Wire.begin(I2C_ADDRESS); 63 | Wire.onRequest(requestEvent); 64 | Wire.onReceive(receiveEvent); 65 | pinMode(LED_BUILTIN, OUTPUT); 66 | pinMode(IRQ_PIN, OUTPUT); 67 | analogWriteResolution(8); 68 | } 69 | 70 | volatile uint8_t command = 0; 71 | volatile uint8_t target = 0; 72 | volatile uint8_t irq_status = 0; 73 | volatile unsigned long nextTimedEvent = 0; 74 | volatile unsigned long lastMessageReceived = 0; 75 | 76 | bool irq_enabled = true; 77 | 78 | void loop() { 79 | if (command == RESET || ((lastMessageReceived != 0) && (millis() - lastMessageReceived > 10000))) { 80 | reboot(); 81 | } 82 | if (command == PING) { 83 | lastMessageReceived = millis(); 84 | } 85 | executeTimedEvents(); 86 | } 87 | 88 | void receiveEvent(int howMany) { 89 | noInterrupts(); 90 | command = Wire.read(); 91 | 92 | if (command < GET_VERSION) { 93 | // empty buffer 94 | while (Wire.available()) { 95 | Wire.read(); 96 | } 97 | interrupts(); 98 | return; 99 | } 100 | 101 | if (Wire.available()) { 102 | target = Wire.read(); 103 | } else { 104 | interrupts(); 105 | return; 106 | } 107 | int value = 0; 108 | 109 | 110 | if (!Wire.available()) { 111 | interrupts(); 112 | return; 113 | } 114 | 115 | uint8_t buf[12]; 116 | int i = 0; 117 | while (Wire.available() && i < sizeof(buf)) { 118 | buf[i++] = (uint8_t)Wire.read(); 119 | } 120 | 121 | // copies the bytes to int 122 | memcpy(&value, buf, sizeof(value)); 123 | 124 | switch (command) { 125 | case SET_PWM_DUTY_CYCLE_SERVO: 126 | servos[target]->setDuty(value); 127 | break; 128 | case SET_PWM_FREQUENCY_SERVO: 129 | servos[target]->setFrequency(value); 130 | break; 131 | case SET_PWM_DUTY_CYCLE_DC_MOTOR: 132 | ((PIDWrapper*)dcmotors[target]->pid)->stop(); 133 | dcmotors[target]->setDuty(value); 134 | break; 135 | case SET_PWM_FREQUENCY_DC_MOTOR: 136 | dcmotors[target]->setFrequency(value); 137 | break; 138 | case RESET_COUNT_ENCODER: 139 | encoders[target]->resetCounter(value); 140 | break; 141 | case SET_INTERRUPT_ON_COUNT_ENCODER: 142 | encoders[target]->setIrqOnCount(value); 143 | break; 144 | case SET_INTERRUPT_ON_VELOCITY_ENCODER: 145 | encoders[target]->setIrqOnVelocity(value); 146 | break; 147 | case SET_PID_GAIN_CL_MOTOR: 148 | { 149 | Fix16 P = *((Fix16*)&buf[0]); 150 | Fix16 I = *((Fix16*)&buf[4]); 151 | Fix16 D = *((Fix16*)&buf[8]); 152 | pid_control[target]->setGains(P, I, D); 153 | break; 154 | } 155 | case RESET_PID_GAIN_CL_MOTOR: 156 | pid_control[target]->resetGains(); 157 | break; 158 | case SET_CONTROL_MODE_CL_MOTOR: 159 | pid_control[target]->setControlMode((cl_control)value); 160 | break; 161 | case SET_POSITION_SETPOINT_CL_MOTOR: 162 | pid_control[target]->setSetpoint(TARGET_POSITION, Fix16(value * 1.0)); //Change to integer. "value" is a 32 bit data type in this case (int). 163 | break; 164 | case SET_VELOCITY_SETPOINT_CL_MOTOR: 165 | pid_control[target]->setSetpoint(TARGET_VELOCITY, Fix16(value * 1.0)); //Change to integer 166 | break; 167 | case SET_MAX_ACCELERATION_CL_MOTOR: { 168 | pid_control[target]->setMaxAcceleration(Fix16(value * 1.0)); 169 | break; 170 | } 171 | case SET_MAX_VELOCITY_CL_MOTOR: 172 | pid_control[target]->setMaxVelocity(Fix16(value * 1.0)); 173 | break; 174 | case SET_MIN_MAX_DUTY_CYCLE_CL_MOTOR: 175 | pid_control[target]->setLimits(*((int16_t*)&buf[0]), *((int16_t*)&buf[2])); 176 | break; 177 | } 178 | interrupts(); 179 | } 180 | 181 | void requestEvent() { 182 | noInterrupts(); 183 | //deassert IRQ 184 | if (irq_enabled) { 185 | digitalWrite(IRQ_PIN, HIGH); 186 | } 187 | 188 | // Always reply with irq status 189 | Wire.write(irq_status); 190 | 191 | switch (command) { 192 | case GET_VERSION: 193 | getFWVersion(); 194 | break; 195 | case GET_RAW_COUNT_ENCODER: 196 | encoders[target]->getRawCount(); 197 | break; 198 | case GET_OVERFLOW_UNDERFLOW_STATUS_ENCODER: 199 | encoders[target]->getOverflowUnderflow(); 200 | break; 201 | case GET_COUNT_PER_SECOND_ENCODER: 202 | encoders[target]->getCountPerSecond(); 203 | break; 204 | case GET_RAW_ADC_BATTERY: 205 | battery->getRaw(); 206 | break; 207 | case GET_CONVERTED_ADC_BATTERY: 208 | battery->getConverted(); 209 | break; 210 | case GET_FILTERED_ADC_BATTERY: 211 | battery->getFiltered(); 212 | break; 213 | case GET_INTERNAL_TEMP: 214 | getInternalTemperature(); 215 | break; 216 | case CLEAR_IRQ: 217 | Wire.write((int)irq_status); 218 | irq_status = 0; 219 | break; 220 | case GET_FREE_RAM: 221 | Wire.write((int)FreeRam()); 222 | break; 223 | case GET_PID_VAL: 224 | Fix16 gains[3]; 225 | pid_control[target]->getGains((Fix16*)gains); 226 | 227 | PIDGains pidGains; 228 | pidGains.P = gains[0]; 229 | pidGains.I = gains[1]; 230 | pidGains.D = gains[2]; 231 | 232 | Wire.write((uint8_t*)&pidGains, sizeof(pidGains)); 233 | 234 | break; 235 | } 236 | interrupts(); 237 | } 238 | 239 | void requestAttention(int cause) { 240 | irq_status |= (1 << cause); 241 | if (irq_enabled) { 242 | digitalWrite(IRQ_PIN, LOW); 243 | } 244 | } 245 | 246 | void getFWVersion() { 247 | Wire.write(FW_VERSION); 248 | } 249 | 250 | void getInternalTemperature() { 251 | //Wire.write(temp_raw_to_mdeg(temp_read_raw())); 252 | } 253 | 254 | void reboot() { 255 | NVIC_SystemReset(); 256 | } 257 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | GNU LESSER GENERAL PUBLIC LICENSE 2 | Version 3, 29 June 2007 3 | 4 | Copyright (C) 2007 Free Software Foundation, Inc. 5 | Everyone is permitted to copy and distribute verbatim copies 6 | of this license document, but changing it is not allowed. 7 | 8 | 9 | This version of the GNU Lesser General Public License incorporates 10 | the terms and conditions of version 3 of the GNU General Public 11 | License, supplemented by the additional permissions listed below. 12 | 13 | 0. Additional Definitions. 14 | 15 | As used herein, "this License" refers to version 3 of the GNU Lesser 16 | General Public License, and the "GNU GPL" refers to version 3 of the GNU 17 | General Public License. 18 | 19 | "The Library" refers to a covered work governed by this License, 20 | other than an Application or a Combined Work as defined below. 21 | 22 | An "Application" is any work that makes use of an interface provided 23 | by the Library, but which is not otherwise based on the Library. 24 | Defining a subclass of a class defined by the Library is deemed a mode 25 | of using an interface provided by the Library. 26 | 27 | A "Combined Work" is a work produced by combining or linking an 28 | Application with the Library. The particular version of the Library 29 | with which the Combined Work was made is also called the "Linked 30 | Version". 31 | 32 | The "Minimal Corresponding Source" for a Combined Work means the 33 | Corresponding Source for the Combined Work, excluding any source code 34 | for portions of the Combined Work that, considered in isolation, are 35 | based on the Application, and not on the Linked Version. 36 | 37 | The "Corresponding Application Code" for a Combined Work means the 38 | object code and/or source code for the Application, including any data 39 | and utility programs needed for reproducing the Combined Work from the 40 | Application, but excluding the System Libraries of the Combined Work. 41 | 42 | 1. Exception to Section 3 of the GNU GPL. 43 | 44 | You may convey a covered work under sections 3 and 4 of this License 45 | without being bound by section 3 of the GNU GPL. 46 | 47 | 2. Conveying Modified Versions. 48 | 49 | If you modify a copy of the Library, and, in your modifications, a 50 | facility refers to a function or data to be supplied by an Application 51 | that uses the facility (other than as an argument passed when the 52 | facility is invoked), then you may convey a copy of the modified 53 | version: 54 | 55 | a) under this License, provided that you make a good faith effort to 56 | ensure that, in the event an Application does not supply the 57 | function or data, the facility still operates, and performs 58 | whatever part of its purpose remains meaningful, or 59 | 60 | b) under the GNU GPL, with none of the additional permissions of 61 | this License applicable to that copy. 62 | 63 | 3. Object Code Incorporating Material from Library Header Files. 64 | 65 | The object code form of an Application may incorporate material from 66 | a header file that is part of the Library. You may convey such object 67 | code under terms of your choice, provided that, if the incorporated 68 | material is not limited to numerical parameters, data structure 69 | layouts and accessors, or small macros, inline functions and templates 70 | (ten or fewer lines in length), you do both of the following: 71 | 72 | a) Give prominent notice with each copy of the object code that the 73 | Library is used in it and that the Library and its use are 74 | covered by this License. 75 | 76 | b) Accompany the object code with a copy of the GNU GPL and this license 77 | document. 78 | 79 | 4. Combined Works. 80 | 81 | You may convey a Combined Work under terms of your choice that, 82 | taken together, effectively do not restrict modification of the 83 | portions of the Library contained in the Combined Work and reverse 84 | engineering for debugging such modifications, if you also do each of 85 | the following: 86 | 87 | a) Give prominent notice with each copy of the Combined Work that 88 | the Library is used in it and that the Library and its use are 89 | covered by this License. 90 | 91 | b) Accompany the Combined Work with a copy of the GNU GPL and this license 92 | document. 93 | 94 | c) For a Combined Work that displays copyright notices during 95 | execution, include the copyright notice for the Library among 96 | these notices, as well as a reference directing the user to the 97 | copies of the GNU GPL and this license document. 98 | 99 | d) Do one of the following: 100 | 101 | 0) Convey the Minimal Corresponding Source under the terms of this 102 | License, and the Corresponding Application Code in a form 103 | suitable for, and under terms that permit, the user to 104 | recombine or relink the Application with a modified version of 105 | the Linked Version to produce a modified Combined Work, in the 106 | manner specified by section 6 of the GNU GPL for conveying 107 | Corresponding Source. 108 | 109 | 1) Use a suitable shared library mechanism for linking with the 110 | Library. A suitable mechanism is one that (a) uses at run time 111 | a copy of the Library already present on the user's computer 112 | system, and (b) will operate properly with a modified version 113 | of the Library that is interface-compatible with the Linked 114 | Version. 115 | 116 | e) Provide Installation Information, but only if you would otherwise 117 | be required to provide such information under section 6 of the 118 | GNU GPL, and only to the extent that such information is 119 | necessary to install and execute a modified version of the 120 | Combined Work produced by recombining or relinking the 121 | Application with a modified version of the Linked Version. (If 122 | you use option 4d0, the Installation Information must accompany 123 | the Minimal Corresponding Source and Corresponding Application 124 | Code. If you use option 4d1, you must provide the Installation 125 | Information in the manner specified by section 6 of the GNU GPL 126 | for conveying Corresponding Source.) 127 | 128 | 5. Combined Libraries. 129 | 130 | You may place library facilities that are a work based on the 131 | Library side by side in a single library together with other library 132 | facilities that are not Applications and are not covered by this 133 | License, and convey such a combined library under terms of your 134 | choice, if you do both of the following: 135 | 136 | a) Accompany the combined library with a copy of the same work based 137 | on the Library, uncombined with any other library facilities, 138 | conveyed under the terms of this License. 139 | 140 | b) Give prominent notice with the combined library that part of it 141 | is a work based on the Library, and explaining where to find the 142 | accompanying uncombined form of the same work. 143 | 144 | 6. Revised Versions of the GNU Lesser General Public License. 145 | 146 | The Free Software Foundation may publish revised and/or new versions 147 | of the GNU Lesser General Public License from time to time. Such new 148 | versions will be similar in spirit to the present version, but may 149 | differ in detail to address new problems or concerns. 150 | 151 | Each version is given a distinguishing version number. If the 152 | Library as you received it specifies that a certain numbered version 153 | of the GNU Lesser General Public License "or any later version" 154 | applies to it, you have the option of following the terms and 155 | conditions either of that published version or of any later version 156 | published by the Free Software Foundation. If the Library as you 157 | received it does not specify a version number of the GNU Lesser 158 | General Public License, you may choose any version of the GNU Lesser 159 | General Public License ever published by the Free Software Foundation. 160 | 161 | If the Library as you received it specifies that a proxy can decide 162 | whether future versions of the GNU Lesser General Public License shall 163 | apply, that proxy's public statement of acceptance of any version is 164 | permanent authorization for you to choose that version for the 165 | Library. 166 | -------------------------------------------------------------------------------- /extras/D11-Firmware/src/Encoder/utility/interrupt_pins.h: -------------------------------------------------------------------------------- 1 | // interrupt pins for known boards 2 | 3 | // Teensy (and maybe others) define these automatically 4 | #if !defined(CORE_NUM_INTERRUPT) 5 | 6 | // Wiring boards 7 | #if defined(WIRING) 8 | #define CORE_NUM_INTERRUPT NUM_EXTERNAL_INTERRUPTS 9 | #if NUM_EXTERNAL_INTERRUPTS > 0 10 | #define CORE_INT0_PIN EI0 11 | #endif 12 | #if NUM_EXTERNAL_INTERRUPTS > 1 13 | #define CORE_INT1_PIN EI1 14 | #endif 15 | #if NUM_EXTERNAL_INTERRUPTS > 2 16 | #define CORE_INT2_PIN EI2 17 | #endif 18 | #if NUM_EXTERNAL_INTERRUPTS > 3 19 | #define CORE_INT3_PIN EI3 20 | #endif 21 | #if NUM_EXTERNAL_INTERRUPTS > 4 22 | #define CORE_INT4_PIN EI4 23 | #endif 24 | #if NUM_EXTERNAL_INTERRUPTS > 5 25 | #define CORE_INT5_PIN EI5 26 | #endif 27 | #if NUM_EXTERNAL_INTERRUPTS > 6 28 | #define CORE_INT6_PIN EI6 29 | #endif 30 | #if NUM_EXTERNAL_INTERRUPTS > 7 31 | #define CORE_INT7_PIN EI7 32 | #endif 33 | 34 | // Arduino Uno, Duemilanove, Diecimila, LilyPad, Mini, Fio, etc... 35 | #elif defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega8__) 36 | #define CORE_NUM_INTERRUPT 2 37 | #define CORE_INT0_PIN 2 38 | #define CORE_INT1_PIN 3 39 | 40 | // Arduino Mega 41 | #elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) 42 | #define CORE_NUM_INTERRUPT 6 43 | #define CORE_INT0_PIN 2 44 | #define CORE_INT1_PIN 3 45 | #define CORE_INT2_PIN 21 46 | #define CORE_INT3_PIN 20 47 | #define CORE_INT4_PIN 19 48 | #define CORE_INT5_PIN 18 49 | 50 | // Arduino Leonardo (untested) 51 | #elif defined(__AVR_ATmega32U4__) && !defined(CORE_TEENSY) 52 | #define CORE_NUM_INTERRUPT 5 53 | #define CORE_INT0_PIN 3 54 | #define CORE_INT1_PIN 2 55 | #define CORE_INT2_PIN 0 56 | #define CORE_INT3_PIN 1 57 | #define CORE_INT4_PIN 7 58 | 59 | // Sanguino (untested) 60 | #elif defined(__AVR_ATmega644P__) || defined(__AVR_ATmega644__) 61 | #define CORE_NUM_INTERRUPT 3 62 | #define CORE_INT0_PIN 10 63 | #define CORE_INT1_PIN 11 64 | #define CORE_INT2_PIN 2 65 | 66 | // Chipkit Uno32 - attachInterrupt may not support CHANGE option 67 | #elif defined(__PIC32MX__) && defined(_BOARD_UNO_) 68 | #define CORE_NUM_INTERRUPT 5 69 | #define CORE_INT0_PIN 38 70 | #define CORE_INT1_PIN 2 71 | #define CORE_INT2_PIN 7 72 | #define CORE_INT3_PIN 8 73 | #define CORE_INT4_PIN 35 74 | 75 | // Chipkit Uno32 - attachInterrupt may not support CHANGE option 76 | #elif defined(__PIC32MX__) && defined(_BOARD_MEGA_) 77 | #define CORE_NUM_INTERRUPT 5 78 | #define CORE_INT0_PIN 3 79 | #define CORE_INT1_PIN 2 80 | #define CORE_INT2_PIN 7 81 | #define CORE_INT3_PIN 21 82 | #define CORE_INT4_PIN 20 83 | 84 | // http://hlt.media.mit.edu/?p=1229 85 | #elif defined(__AVR_ATtiny45__) || defined(__AVR_ATtiny85__) 86 | #define CORE_NUM_INTERRUPT 1 87 | #define CORE_INT0_PIN 2 88 | 89 | //https://github.com/SpenceKonde/ATTinyCore/blob/master/avr/extras/ATtiny_x313.md 90 | #elif defined(__AVR_ATtinyX313__) 91 | #define CORE_NUM_INTERRUPT 2 92 | #define CORE_INT0_PIN 4 93 | #define CORE_INT1_PIN 5 94 | 95 | // Attiny167 same core as abobe 96 | #elif defined(__AVR_ATtiny167__) 97 | #define CORE_NUM_INTERRUPT 2 98 | #define CORE_INT0_PIN 14 99 | #define CORE_INT1_PIN 3 100 | 101 | 102 | // Arduino Due 103 | #elif defined(__SAM3X8E__) 104 | #define CORE_NUM_INTERRUPT 54 105 | #define CORE_INT0_PIN 0 106 | #define CORE_INT1_PIN 1 107 | #define CORE_INT2_PIN 2 108 | #define CORE_INT3_PIN 3 109 | #define CORE_INT4_PIN 4 110 | #define CORE_INT5_PIN 5 111 | #define CORE_INT6_PIN 6 112 | #define CORE_INT7_PIN 7 113 | #define CORE_INT8_PIN 8 114 | #define CORE_INT9_PIN 9 115 | #define CORE_INT10_PIN 10 116 | #define CORE_INT11_PIN 11 117 | #define CORE_INT12_PIN 12 118 | #define CORE_INT13_PIN 13 119 | #define CORE_INT14_PIN 14 120 | #define CORE_INT15_PIN 15 121 | #define CORE_INT16_PIN 16 122 | #define CORE_INT17_PIN 17 123 | #define CORE_INT18_PIN 18 124 | #define CORE_INT19_PIN 19 125 | #define CORE_INT20_PIN 20 126 | #define CORE_INT21_PIN 21 127 | #define CORE_INT22_PIN 22 128 | #define CORE_INT23_PIN 23 129 | #define CORE_INT24_PIN 24 130 | #define CORE_INT25_PIN 25 131 | #define CORE_INT26_PIN 26 132 | #define CORE_INT27_PIN 27 133 | #define CORE_INT28_PIN 28 134 | #define CORE_INT29_PIN 29 135 | #define CORE_INT30_PIN 30 136 | #define CORE_INT31_PIN 31 137 | #define CORE_INT32_PIN 32 138 | #define CORE_INT33_PIN 33 139 | #define CORE_INT34_PIN 34 140 | #define CORE_INT35_PIN 35 141 | #define CORE_INT36_PIN 36 142 | #define CORE_INT37_PIN 37 143 | #define CORE_INT38_PIN 38 144 | #define CORE_INT39_PIN 39 145 | #define CORE_INT40_PIN 40 146 | #define CORE_INT41_PIN 41 147 | #define CORE_INT42_PIN 42 148 | #define CORE_INT43_PIN 43 149 | #define CORE_INT44_PIN 44 150 | #define CORE_INT45_PIN 45 151 | #define CORE_INT46_PIN 46 152 | #define CORE_INT47_PIN 47 153 | #define CORE_INT48_PIN 48 154 | #define CORE_INT49_PIN 49 155 | #define CORE_INT50_PIN 50 156 | #define CORE_INT51_PIN 51 157 | #define CORE_INT52_PIN 52 158 | #define CORE_INT53_PIN 53 159 | 160 | // ESP8266 (https://github.com/esp8266/Arduino/) 161 | #elif defined(ESP8266) 162 | #define CORE_NUM_INTERRUPT EXTERNAL_NUM_INTERRUPTS 163 | #define CORE_INT0_PIN 0 164 | #define CORE_INT1_PIN 1 165 | #define CORE_INT2_PIN 2 166 | #define CORE_INT3_PIN 3 167 | #define CORE_INT4_PIN 4 168 | #define CORE_INT5_PIN 5 169 | // GPIO6-GPIO11 are typically used to interface with the flash memory IC on 170 | // most esp8266 modules, so we should avoid adding interrupts to these pins. 171 | #define CORE_INT12_PIN 12 172 | #define CORE_INT13_PIN 13 173 | #define CORE_INT14_PIN 14 174 | #define CORE_INT15_PIN 15 175 | 176 | // ESP32 (https://github.com/espressif/arduino-esp32) 177 | #elif defined(ESP32) 178 | 179 | #define CORE_NUM_INTERRUPT 40 180 | #define CORE_INT0_PIN 0 181 | #define CORE_INT1_PIN 1 182 | #define CORE_INT2_PIN 2 183 | #define CORE_INT3_PIN 3 184 | #define CORE_INT4_PIN 4 185 | #define CORE_INT5_PIN 5 186 | // GPIO6-GPIO11 are typically used to interface with the flash memory IC on 187 | // esp32, so we should avoid adding interrupts to these pins. 188 | #define CORE_INT12_PIN 12 189 | #define CORE_INT13_PIN 13 190 | #define CORE_INT14_PIN 14 191 | #define CORE_INT15_PIN 15 192 | #define CORE_INT16_PIN 16 193 | #define CORE_INT17_PIN 17 194 | #define CORE_INT18_PIN 18 195 | #define CORE_INT19_PIN 19 196 | #define CORE_INT21_PIN 21 197 | #define CORE_INT22_PIN 22 198 | #define CORE_INT23_PIN 23 199 | #define CORE_INT25_PIN 25 200 | #define CORE_INT26_PIN 26 201 | #define CORE_INT27_PIN 27 202 | #define CORE_INT32_PIN 32 203 | #define CORE_INT33_PIN 33 204 | #define CORE_INT34_PIN 34 205 | #define CORE_INT35_PIN 35 206 | #define CORE_INT36_PIN 36 207 | #define CORE_INT39_PIN 39 208 | 209 | 210 | // Arduino Zero - TODO: interrupts do not seem to work 211 | // please help, contribute a fix! 212 | #elif defined(ARDUINO_ARCH_SAMD) 213 | #define CORE_NUM_INTERRUPT 20 214 | #define CORE_INT0_PIN 0 215 | #define CORE_INT1_PIN 1 216 | #define CORE_INT2_PIN 2 217 | #define CORE_INT3_PIN 3 218 | #define CORE_INT5_PIN 5 219 | #define CORE_INT6_PIN 6 220 | #define CORE_INT7_PIN 7 221 | #define CORE_INT8_PIN 8 222 | #define CORE_INT9_PIN 9 223 | #define CORE_INT10_PIN 10 224 | #define CORE_INT11_PIN 11 225 | #define CORE_INT12_PIN 12 226 | #define CORE_INT13_PIN 13 227 | #define CORE_INT14_PIN 14 228 | #define CORE_INT15_PIN 15 229 | #define CORE_INT16_PIN 16 230 | #define CORE_INT17_PIN 17 231 | #define CORE_INT18_PIN 18 232 | #define CORE_INT19_PIN 19 233 | 234 | // Arduino 101 235 | #elif defined(__arc__) 236 | #define CORE_NUM_INTERRUPT 14 237 | #define CORE_INT2_PIN 2 238 | #define CORE_INT5_PIN 5 239 | #define CORE_INT7_PIN 7 240 | #define CORE_INT8_PIN 8 241 | #define CORE_INT10_PIN 10 242 | #define CORE_INT11_PIN 11 243 | #define CORE_INT12_PIN 12 244 | #define CORE_INT13_PIN 13 245 | 246 | #endif 247 | #endif 248 | 249 | #if !defined(CORE_NUM_INTERRUPT) 250 | #error "Interrupts are unknown for this board, please add to this code" 251 | #endif 252 | #if CORE_NUM_INTERRUPT <= 0 253 | #error "Encoder requires interrupt pins, but this board does not have any :(" 254 | #error "You could try defining ENCODER_DO_NOT_USE_INTERRUPTS as a kludge." 255 | #endif 256 | 257 | -------------------------------------------------------------------------------- /extras/D11-Firmware/src/PID/PID_v1.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************************** 2 | * Arduino PID Library - Version 1.2.1 3 | * by Brett Beauregard brettbeauregard.com 4 | * 5 | * This Library is licensed under the MIT License 6 | **********************************************************************************************/ 7 | 8 | #if ARDUINO >= 100 9 | #include "Arduino.h" 10 | #else 11 | #include "WProgram.h" 12 | #endif 13 | 14 | #include "PID_v1.h" 15 | 16 | /*Constructor (...)********************************************************* 17 | * The parameters specified here are those for for which we can't set up 18 | * reliable defaults, so we need to have the user set them. 19 | ***************************************************************************/ 20 | PID::PID(Fix16* Input, Fix16* Output, Fix16* Setpoint, 21 | Fix16 Kp, Fix16 Ki, Fix16 Kd, int POn, int ControllerDirection) 22 | { 23 | myOutput = Output; 24 | myInput = Input; 25 | mySetpoint = Setpoint; 26 | inAuto = false; 27 | 28 | PID::SetOutputLimits(Fix16(0.0f), Fix16(255.0f)); //default output limit corresponds to 29 | //the arduino pwm limits 30 | 31 | SampleTime = 10; //default Controller Sample Time is 0.1 seconds 32 | 33 | PID::SetControllerDirection(ControllerDirection); 34 | PID::SetTunings(Kp, Ki, Kd, POn); 35 | 36 | lastTime = millis()-SampleTime; 37 | } 38 | 39 | /*Constructor (...)********************************************************* 40 | * To allow backwards compatability for v1.1, or for people that just want 41 | * to use Proportional on Error without explicitly saying so 42 | ***************************************************************************/ 43 | 44 | PID::PID(Fix16* Input, Fix16* Output, Fix16* Setpoint, 45 | Fix16 Kp, Fix16 Ki, Fix16 Kd, int ControllerDirection) 46 | :PID::PID(Input, Output, Setpoint, Kp, Ki, Kd, P_ON_E, ControllerDirection) 47 | { 48 | 49 | } 50 | 51 | 52 | /* Compute() ********************************************************************** 53 | * This, as they say, is where the magic happens. this function should be called 54 | * every time "void loop()" executes. the function will decide for itself whether a new 55 | * pid Output needs to be computed. returns true when the output is computed, 56 | * false when nothing has been done. 57 | **********************************************************************************/ 58 | bool PID::Compute() 59 | { 60 | if(!inAuto) return false; 61 | unsigned long now = millis(); 62 | unsigned long timeChange = (now - lastTime); 63 | if(timeChange>=SampleTime) 64 | { 65 | /*Compute all the working error variables*/ 66 | Fix16 input = *myInput; 67 | Fix16 error = *mySetpoint - input; 68 | Fix16 dError = (error - lastError); 69 | iError += error; //add error to the integral term 70 | 71 | Fix16 Pout = kp * error; 72 | Fix16 Iout = ki * iError; 73 | Fix16 Dout = kd * dError; 74 | 75 | Fix16 output; 76 | output = Pout + Iout + Dout; 77 | 78 | if(output > outMax) output = outMax; 79 | else if(output < outMin) output = outMin; 80 | *myOutput = output; 81 | 82 | /*Remember some variables for next time*/ 83 | lastInput = input; 84 | lastTime = now; 85 | lastError = error; 86 | return true; 87 | } 88 | else return false; 89 | } 90 | 91 | /* SetTunings(...)************************************************************* 92 | * This function allows the controller's dynamic performance to be adjusted. 93 | * it's called automatically from the constructor, but tunings can also 94 | * be adjusted on the fly during normal operation 95 | ******************************************************************************/ 96 | void PID::SetTunings(Fix16 Kp, Fix16 Ki, Fix16 Kd, int POn) 97 | { 98 | if (Kp 0) 131 | { 132 | Fix16 ratio = Fix16((float)NewSampleTime) 133 | / Fix16((float)SampleTime); 134 | ki *= ratio; 135 | kd /= ratio; 136 | SampleTime = (unsigned long)NewSampleTime; 137 | } 138 | } 139 | 140 | /* SetOutputLimits(...)**************************************************** 141 | * This function will be used far more often than SetInputLimits. while 142 | * the input to the controller will generally be in the 0-1023 range (which is 143 | * the default already,) the output will be a little different. maybe they'll 144 | * be doing a time window and will need 0-8000 or something. or maybe they'll 145 | * want to clamp it from 0-125. who knows. at any rate, that can all be done 146 | * here. 147 | **************************************************************************/ 148 | void PID::SetOutputLimits(Fix16 Min, Fix16 Max) 149 | { 150 | if(Min >= Max) return; 151 | outMin = Min; 152 | outMax = Max; 153 | 154 | if(inAuto) 155 | { 156 | if(*myOutput > outMax) *myOutput = outMax; 157 | else if(*myOutput < outMin) *myOutput = outMin; 158 | 159 | if(outputSum > outMax) outputSum= outMax; 160 | else if(outputSum < outMin) outputSum= outMin; 161 | } 162 | } 163 | 164 | /* SetMode(...)**************************************************************** 165 | * Allows the controller Mode to be set to manual (0) or Automatic (non-zero) 166 | * when the transition from manual to auto occurs, the controller is 167 | * automatically initialized 168 | ******************************************************************************/ 169 | void PID::SetMode(int Mode) 170 | { 171 | bool newAuto = (Mode == AUTOMATIC); 172 | if(newAuto && !inAuto) 173 | { /*we just went from manual to auto*/ 174 | PID::Initialize(); 175 | } 176 | inAuto = newAuto; 177 | } 178 | 179 | /* Initialize()**************************************************************** 180 | * does all the things that need to happen to ensure a bumpless transfer 181 | * from manual to automatic mode. 182 | ******************************************************************************/ 183 | void PID::Initialize() 184 | { 185 | outputSum = *myOutput; 186 | lastInput = *myInput; 187 | if(outputSum > outMax) outputSum = outMax; 188 | else if(outputSum < outMin) outputSum = outMin; 189 | } 190 | 191 | /* SetControllerDirection(...)************************************************* 192 | * The PID will either be connected to a DIRECT acting process (+Output leads 193 | * to +Input) or a REVERSE acting process(+Output leads to -Input.) we need to 194 | * know which one, because otherwise we may increase the output when we should 195 | * be decreasing. This is called from the constructor. 196 | ******************************************************************************/ 197 | void PID::SetControllerDirection(int Direction) 198 | { 199 | if(inAuto && Direction !=controllerDirection) 200 | { 201 | kp = (Fix16(0.0) - kp); 202 | ki = (Fix16(0.0) - ki); 203 | kd = (Fix16(0.0) - kd); 204 | } 205 | controllerDirection = Direction; 206 | } 207 | 208 | /* Status Funcions************************************************************* 209 | * Just because you set the Kp=-1 doesn't mean it actually happened. these 210 | * functions query the internal state of the PID. they're here for display 211 | * purposes. this are the functions the PID Front-end uses for example 212 | ******************************************************************************/ 213 | Fix16 PID::GetKp(){ return dispKp; } 214 | Fix16 PID::GetKi(){ return dispKi;} 215 | Fix16 PID::GetKd(){ return dispKd;} 216 | int PID::GetMode(){ return inAuto ? AUTOMATIC : MANUAL;} 217 | int PID::GetDirection(){ return controllerDirection;} 218 | 219 | --------------------------------------------------------------------------------