diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..c314114 --- /dev/null +++ b/.clang-format @@ -0,0 +1,288 @@ +--- +Language: Cpp +AccessModifierOffset: -2 +AlignAfterOpenBracket: Align +AlignArrayOfStructures: None +AlignConsecutiveAssignments: + Enabled: false + AcrossEmptyLines: false + AcrossComments: false + AlignCompound: false + AlignFunctionDeclarations: false + AlignFunctionPointers: false + PadOperators: true +AlignConsecutiveBitFields: + Enabled: false + AcrossEmptyLines: false + AcrossComments: false + AlignCompound: false + AlignFunctionDeclarations: false + AlignFunctionPointers: false + PadOperators: false +AlignConsecutiveDeclarations: + Enabled: false + AcrossEmptyLines: false + AcrossComments: false + AlignCompound: false + AlignFunctionDeclarations: true + AlignFunctionPointers: false + PadOperators: false +AlignConsecutiveMacros: + Enabled: false + AcrossEmptyLines: false + AcrossComments: false + AlignCompound: false + AlignFunctionDeclarations: false + AlignFunctionPointers: false + PadOperators: false +AlignConsecutiveShortCaseStatements: + Enabled: false + AcrossEmptyLines: false + AcrossComments: false + AlignCaseArrows: false + AlignCaseColons: false +AlignConsecutiveTableGenBreakingDAGArgColons: + Enabled: false + AcrossEmptyLines: false + AcrossComments: false + AlignCompound: false + AlignFunctionDeclarations: false + AlignFunctionPointers: false + PadOperators: false +AlignConsecutiveTableGenCondOperatorColons: + Enabled: false + AcrossEmptyLines: false + AcrossComments: false + AlignCompound: false + AlignFunctionDeclarations: false + AlignFunctionPointers: false + PadOperators: false +AlignConsecutiveTableGenDefinitionColons: + Enabled: false + AcrossEmptyLines: false + AcrossComments: false + AlignCompound: false + AlignFunctionDeclarations: false + AlignFunctionPointers: false + PadOperators: false +AlignEscapedNewlines: Right +AlignOperands: Align +AlignTrailingComments: + Kind: Always + OverEmptyLines: 0 +AllowAllArgumentsOnNextLine: true +AllowAllParametersOfDeclarationOnNextLine: true +AllowBreakBeforeNoexceptSpecifier: Never +AllowShortBlocksOnASingleLine: Never +AllowShortCaseExpressionOnASingleLine: true +AllowShortCaseLabelsOnASingleLine: false +AllowShortCompoundRequirementOnASingleLine: true +AllowShortEnumsOnASingleLine: true +AllowShortFunctionsOnASingleLine: All +AllowShortIfStatementsOnASingleLine: Never +AllowShortLambdasOnASingleLine: All +AllowShortLoopsOnASingleLine: false +AllowShortNamespacesOnASingleLine: false +AlwaysBreakAfterDefinitionReturnType: None +AlwaysBreakBeforeMultilineStrings: false +AttributeMacros: + - __capability +BinPackArguments: true +BinPackParameters: BinPack +BitFieldColonSpacing: Both +BraceWrapping: + AfterCaseLabel: false + AfterClass: false + AfterControlStatement: Never + AfterEnum: false + AfterExternBlock: false + AfterFunction: false + AfterNamespace: false + AfterObjCDeclaration: false + AfterStruct: false + AfterUnion: false + BeforeCatch: false + BeforeElse: false + BeforeLambdaBody: false + BeforeWhile: false + IndentBraces: false + SplitEmptyFunction: true + SplitEmptyRecord: true + SplitEmptyNamespace: true +BreakAdjacentStringLiterals: true +BreakAfterAttributes: Leave +BreakAfterJavaFieldAnnotations: false +BreakAfterReturnType: None +BreakArrays: true +BreakBeforeBinaryOperators: None +BreakBeforeConceptDeclarations: Always +BreakBeforeBraces: Attach +BreakBeforeInlineASMColon: OnlyMultiline +BreakBeforeTernaryOperators: true +BreakBinaryOperations: Never +BreakConstructorInitializers: BeforeColon +BreakFunctionDefinitionParameters: false +BreakInheritanceList: BeforeColon +BreakStringLiterals: true +BreakTemplateDeclarations: MultiLine +ColumnLimit: 80 +CommentPragmas: '^ IWYU pragma:' +CompactNamespaces: false +ConstructorInitializerIndentWidth: 4 +ContinuationIndentWidth: 4 +Cpp11BracedListStyle: true +DerivePointerAlignment: false +DisableFormat: false +EmptyLineAfterAccessModifier: Never +EmptyLineBeforeAccessModifier: LogicalBlock +ExperimentalAutoDetectBinPacking: false +FixNamespaceComments: true +ForEachMacros: + - foreach + - Q_FOREACH + - BOOST_FOREACH +IfMacros: + - KJ_IF_MAYBE +IncludeBlocks: Preserve +IncludeCategories: + - Regex: '^"(llvm|llvm-c|clang|clang-c)/' + Priority: 2 + SortPriority: 0 + CaseSensitive: false + - Regex: '^(<|"(gtest|gmock|isl|json)/)' + Priority: 3 + SortPriority: 0 + CaseSensitive: false + - Regex: '.*' + Priority: 1 + SortPriority: 0 + CaseSensitive: false +IncludeIsMainRegex: '(Test)?$' +IncludeIsMainSourceRegex: '' +IndentAccessModifiers: false +IndentCaseBlocks: false +IndentCaseLabels: false +IndentExportBlock: true +IndentExternBlock: AfterExternBlock +IndentGotoLabels: true +IndentPPDirectives: None +IndentRequiresClause: true +IndentWidth: 2 +IndentWrappedFunctionNames: false +InsertBraces: false +InsertNewlineAtEOF: false +InsertTrailingCommas: None +IntegerLiteralSeparator: + Binary: 0 + BinaryMinDigits: 0 + Decimal: 0 + DecimalMinDigits: 0 + Hex: 0 + HexMinDigits: 0 +JavaScriptQuotes: Leave +JavaScriptWrapImports: true +KeepEmptyLines: + AtEndOfFile: false + AtStartOfBlock: true + AtStartOfFile: true +KeepFormFeed: false +LambdaBodyIndentation: Signature +LineEnding: DeriveLF +MacroBlockBegin: '' +MacroBlockEnd: '' +MainIncludeChar: Quote +MaxEmptyLinesToKeep: 1 +NamespaceIndentation: None +ObjCBinPackProtocolList: Auto +ObjCBlockIndentWidth: 2 +ObjCBreakBeforeNestedBlockParam: true +ObjCSpaceAfterProperty: false +ObjCSpaceBeforeProtocolList: true +PackConstructorInitializers: BinPack +PenaltyBreakAssignment: 2 +PenaltyBreakBeforeFirstCallParameter: 19 +PenaltyBreakBeforeMemberAccess: 150 +PenaltyBreakComment: 300 +PenaltyBreakFirstLessLess: 120 +PenaltyBreakOpenParenthesis: 0 +PenaltyBreakScopeResolution: 500 +PenaltyBreakString: 1000 +PenaltyBreakTemplateDeclaration: 10 +PenaltyExcessCharacter: 1000000 +PenaltyIndentedWhitespace: 0 +PenaltyReturnTypeOnItsOwnLine: 60 +PointerAlignment: Right +PPIndentWidth: -1 +QualifierAlignment: Leave +ReferenceAlignment: Pointer +ReflowComments: Always +RemoveBracesLLVM: false +RemoveEmptyLinesInUnwrappedLines: false +RemoveParentheses: Leave +RemoveSemicolon: false +RequiresClausePosition: OwnLine +RequiresExpressionIndentation: OuterScope +SeparateDefinitionBlocks: Leave +ShortNamespaceLines: 1 +SkipMacroDefinitionBody: false +SortIncludes: CaseSensitive +SortJavaStaticImport: Before +SortUsingDeclarations: LexicographicNumeric +SpaceAfterCStyleCast: false +SpaceAfterLogicalNot: false +SpaceAfterTemplateKeyword: true +SpaceAroundPointerQualifiers: Default +SpaceBeforeAssignmentOperators: true +SpaceBeforeCaseColon: false +SpaceBeforeCpp11BracedList: false +SpaceBeforeCtorInitializerColon: true +SpaceBeforeInheritanceColon: true +SpaceBeforeJsonColon: false +SpaceBeforeParens: ControlStatements +SpaceBeforeParensOptions: + AfterControlStatements: true + AfterForeachMacros: true + AfterFunctionDefinitionName: false + AfterFunctionDeclarationName: false + AfterIfMacros: true + AfterOverloadedOperator: false + AfterPlacementOperator: true + AfterRequiresInClause: false + AfterRequiresInExpression: false + BeforeNonEmptyParentheses: false +SpaceBeforeRangeBasedForLoopColon: true +SpaceBeforeSquareBrackets: false +SpaceInEmptyBlock: false +SpacesBeforeTrailingComments: 1 +SpacesInAngles: Never +SpacesInContainerLiterals: true +SpacesInLineCommentPrefix: + Minimum: 1 + Maximum: -1 +SpacesInParens: Never +SpacesInParensOptions: + ExceptDoubleParentheses: false + InCStyleCasts: false + InConditionalStatements: false + InEmptyParentheses: false + Other: false +SpacesInSquareBrackets: false +Standard: Latest +StatementAttributeLikeMacros: + - Q_EMIT +StatementMacros: + - Q_UNUSED + - QT_REQUIRE_VERSION +TableGenBreakInsideDAGArg: DontBreak +TabWidth: 8 +UseTab: Never +VerilogBreakBetweenInstancePorts: true +WhitespaceSensitiveMacros: + - BOOST_PP_STRINGIZE + - CF_SWIFT_NAME + - NS_SWIFT_NAME + - PP_STRINGIZE + - STRINGIZE +WrapNamespaceBodyWithEmptyLines: Leave +... + diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..03da703 --- /dev/null +++ b/.gitignore @@ -0,0 +1,4 @@ +*.swp +/build +compile_commands.json +.cache/ diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000..b4f473c --- /dev/null +++ b/.gitmodules @@ -0,0 +1,9 @@ +[submodule "deps/Almond_PCB"] + path = deps/Almond_PCB + url = git@github.com:Open-Bionics/Almond_PCB.git +[submodule "deps/ArduinoCore-avr"] + path = deps/ArduinoCore-avr + url = git@github.com:arduino/ArduinoCore-avr.git +[submodule "deps/ArduinoCore-API"] + path = deps/ArduinoCore-API + url = git@github.com:arduino/ArduinoCore-API.git diff --git a/Makefile b/Makefile new file mode 100644 index 0000000..81b8659 --- /dev/null +++ b/Makefile @@ -0,0 +1,31 @@ +CXX=avr-g++ +CXX_FLAGS=-Os +MCU_FLAGS=\ + -mmcu=atmega2560 \ + -D F_CPU=16000000UL \ + +ARDUINO_INCLUDE=\ + -D ARDUINO_ARCH_AVR \ + -D ARDUINO_AVR_MEGA2560 \ + -I deps/ArduinoCore-avr/cores/arduino/ \ + -I deps/ArduinoCore-avr/variants/mega/ \ + +# Find all implementation files excluding non AVR archs (eg: samd) +EXECUTABLES= $(shell find src -type f -name *.cpp \ + -not -path */samd_*) + +.PHONY: clean + +all: $(patsubst src/%.cpp,build/%.o,$(EXECUTABLES)) | build + + +build/%.o: src/%.cpp | build + mkdir -p $(dir $@) + $(CXX) $(CXX_FLAGS) $(MCU_FLAGS) $(ARDUINO_INCLUDE) -c\ + $< -o $@ + +build: + mkdir build + +clean: + rm -rf build/* diff --git a/README.md b/README.md index 7612c8d..b7a3e2b 100644 --- a/README.md +++ b/README.md @@ -24,3 +24,20 @@ To view a copy of this license, visit http://creativecommons.org/licenses/by-sa/ wrote this file. As long as you retain this notice you can do whatever you want with this stuff. If we meet some day, and you think this stuff is worth it, you can buy me a beer in return Olly McBride + + +## Development Instructions + +### How to build the project + +For **the first time** you clone the project fetch the dependecies using the following `git` command: + +``` +git submodule update --init +``` + +To build and rebuild the project use the following GNU Make command: + +``` +make +``` diff --git a/deps/Almond_PCB b/deps/Almond_PCB new file mode 160000 index 0000000..34d1181 --- /dev/null +++ b/deps/Almond_PCB @@ -0,0 +1 @@ +Subproject commit 34d1181837e49c1d00af6181a6ac37bb388f19ed diff --git a/deps/ArduinoCore-API b/deps/ArduinoCore-API new file mode 160000 index 0000000..4a02bfc --- /dev/null +++ b/deps/ArduinoCore-API @@ -0,0 +1 @@ +Subproject commit 4a02bfc0a924e1fec34c3bb82ffd5dfba7643a0c diff --git a/deps/ArduinoCore-avr b/deps/ArduinoCore-avr new file mode 160000 index 0000000..c8c514c --- /dev/null +++ b/deps/ArduinoCore-avr @@ -0,0 +1 @@ +Subproject commit c8c514c9a19602542bc32c7033f48fecbbda4401 diff --git a/deps/README.md b/deps/README.md new file mode 100644 index 0000000..9b664a4 --- /dev/null +++ b/deps/README.md @@ -0,0 +1,3 @@ +# Dependencies + +This directory contains the dependencies needed to build the project diff --git a/src/FingerLib.cpp b/src/FingerLib.cpp index 8a941ab..f96f5b2 100644 --- a/src/FingerLib.cpp +++ b/src/FingerLib.cpp @@ -4,580 +4,499 @@ * Created: 02/11/2015 11:12:48 * Author: Olly McBride * - * This work is licensed under the Creative Commons Attribution 4.0 International License. - * To view a copy of this license, visit http://creativecommons.org/licenses/by/4.0/. + * This work is licensed under the Creative Commons Attribution 4.0 + * International License. To view a copy of this license, visit + * http://creativecommons.org/licenses/by/4.0/. * - */ + * Modified by David Kennedy S. Araujo (software@davidkennedy.dev) to clean code + * + */ #include "FingerLib.h" -uint8_t _TotalFingerCount = 0; // the total number of finger instances -uint8_t _TotalAttachedFingers = 0; // the total number of attached/configured fingers - - -Finger* fingerISRList[MAX_FINGERS] = { NULL }; // pointer to an instance of the Finger class -bool _posCtrlTimerInit = false; // flag to prevent multiple timer initialisations - - - -////////////////////////////// Constructor/Destructor ////////////////////////////// -Finger::Finger() -{ - if (_fingerIndex >= MAX_FINGERS) // if too many fingers have been initialised - { - _fingerIndex = -1; // set current finger number to be empty - _isActive = false; // set current finger as inactive - } - else - { - _fingerIndex = _TotalFingerCount++; // count the total number of fingers initialised - } - - motorEnable(false); - - _pos = { 0 }; - _dir = { 0 }; - _speed = { 0 }; - _PWM = { 0 }; -#ifdef FORCE_SENSE - _force = { 0 }; +uint8_t _TotalFingerCount = 0; // the total number of finger instances +uint8_t _TotalAttachedFingers = + 0; // the total number of attached/configured fingers + +Finger *fingerISRList[MAX_FINGERS] = { + NULL}; // pointer to an instance of the Finger class +bool _posCtrlTimerInit = + false; // flag to prevent multiple timer initialisations + +////////////////////////////// Constructor/Destructor +///////////////////////////////// +Finger::Finger() { + if (_fingerIndex >= MAX_FINGERS) // if too many fingers have been initialised + { + _fingerIndex = -1; // set current finger number to be empty + _isActive = false; // set current finger as inactive + } else { + _fingerIndex = + _TotalFingerCount++; // count the total number of fingers initialised + } + + motorEnable(false); + + _pos = {0}; + _dir = {0}; + _speed = {0}; + _PWM = {0}; +#ifdef FORCE_SENSE + _force = {0}; #endif - _interruptEn = true; // use the timer interrupt by default + _interruptEn = true; // use the timer interrupt by default } ////////////////////////////// Public Methods ////////////////////////////// - // INITIALISATION // attach pins to a finger using only position control -uint8_t Finger::attach(uint8_t dir0, uint8_t dir1, uint8_t posSns) -{ - return attach(dir0, dir1, posSns, (uint8_t)-1, 0); // no force sense, do not invert +uint8_t Finger::attach(uint8_t dir0, uint8_t dir1, uint8_t posSns) { + return attach(dir0, dir1, posSns, (uint8_t)-1, + 0); // no force sense, do not invert } -// attach pins to a finger using only position control, but allow the direction to be inverted -uint8_t Finger::attach(uint8_t dir0, uint8_t dir1, uint8_t posSns, bool inv) -{ - return attach(dir0, dir1, posSns, (uint8_t)-1, inv); // no force sense +// attach pins to a finger using only position control, but allow the direction +// to be inverted +uint8_t Finger::attach(uint8_t dir0, uint8_t dir1, uint8_t posSns, bool inv) { + return attach(dir0, dir1, posSns, (uint8_t)-1, inv); // no force sense } -// attach pins to a finger using position control and force control, and allow the direction to be inverted -uint8_t Finger::attach(uint8_t dir0, uint8_t dir1, uint8_t posSns, uint8_t forceSns, bool inv) -{ - // if the current finger number is valid - if ((_fingerIndex >= 0) && (_fingerIndex < MAX_FINGERS)) - { - motorEnable(false); // disable the motor - - // attach all finger pins - _pin.dir[0] = dir0; - _pin.dir[1] = dir1; - _pin.posSns = posSns; +// attach pins to a finger using position control and force control, and allow +// the direction to be inverted +uint8_t Finger::attach(uint8_t dir0, uint8_t dir1, uint8_t posSns, + uint8_t forceSns, bool inv) { + // if the current finger number is valid + if ((_fingerIndex >= 0) && (_fingerIndex < MAX_FINGERS)) { + motorEnable(false); // disable the motor + + // attach all finger pins + _pin.dir[0] = dir0; + _pin.dir[1] = dir1; + _pin.posSns = posSns; #ifdef FORCE_SENSE - _pin.forceSns = forceSns; + _pin.forceSns = forceSns; #endif - _invert = inv; // store whether to invert the finger direction - - // configure pins - pinMode(dir0, OUTPUT); // set direction1 pin to output - pinMode(dir1, OUTPUT); // set direction2 pin to output - pinMode(posSns, INPUT); // set position sense pin to input + _invert = inv; // store whether to invert the finger direction + + // configure pins + pinMode(dir0, OUTPUT); // set direction1 pin to output + pinMode(dir1, OUTPUT); // set direction2 pin to output + pinMode(posSns, INPUT); // set position sense pin to input #ifdef FORCE_SENSE - pinMode(forceSns, INPUT); // set force sense pin to input + pinMode(forceSns, INPUT); // set force sense pin to input #endif - // initialise circle buffer - _velBuff.begin(VEL_BUFF_SIZE); -#if defined(FORCE_SENSE) - _IBuff.begin(CURR_SENSE_BUFF_SIZE); + // initialise circle buffer + _velBuff.begin(VEL_BUFF_SIZE); +#if defined(FORCE_SENSE) + _IBuff.begin(CURR_SENSE_BUFF_SIZE); #endif - // set limits and initial values - setPosLimits(MIN_FINGER_POS, MAX_FINGER_POS); - setPWMLimits(MIN_FINGER_PWM, MAX_FINGER_PWM); - writeDir(OPEN); - writeSpeed(MAX_FINGER_PWM); + // set limits and initial values + setPosLimits(MIN_FINGER_POS, MAX_FINGER_POS); + setPWMLimits(MIN_FINGER_PWM, MAX_FINGER_PWM); + writeDir(OPEN); + writeSpeed(MAX_FINGER_PWM); #ifdef FORCE_SENSE - forceSenseEnable(true); + forceSenseEnable(true); #endif - // if finger is being attached for the first time - if (!_isActive) - { - _TotalAttachedFingers++; // update the count of total attached fingers - } + // if finger is being attached for the first time + if (!_isActive) { + _TotalAttachedFingers++; // update the count of total attached fingers + } - // add a pointer to the current finger instance to the list for position control calls, used by the ISR - fingerISRList[_fingerIndex] = this; + // add a pointer to the current finger instance to the list for position + // control calls, used by the ISR + fingerISRList[_fingerIndex] = this; - // initialise the position control timer - if (!_posCtrlTimerInit) - { - if (_interruptEn) // if the interrupt is enabled for motor control - { - _passMotorPtr(&_fingerControlCallback); // attach the finger control function to the timer - } + // initialise the position control timer + if (!_posCtrlTimerInit) { + if (_interruptEn) // if the interrupt is enabled for motor control + { + _passMotorPtr(&_fingerControlCallback); // attach the finger control + // function to the timer + } - _posCtrlTimerSetup(); // initialise and start the timer - _posCtrlTimerInit = true; - } + _posCtrlTimerSetup(); // initialise and start the timer + _posCtrlTimerInit = true; + } #ifdef FORCE_SENSE - // attach current sense control interrupt, initialise and sync PWM timers - initCurrentSense(_pin.dir[0], _pin.dir[1], &_currentSenseCallback); + // attach current sense control interrupt, initialise and sync PWM timers + initCurrentSense(_pin.dir[0], _pin.dir[1], &_currentSenseCallback); #endif #ifdef ARDUINO_AVR_MEGA2560 - // if using the Atmega2560, set the PWM freq to > 20kHz to prevent humming - setPWMFreq(dir0, 0x01); // set PWM frequency to max freq - setPWMFreq(dir1, 0x01); // set PWM frequency to max freq + // if using the Atmega2560, set the PWM freq to > 20kHz to prevent humming + setPWMFreq(dir0, 0x01); // set PWM frequency to max freq + setPWMFreq(dir1, 0x01); // set PWM frequency to max freq #endif - motorEnable(true); // re-enable the motor - _isActive = true; // set the current finger to be active - return _fingerIndex; // return the current finger number - } - else // if the current finger number isn't valid - { - //MYSERIAL.println("ERROR - Too many fingers attached"); - _isActive = false; // set the current finger to be inactive - return (uint8_t)(-1); // return BLANK - } + motorEnable(true); // re-enable the motor + _isActive = true; // set the current finger to be active + return _fingerIndex; // return the current finger number + } else // if the current finger number isn't valid + { + // MYSERIAL.println("ERROR - Too many fingers attached"); + _isActive = false; // set the current finger to be inactive + return (uint8_t)(-1); // return BLANK + } } // deactivate the finger -void Finger::detach(void) -{ - _isActive = false; - _TotalAttachedFingers--; +void Finger::detach(void) { + _isActive = false; + _TotalAttachedFingers--; } -// return true if the current finger is attached and initialised correctly -bool Finger::attached(void) -{ - return _isActive; -} +// return true if the current finger is attached and initialised correctly +bool Finger::attached(void) { return _isActive; } // invert the current finger direction -void Finger::invertFingerDir(void) -{ - _invert = !_invert; -} - +void Finger::invertFingerDir(void) { _invert = !_invert; } // LIMITS // set the maximum and minimum position limits -void Finger::setPosLimits(int min, int max) -{ - // set limits - _pos.limit.min = min; - _pos.limit.max = max; +void Finger::setPosLimits(int min, int max) { + // set limits + _pos.limit.min = min; + _pos.limit.max = max; } // set the maximum and minimum speed limits -void Finger::setPWMLimits(int min, int max) -{ - // set limits - _PWM.limit.min = min; - _PWM.limit.max = max; - -#if defined(USE_PID) - _PID.setLimits(-(double)_PWM.limit.max, _PWM.limit.max); +void Finger::setPWMLimits(int min, int max) { + // set limits + _PWM.limit.min = min; + _PWM.limit.max = max; + +#if defined(USE_PID) + _PID.setLimits(-(double)_PWM.limit.max, _PWM.limit.max); #endif } #ifdef FORCE_SENSE // set the maximum and minimum force limits -void Finger::setForceLimits(int min, int max) -{ - // set limits - _force.limit.min = convertForceToADC(min); // force values converted to ADC values for quicker maths - _force.limit.max = convertForceToADC(max); // force values converted to ADC values for quicker maths +void Finger::setForceLimits(int min, int max) { + // set limits + _force.limit.min = convertForceToADC( + min); // force values converted to ADC values for quicker maths + _force.limit.max = convertForceToADC( + max); // force values converted to ADC values for quicker maths } #endif - // POS // write a target position to the finger -void Finger::writePos(int value) -{ - // constrain position value to limits - _pos.targ = constrain((uint16_t)value, _pos.limit.min, _pos.limit.max); - - // calculate new position error (to remove false positives in reachedPos() ) - _pos.error = (_pos.targ - _pos.curr); - - //determine direction of travel - if ((uint16_t)value > _pos.curr) - { - _dir.targ = CLOSE; - } - else - { - _dir.targ = OPEN; - } +void Finger::writePos(int value) { + // constrain position value to limits + _pos.targ = constrain((uint16_t)value, _pos.limit.min, _pos.limit.max); + + // calculate new position error (to remove false positives in reachedPos() ) + _pos.error = (_pos.targ - _pos.curr); + + // determine direction of travel + if ((uint16_t)value > _pos.curr) { + _dir.targ = CLOSE; + } else { + _dir.targ = OPEN; + } } // write a change in position to the finger -void Finger::movePos(int value) -{ - // change position - _pos.targ += readPos() + value; +void Finger::movePos(int value) { + // change position + _pos.targ += readPos() + value; - // constrain position value to limits - _pos.targ = constrain((uint16_t)_pos.targ, _pos.limit.min, _pos.limit.max); + // constrain position value to limits + _pos.targ = constrain((uint16_t)_pos.targ, _pos.limit.min, _pos.limit.max); - // calculate new position error (to remove false positives in reachedPos() ) - _pos.error = (_pos.targ - _pos.curr); + // calculate new position error (to remove false positives in reachedPos() ) + _pos.error = (_pos.targ - _pos.curr); } // return the current position -int16_t Finger::readPos(void) -{ - // read finger position - noInterrupts(); - _pos.curr = analogRead(_pin.posSns); - - // invert finger direction if enabled - if (_invert) - { - _pos.curr = MAX_POS_SENSOR_VAL - _pos.curr; - } - interrupts(); - - return _pos.curr; +int16_t Finger::readPos(void) { + // read finger position + noInterrupts(); + _pos.curr = analogRead(_pin.posSns); + + // invert finger direction if enabled + if (_invert) { + _pos.curr = MAX_POS_SENSOR_VAL - _pos.curr; + } + interrupts(); + + return _pos.curr; } // return the error between the current position and the target position -int16_t Finger::readPosError(void) -{ - return _pos.error; -} +int16_t Finger::readPosError(void) { return _pos.error; } // return the target position -uint16_t Finger::readTargetPos(void) -{ - return _pos.targ; -} +uint16_t Finger::readTargetPos(void) { return _pos.targ; } // returns true if position reached -bool Finger::reachedPos(void) -{ - // if the position error is within a default tolerance - if (abs(readPosError()) < POS_REACHED_TOLERANCE) - { - return true; - } - else - { - return false; - } +bool Finger::reachedPos(void) { + // if the position error is within a default tolerance + if (abs(readPosError()) < POS_REACHED_TOLERANCE) { + return true; + } else { + return false; + } } // returns true if position reached -bool Finger::reachedPos(uint16_t posErr) -{ - // if the position error is within a tolerance - if (abs(readPosError()) < (int16_t)posErr) - { - return true; - } - else - { - return false; - } +bool Finger::reachedPos(uint16_t posErr) { + // if the position error is within a tolerance + if (abs(readPosError()) < (int16_t)posErr) { + return true; + } else { + return false; + } } - // DIR // write a target direction to the finger -void Finger::writeDir(int value) -{ - // store direction - _dir.targ = value; - - // set new target position based on input direction - if (_dir.targ == OPEN) - { - _pos.targ = _pos.limit.min; - } - else if (_dir.targ == CLOSE) - { - _pos.targ = _pos.limit.max; - } +void Finger::writeDir(int value) { + // store direction + _dir.targ = value; + + // set new target position based on input direction + if (_dir.targ == OPEN) { + _pos.targ = _pos.limit.min; + } else if (_dir.targ == CLOSE) { + _pos.targ = _pos.limit.max; + } } // return the current direction -uint8_t Finger::readDir(void) -{ - return _dir.targ; -} +uint8_t Finger::readDir(void) { return _dir.targ; } // open the finger -void Finger::open(void) -{ - writePos(_pos.limit.min); -} +void Finger::open(void) { writePos(_pos.limit.min); } // close the finger -void Finger::close(void) -{ - writePos(_pos.limit.max); -} +void Finger::close(void) { writePos(_pos.limit.max); } // toggle finger between open/closed -void Finger::open_close(void) -{ - open_close(!readDir()); -} +void Finger::open_close(void) { open_close(!readDir()); } -// set finger to open/close -void Finger::open_close(boolean dir) -{ - if (dir == OPEN) - { - open(); - } - else if (dir == CLOSE) - { - close(); - } +// set finger to open/close +void Finger::open_close(boolean dir) { + if (dir == OPEN) { + open(); + } else if (dir == CLOSE) { + close(); + } } - // SPEED // write a target speed to the finger -void Finger::writeSpeed(int value) -{ - _speed.targ = constrain((int16_t)value, -_PWM.limit.max, _PWM.limit.max); // store vectorised speed - _PWM.targ = _speed.targ; +void Finger::writeSpeed(int value) { + _speed.targ = constrain((int16_t)value, -_PWM.limit.max, + _PWM.limit.max); // store vectorised speed + _PWM.targ = _speed.targ; -#if defined(USE_PID) - _PID.setLimits(-abs(_PWM.targ), abs(_PWM.targ)); +#if defined(USE_PID) + _PID.setLimits(-abs(_PWM.targ), abs(_PWM.targ)); #endif } // return the current movement speed -float Finger::readSpeed(void) -{ - //pauseInterrupt(); // pause 'control()' interrupt to prevent a race condition +float Finger::readSpeed(void) { + // pauseInterrupt(); // pause 'control()' interrupt + // to prevent a race condition - calcVel(); // read finger speed (ADC/per) - _velBuff.write(_speed.raw); - _speed.prev = _speed.curr; - _speed.curr = _velBuff.readMean(); + calcVel(); // read finger speed (ADC/per) + _velBuff.write(_speed.raw); + _speed.prev = _speed.curr; + _speed.curr = _velBuff.readMean(); - //resumeInterrupt(); // resume 'control()' interrupt to prevent a race condition + // resumeInterrupt(); // resume 'control()' interrupt to + // prevent a race condition - return _speed.raw; + return _speed.raw; } // return the target movement speed -float Finger::readTargetSpeed(void) -{ - return _speed.targ; -} +float Finger::readTargetSpeed(void) { return _speed.targ; } // return the current speed being written to the finger -int Finger::readPWM(void) -{ - return _PWM.curr; -} +int Finger::readPWM(void) { return _PWM.curr; } // return the target speed -int Finger::readTargetPWM(void) -{ - return _PWM.targ; -} +int Finger::readTargetPWM(void) { return _PWM.targ; } #ifdef FORCE_SENSE // FORCE // return the current force value. If force sense is disabled, return blank (-1) -float Finger::readForce(void) -{ - // if force sense is not enabled, return - if (!_forceSnsEn) - { - return (-1); - } - else - { - return convertADCToForce(readCurrent()); - } +float Finger::readForce(void) { + // if force sense is not enabled, return + if (!_forceSnsEn) { + return (-1); + } else { + return convertADCToForce(readCurrent()); + } } // return the latest force sense ADC value -uint16_t Finger::readCurrent(void) -{ - // if force sense is not enabled, return - if (!_forceSnsEn) - { - return 0; - } - else - { - return _IBuff.readMean(); - } +uint16_t Finger::readCurrent(void) { + // if force sense is not enabled, return + if (!_forceSnsEn) { + return 0; + } else { + return _IBuff.readMean(); + } } // return true if the force limit has been reached -bool Finger::reachedForceLimit(void) -{ - return _force.limit.reached; -} +bool Finger::reachedForceLimit(void) { return _force.limit.reached; } // read the current force and discount the current spike -void Finger::calcCurrentSns(void) -{ - // if still within current spike period - if (_currentSpikeTimer.started() && !_currentSpikeTimer.finished()) - { - _force.curr = 0; - } - else - { - noInterrupts(); - _force.curr = analogRead(_pin.forceSns); - interrupts(); - } - - // store the read current into a buffer for smoothing - _IBuff.write((uint16_t)_force.curr); +void Finger::calcCurrentSns(void) { + // if still within current spike period + if (_currentSpikeTimer.started() && !_currentSpikeTimer.finished()) { + _force.curr = 0; + } else { + noInterrupts(); + _force.curr = analogRead(_pin.forceSns); + interrupts(); + } + + // store the read current into a buffer for smoothing + _IBuff.write((uint16_t)_force.curr); } // convert ADC current sense value to force value (float) -float Finger::convertADCToForce(int ADCVal) -{ - const float m = CURRENT_SENSE_CONST_M; // generated from the equation of the line from the force-current graph - const float c = CURRENT_SENSE_CONST_C; // generated from the equation of the line from the force-current graph - const float k = ADC_VALS_PER_MA_DRAW; // number of ADC values per mA draw of the motor +float Finger::convertADCToForce(int ADCVal) { + const float m = CURRENT_SENSE_CONST_M; // generated from the equation of the + // line from the force-current graph + const float c = CURRENT_SENSE_CONST_C; // generated from the equation of the + // line from the force-current graph + const float k = + ADC_VALS_PER_MA_DRAW; // number of ADC values per mA draw of the motor - float force = (((float)ADCVal / k) - c) / m; // convert ADC value to current draw to output force - force = constrain(force, 0, 1000); // constrain the force value to reasonable limits + float force = (((float)ADCVal / k) - c) / + m; // convert ADC value to current draw to output force + force = constrain(force, 0, + 1000); // constrain the force value to reasonable limits - return force; + return force; } // convert force value to ADC current sense value (int) -int Finger::convertForceToADC(float force) -{ - const float m = CURRENT_SENSE_CONST_M; // generated from the equation of the line from the force-current graph - const float c = CURRENT_SENSE_CONST_C; // generated from the equation of the line from the force-current graph - const float k = ADC_VALS_PER_MA_DRAW; // number of ADC values per mA draw of the motor +int Finger::convertForceToADC(float force) { + const float m = CURRENT_SENSE_CONST_M; // generated from the equation of the + // line from the force-current graph + const float c = CURRENT_SENSE_CONST_C; // generated from the equation of the + // line from the force-current graph + const float k = + ADC_VALS_PER_MA_DRAW; // number of ADC values per mA draw of the motor - int ADCvals = (int)(((m * force) + c) * k); // convert the force to current draw to ADC values - ADCvals = constrain(ADCvals, 0, 1024); // constrain the ADC value to 10-bit ADC limit + int ADCvals = (int)(((m * force) + c) * + k); // convert the force to current draw to ADC values + ADCvals = constrain(ADCvals, 0, + 1024); // constrain the ADC value to 10-bit ADC limit - return ADCvals; + return ADCvals; } -#endif // FORCE_SENSE - +#endif // FORCE_SENSE // STOP/START // stop the motor and hold position -void Finger::stopMotor(void) -{ - // set target position to current position - writePos(readPos()); +void Finger::stopMotor(void) { + // set target position to current position + writePos(readPos()); } - // ENABLE/DISABLE // set motor to be enabled/disabled -void Finger::motorEnable(bool en) -{ - _motorEn = en; - - if (_motorEn) - { - _PID.reset(); - } +void Finger::motorEnable(bool en) { + _motorEn = en; + + if (_motorEn) { + _PID.reset(); + } } // return true if the motor is enabled -bool Finger::enabled(void) -{ - return _motorEn; -} +bool Finger::enabled(void) { return _motorEn; } #ifdef FORCE_SENSE // set force sensing to be enabled/disabled -void Finger::forceSenseEnable(bool en) -{ - _forceSnsEn = en; -} +void Finger::forceSenseEnable(bool en) { _forceSnsEn = en; } #endif // enable timer interrupt for motor control -void Finger::enableInterrupt(void) -{ - _passMotorPtr(&_fingerControlCallback); // attach the finger control function to the timer +void Finger::enableInterrupt(void) { + _passMotorPtr(&_fingerControlCallback); // attach the finger control function + // to the timer - _interruptEn = true; + _interruptEn = true; } // disable timer interrupt for motor control -void Finger::disableInterrupt(void) -{ - _passMotorPtr(NULL); // prevent the interrupt from calling the motor control function - - _interruptEn = false; -} - +void Finger::disableInterrupt(void) { + _passMotorPtr( + NULL); // prevent the interrupt from calling the motor control function + _interruptEn = false; +} // PRINT //// print the current position (no new line) -//void Finger::printPos(void) +// void Finger::printPos(void) //{ // printPos(0); -//} +// } // //// print the current position (new line) -//void Finger::printPos(bool newL) +// void Finger::printPos(bool newL) //{ // MYSERIAL.print("Pos "); // MYSERIAL.print(readPos()); // MYSERIAL.print(" "); // if (newL) // MYSERIAL.print("\n"); -//} +// } // //// print the current position error (no new line) -//void Finger::printPosError(void) +// void Finger::printPosError(void) //{ // printPosError(0); -//} +// } // //// print the current position error (new line) -//void Finger::printPosError(bool newL) +// void Finger::printPosError(bool newL) //{ // MYSERIAL.print("Err "); // MYSERIAL.print(readPosError()); // MYSERIAL.print(" "); // if (newL) // MYSERIAL.print("\n"); -//} +// } // //// print the current direction (no new line) -//void Finger::printDir(void) +// void Finger::printDir(void) //{ // printDir(0); -//} +// } // //// print the current direction (new line) -//void Finger::printDir(bool newL) +// void Finger::printDir(bool newL) //{ // const char* _dirString[2] = { "OPEN","CLOSE" }; // direction string // @@ -586,42 +505,43 @@ void Finger::disableInterrupt(void) // MYSERIAL.print(" "); // if (newL) // MYSERIAL.print("\n"); -//} +// } // //// print whether the target position has been reached (no new line) -//void Finger::printReached(void) +// void Finger::printReached(void) //{ // printReached(0); -//} +// } // //// print whether the target position has been reached (new line) -//void Finger::printReached(bool newL) +// void Finger::printReached(bool newL) //{ // MYSERIAL.print("Reached "); // MYSERIAL.print(reachedPos()); // MYSERIAL.print(" "); // if (newL) // MYSERIAL.print("\n"); -//} +// } // //// print the current speed (no new line) -//void Finger::printSpeed(void) +// void Finger::printSpeed(void) //{ // printSpeed(0); -//} +// } // //// print the current speed (new line) -//void Finger::printSpeed(bool newL) +// void Finger::printSpeed(bool newL) //{ // MYSERIAL.print("Speed "); // MYSERIAL.print(readSpeed()); // MYSERIAL.print(" "); // if (newL) // MYSERIAL.print("\n"); -//} +// } // -//// print current position, direction, speed and whether the target position has been reached -//void Finger::printDetails(void) +//// print current position, direction, speed and whether the target position +/// has been reached +// void Finger::printDetails(void) //{ // MYSERIAL.print("Finger "); // MYSERIAL.print(_fingerIndex); @@ -631,10 +551,10 @@ void Finger::disableInterrupt(void) // printSpeed(); // printReached(true); // print new line after // -//} +// } // //// print finger number, pins and limits -//void Finger::printConfig(void) +// void Finger::printConfig(void) //{ // MYSERIAL.print("Finger"); // MYSERIAL.print(_fingerIndex); @@ -645,10 +565,10 @@ void Finger::disableInterrupt(void) // MYSERIAL.print(_pin.dir[1]); // MYSERIAL.print(" \tposSense: "); // MYSERIAL.print(_pin.posSns); -//#ifdef FORCE_SENSE +// #ifdef FORCE_SENSE // MYSERIAL.print(" \tforceSense: "); // MYSERIAL.print(_pin.forceSns); -//#endif +// #endif // MYSERIAL.print("\tinvert: "); // MYSERIAL.println(_invert); // @@ -662,380 +582,358 @@ void Finger::disableInterrupt(void) // MYSERIAL.print("\tMaxSpeed: "); // MYSERIAL.println(_PWM.limit.max); // -//#ifdef FORCE_SENSE +// #ifdef FORCE_SENSE // MYSERIAL.print("MinForce: "); // MYSERIAL.print(_force.limit.min); // MYSERIAL.print("\tMaxForce: "); // MYSERIAL.println(_force.limit.max); -//#endif +// #endif // // // MYSERIAL.print("\n"); -//} - +// } // -void Finger::control(void) -{ - // read finger position (reads to _pos.curr internally() ) - readPos(); - - // read finger speed (including calculating the vel) - readSpeed(); - -//#ifdef FORCE_SENSE -// // if force sense is enabled, run force control -// if (_forceSnsEn) -// { -// forceController(); -// } -// -//#endif - +void Finger::control(void) { + // read finger position (reads to _pos.curr internally() ) + readPos(); + + // read finger speed (including calculating the vel) + readSpeed(); + + // #ifdef FORCE_SENSE + // // if force sense is enabled, run force control + // if (_forceSnsEn) + // { + // forceController(); + // } + // + // #endif #ifdef FORCE_SENSE - if (_forceSnsEn && _motorEn) - { - stallDetection(); - } + if (_forceSnsEn && _motorEn) { + stallDetection(); + } #endif - - positionController(); // run the position controller + positionController(); // run the position controller } - - - - - - - - - ////////////////////////////// Private Methods ////////////////////////////// #if defined(USE_PID) // position controller (using PID) -void Finger::positionController(void) -{ - // run pos PID controller to calculate target speed - _speed.targ = _PID.run(_pos.targ, _pos.curr); +void Finger::positionController(void) { + // run pos PID controller to calculate target speed + _speed.targ = _PID.run(_pos.targ, _pos.curr); - motorControl(_speed.targ); + motorControl(_speed.targ); } #else // position controller (using custom P controller) -// controls motor PWM values based on current and target position using a proportional controller (triggered by interrupt) -// total duration = 439us, therefore max freq = 2kHz. We use 200Hz (5ms), where 0.5ms = motor control, 4.5ms = program runtime -void Finger::positionController(void) -{ - signed int motorSpeed = 0; // used to calculate the motor speed as a vector (±255) - float m; // the proportional gradient - signed int vectorise = 1; // changes the sign '±' of the value +// controls motor PWM values based on current and target position using a +// proportional controller (triggered by interrupt) total duration = 439us, +// therefore max freq = 2kHz. We use 200Hz (5ms), where 0.5ms = motor +// control, 4.5ms = program runtime +void Finger::positionController(void) { + signed int motorSpeed = + 0; // used to calculate the motor speed as a vector (±255) + float m; // the proportional gradient + signed int vectorise = 1; // changes the sign '±' of the value #if defined(ARDUINO_AVR_MEGA2560) - int proportionalOffset = 300; - signed int motorStopOffset = 25; + int proportionalOffset = 300; + signed int motorStopOffset = 25; #elif defined(ARDUINO_ARCH_SAMD) - int proportionalOffset = 300; - signed int motorStopOffset = 20; + int proportionalOffset = 300; + signed int motorStopOffset = 20; #endif - // calc positional error - _pos.error = (signed int)(_pos.targ - _pos.curr); - - // speed/position line gradient - m = (float)(((float)_PWM.targ) / ((float)proportionalOffset)); - - // change the ± sign on the motorSpeed depending on required direction - if (_pos.error >= 0) - vectorise = -1; - - // constrain speed to posError/speed graph - if (abs(_pos.error) < motorStopOffset) // motor dead zone - { - motorSpeed = 0; - } - else if (_pos.error > (signed int)(proportionalOffset + motorStopOffset)) // set to max speed depending on direction - { - motorSpeed = _PWM.targ; - } - else if (_pos.error < -(signed int)(proportionalOffset + motorStopOffset)) // set to -max speed depending on direction - { - motorSpeed = -_PWM.targ; - } - else if (abs(_pos.error) <= (proportionalOffset + motorStopOffset)) // proportional control - { - motorSpeed = (m * (_pos.error + (motorStopOffset * vectorise))) - (_PWM.limit.min * vectorise); - } - - // constrain speed to limits - motorSpeed = constrain(motorSpeed, -((signed int)_PWM.limit.max), (signed int)_PWM.limit.max); - - // if motor disabled, set speed to 0 - if (!_motorEn) - motorSpeed = 0; - - // send speed to motors - motorControl(motorSpeed); // 15us + // calc positional error + _pos.error = (signed int)(_pos.targ - _pos.curr); + + // speed/position line gradient + m = (float)(((float)_PWM.targ) / ((float)proportionalOffset)); + + // change the ± sign on the motorSpeed depending on required direction + if (_pos.error >= 0) + vectorise = -1; + + // constrain speed to posError/speed graph + if (abs(_pos.error) < motorStopOffset) // motor dead zone + { + motorSpeed = 0; + } else if (_pos.error > + (signed int)(proportionalOffset + + motorStopOffset)) // set to max speed depending on + // direction + { + motorSpeed = _PWM.targ; + } else if (_pos.error < + -(signed int)(proportionalOffset + + motorStopOffset)) // set to -max speed depending on + // direction + { + motorSpeed = -_PWM.targ; + } else if (abs(_pos.error) <= + (proportionalOffset + motorStopOffset)) // proportional control + { + motorSpeed = (m * (_pos.error + (motorStopOffset * vectorise))) - + (_PWM.limit.min * vectorise); + } + + // constrain speed to limits + motorSpeed = constrain(motorSpeed, -((signed int)_PWM.limit.max), + (signed int)_PWM.limit.max); + + // if motor disabled, set speed to 0 + if (!_motorEn) + motorSpeed = 0; + + // send speed to motors + motorControl(motorSpeed); // 15us } #endif -// split the vectorised motor speed into direction and speed values and write to the motor -void Finger::motorControl(signed int speed) -{ - bool dir = OPEN; - - // if the motor is disabled, set the speed to 0 - if (!_motorEn) - { - speed = 0; - } - - // determine direction and invert speed if necessary - if (speed > 0) - { - dir = OPEN; - } - else if (speed < 0) - { - dir = CLOSE; - speed = -speed; - } - - // if speed is not within min/max, set to 0 - speed = window(speed, 0, _PWM.limit.max); - - // store previous and current speed - _PWM.prev = _PWM.curr; - _PWM.curr = (uint8_t)speed; - - // store previous and current direction - _dir.prev = _dir.curr; - _dir.curr = dir; +// split the vectorised motor speed into direction and speed values and write to +// the motor +void Finger::motorControl(signed int speed) { + bool dir = OPEN; -#ifdef FORCE_SENSE - // if motor has just started a movement or direction has changed or the PWM speed has changed by > 1/3 of the max - if (((_PWM.prev == 0) && (_PWM.curr > 0)) || (_dir.curr != _dir.prev) || ((_PWM.curr - _PWM.prev) > (MAX_FINGER_PWM / 3))) - { - // start current spike timer - _currentSpikeTimer.start(CURR_SPIKE_DUR_US); - } -#endif + // if the motor is disabled, set the speed to 0 + if (!_motorEn) { + speed = 0; + } - // invert finger direction if enabled - if (_invert) - { - dir = !dir; - } + // determine direction and invert speed if necessary + if (speed > 0) { + dir = OPEN; + } else if (speed < 0) { + dir = CLOSE; + speed = -speed; + } + // if speed is not within min/max, set to 0 + speed = window(speed, 0, _PWM.limit.max); + // store previous and current speed + _PWM.prev = _PWM.curr; + _PWM.curr = (uint8_t)speed; - // write the speed to the motors - analogWrite(_pin.dir[dir], speed); //write motor speed to one direction pin - analogWrite(_pin.dir[!dir], 0); //write 0 to other direction pin - -} - + // store previous and current direction + _dir.prev = _dir.curr; + _dir.curr = dir; #ifdef FORCE_SENSE - -bool Finger::stallDetection(void) -{ - // if the motor is stopped and drawing a lot of current, set the targ pos to be the curr pos - if ((abs(_speed.curr) <= 1) && (_IBuff.readMean() >= STALL_CURRENT_THRESH)) - { - if (!_motorStallTimer.started()) - { - _motorStallTimer.start(MAX_STALL_TIME_MS); - } - else if (_motorStallTimer.finished()) // if the motor has been stalled for the MAX_STALL_TIME_MS - { - _motorStallTimer.stop(); - - // hold the motor at the current pos - _pos.targ = _pos.curr; - _pos.error = (_pos.targ - _pos.curr); - -//#define MYSERIAL SerialUSB -// MYSERIAL.print("\nF"); -// MYSERIAL.print(_fingerIndex); -// MYSERIAL.print(": stall detected. Setting to "); -// MYSERIAL.println(_pos.targ); -// MYSERIAL.print("\n"); - - return true; - } - } - else - { - _motorStallTimer.stop(); - } - - return false; -} - -// stop the finger if the force limit is reached, or move to reach a target force -void Finger::forceController(void) -{ - // if force sense is not enabled, return - if (!_forceSnsEn) - { - return; - } - - US_NB_DELAY forceTimer; // timer used to detect if the force is being sustained or is just a small peak - const int force_pos_increment = 5; // rate at which the actuator 'searches' for the target force - - // clear force limit flag - _force.limit.reached = false; - - // if force limit is reached - if (_force.curr > _force.limit.max) - { - // store current movement direction so that stopping the motor is not perceived as a direction change - bool tempDir = _dir.curr; - - // stop the motor by setting the target position to the current position - writePos(readPos()); - - // restore the previous direction, as it may have been changed by stopping the motor - _dir.curr = tempDir; - - _force.limit.reached = true; - } - - //// if a target force is set, move until force has been reached - //if (_force.targ != 0) - //{ - // // if target force limit has been reached - // if (_force.curr > _force.targ) - // { - // MYSERIAL.print("F"); - // MYSERIAL.print(_fingerIndex); - // MYSERIAL.println(" target force"); - - // _force.limit.reached = true; - // _force.targ = 0; - // } - // // if target force has not been reached - // else - // { - // // move pos in either the direction determined by _targForceDir in steps of size force_pos_increment - // movePos(-force_pos_increment + (_targForceDir * (2 * force_pos_increment))); - // } - //} -} - + // if motor has just started a movement or direction has changed or the PWM + // speed has changed by > 1/3 of the max + if (((_PWM.prev == 0) && (_PWM.curr > 0)) || (_dir.curr != _dir.prev) || + ((_PWM.curr - _PWM.prev) > (MAX_FINGER_PWM / 3))) { + // start current spike timer + _currentSpikeTimer.start(CURR_SPIKE_DUR_US); + } #endif + // invert finger direction if enabled + if (_invert) { + dir = !dir; + } - -// calculate the velocity of the motor -void Finger::calcVel(void) -{ - // if the duration timer is currently running - //if (_velTimer.started()) - //if (_velTimer.now() > 100000) // 100ms - if (_velTimer.now() > 50000) // 50ms - { - // get the time and position at the current time - double duration = _velTimer.now(); // us. time since last vel calc - double pos = _pos.curr; // store current pos to prevent race condition - double dist = (pos - _pos.prev); // calculate distance moved (using stored pos) - - if (abs(dist) > 0) // if there has been movement - { - _speed.raw = (dist * (double)100000) / duration; // calc vel in ADC ticks per s? - } - else // else if 0 distance has moved - { - _speed.raw = 0.0; // set the velocity to 0 - } - - _velTimer.start(); // restart timer - _pos.prev = pos; // save previous pos - } - else if (!_velTimer.started()) // if the vel timer is not currently running - { - _velTimer.start(); // start the vel timer - _speed.raw = 0; // clear the velocity - } + // write the speed to the motors + analogWrite(_pin.dir[dir], speed); // write motor speed to one direction pin + analogWrite(_pin.dir[!dir], 0); // write 0 to other direction pin } -////////////////////////////// END OF FINGER CLASS ////////////////////////////// - +#ifdef FORCE_SENSE -// runs the control() function of a Finger instance at each call by the timer interrupt -void _fingerControlCallback(void) -{ - static int i = 0; +bool Finger::stallDetection(void) { + // if the motor is stopped and drawing a lot of current, set the targ pos to + // be the curr pos + if ((abs(_speed.curr) <= 1) && (_IBuff.readMean() >= STALL_CURRENT_THRESH)) { + if (!_motorStallTimer.started()) { + _motorStallTimer.start(MAX_STALL_TIME_MS); + } else if (_motorStallTimer.finished()) // if the motor has been stalled for + // the MAX_STALL_TIME_MS + { + _motorStallTimer.stop(); + + // hold the motor at the current pos + _pos.targ = _pos.curr; + _pos.error = (_pos.targ - _pos.curr); + + // #define MYSERIAL SerialUSB + // MYSERIAL.print("\nF"); + // MYSERIAL.print(_fingerIndex); + // MYSERIAL.print(": stall detected. Setting to "); + // MYSERIAL.println(_pos.targ); + // MYSERIAL.print("\n"); + + return true; + } + } else { + _motorStallTimer.stop(); + } + + return false; +} + +// stop the finger if the force limit is reached, or move to reach a target +// force +void Finger::forceController(void) { + // if force sense is not enabled, return + if (!_forceSnsEn) { + return; + } + + US_NB_DELAY forceTimer; // timer used to detect if the force is being + // sustained or is just a small peak + const int force_pos_increment = + 5; // rate at which the actuator 'searches' for the target force + + // clear force limit flag + _force.limit.reached = false; + + // if force limit is reached + if (_force.curr > _force.limit.max) { + // store current movement direction so that stopping the motor is not + // perceived as a direction change + bool tempDir = _dir.curr; + + // stop the motor by setting the target position to the current position + writePos(readPos()); + + // restore the previous direction, as it may have been changed by stopping + // the motor + _dir.curr = tempDir; + + _force.limit.reached = true; + } + + //// if a target force is set, move until force has been reached + // if (_force.targ != 0) + //{ + // // if target force limit has been reached + // if (_force.curr > _force.targ) + // { + // MYSERIAL.print("F"); + // MYSERIAL.print(_fingerIndex); + // MYSERIAL.println(" target force"); + + // _force.limit.reached = true; + // _force.targ = 0; + // } + // // if target force has not been reached + // else + // { + // // move pos in either the direction determined by _targForceDir + // in steps of size force_pos_increment + // movePos(-force_pos_increment + + //(_targForceDir * (2 * force_pos_increment))); + // } + //} +} - if (fingerISRList[i] != NULL) - { - fingerISRList[i]->control(); // run the control() member function of each attached Finger instance - } +#endif - i++; - if (i > _TotalAttachedFingers) - { - i = 0; - } +// calculate the velocity of the motor +void Finger::calcVel(void) { + // if the duration timer is currently running + // if (_velTimer.started()) + // if (_velTimer.now() > 100000) // 100ms + if (_velTimer.now() > 50000) // 50ms + { + // get the time and position at the current time + double duration = _velTimer.now(); // us. time since last vel calc + double pos = _pos.curr; // store current pos to prevent race condition + double dist = + (pos - _pos.prev); // calculate distance moved (using stored pos) + + if (abs(dist) > 0) // if there has been movement + { + _speed.raw = + (dist * (double)100000) / duration; // calc vel in ADC ticks per s? + } else // else if 0 distance has moved + { + _speed.raw = 0.0; // set the velocity to 0 + } + + _velTimer.start(); // restart timer + _pos.prev = pos; // save previous pos + } else if (!_velTimer.started()) // if the vel timer is not currently running + { + _velTimer.start(); // start the vel timer + _speed.raw = 0; // clear the velocity + } +} + +////////////////////////////// END OF FINGER CLASS +///////////////////////////////// + +// runs the control() function of a Finger instance at each call by the timer +// interrupt +void _fingerControlCallback(void) { + static int i = 0; + + if (fingerISRList[i] != NULL) { + fingerISRList[i]->control(); // run the control() member function of each + // attached Finger instance + } + + i++; + if (i > _TotalAttachedFingers) { + i = 0; + } } - #ifdef FORCE_SENSE // runs the calcCurrentSns() function of a Finger instance at each call -void _currentSenseCallback(void) -{ - static int i = 0; - - if (fingerISRList[i] != NULL) - { - fingerISRList[i]->calcCurrentSns(); // run the calcCurrentSns() member function of each attached Finger instance - } - - i++; - if (i > _TotalAttachedFingers) - { - i = 0; - } +void _currentSenseCallback(void) { + static int i = 0; + + if (fingerISRList[i] != NULL) { + fingerISRList[i] + ->calcCurrentSns(); // run the calcCurrentSns() member function of each + // attached Finger instance + } + + i++; + if (i > _TotalAttachedFingers) { + i = 0; + } } #endif #ifdef ARDUINO_AVR_MEGA2560 // change the PWM timer frequency to be out of the audible range -void setPWMFreq(uint8_t pin, uint8_t value) -{ - uint8_t timerNum; - - timerNum = PWM_pin_to_timer(pin); - switch (timerNum) - { - case 0: - //TCCR0B = (TCCR0B & 0xF8) | value; // CHANGING THIS TIMER FREQUENCY WILL BREAK delay() - break; - case 1: - TCCR1B = (TCCR1B & 0xF8) | value; - break; - case 2: - TCCR2B = (TCCR2B & 0xF8) | value; - break; - case 3: - TCCR3B = (TCCR3B & 0xF8) | value; - break; - case 4: - TCCR4B = (TCCR4B & 0xF8) | value; - break; - case 5: - TCCR5B = (TCCR5B & 0xF8) | value; - break; - default: - break; - } +void setPWMFreq(uint8_t pin, uint8_t value) { + uint8_t timerNum; + + timerNum = PWM_pin_to_timer(pin); + switch (timerNum) { + case 0: + // TCCR0B = (TCCR0B & 0xF8) | value; // CHANGING THIS TIMER FREQUENCY WILL + // BREAK delay() + break; + case 1: + TCCR1B = (TCCR1B & 0xF8) | value; + break; + case 2: + TCCR2B = (TCCR2B & 0xF8) | value; + break; + case 3: + TCCR3B = (TCCR3B & 0xF8) | value; + break; + case 4: + TCCR4B = (TCCR4B & 0xF8) | value; + break; + case 5: + TCCR5B = (TCCR5B & 0xF8) | value; + break; + default: + break; + } } #endif \ No newline at end of file diff --git a/src/FingerLib.h b/src/FingerLib.h index 6119138..33c996a 100644 --- a/src/FingerLib.h +++ b/src/FingerLib.h @@ -4,28 +4,28 @@ * Created: 02/11/2015 11:12:48 * Author: Olly McBride * - * This work is licensed under the Creative Commons Attribution 4.0 International License. - * To view a copy of this license, visit http://creativecommons.org/licenses/by/4.0/. + * This work is licensed under the Creative Commons Attribution 4.0 + * International License. To view a copy of this license, visit + * http://creativecommons.org/licenses/by/4.0/. * - */ + * Modified by David Kennedy S. Araujo (software@davidkennedy.dev) to clean code + * + */ #ifndef FINGERLIB_H_ #define FINGERLIB_H_ -#include #include - +#include // CHANGE SETTINGS -// Uncomment out the following to use PID pos control instead of custom P control -#define USE_PID - -// Uncomment the following to enable force/current sensing (Arduino Zero & Chestnut PCB Only) (Automatically enabled when using Chestnut PCB) -//#define FORCE_SENSE - - - +// Uncomment out the following to use PID pos control instead of custom P +// control +#define USE_PID +// Uncomment the following to enable force/current sensing (Arduino Zero & +// Chestnut PCB Only) (Automatically enabled when using Chestnut PCB) +// #define FORCE_SENSE // GENERIC LIBRARIES #include "buffer/CircleBuff.h" @@ -33,279 +33,318 @@ // BOARD SPECIFIC LIBRARIES #if defined(ARDUINO_AVR_MEGA2560) || defined(ARDUINO_AVR_UNO) - //#define MYSERIAL Serial - #include "timers/avr_FingerTimer.h" +// #define MYSERIAL Serial +#include "timers/avr_FingerTimer.h" #elif defined(ARDUINO_ARCH_SAMD) - //#define MYSERIAL SerialUSB - #include "timers/samd_FingerTimer.h" - // Uncomment the following to enable force/current sensing (Arduino Zero & Chestnut PCB Only) - #define FORCE_SENSE +// #define MYSERIAL SerialUSB +#include "timers/samd_FingerTimer.h" +// Uncomment the following to enable force/current sensing (Arduino Zero & +// Chestnut PCB Only) +#define FORCE_SENSE #else - #error FingerLib only supports boards using an Arduino Mega 2560, Arduino UNO, Arduino Zero, Almond PCB or Chestnut PCB. +#error FingerLib only supports boards using an Arduino Mega 2560, Arduino UNO, Arduino Zero, Almond PCB or Chestnut PCB. #endif // SETTING SPECIFIC LIBRARIES -#if defined(USE_PID) - #include "pid/pid_controller.h" -#endif +#if defined(USE_PID) +#include "pid/pid_controller.h" +#endif #if defined(FORCE_SENSE) - #if defined(ARDUINO_ARCH_SAMD) - #include "current_sense/samd_CurrentSense.h" - #else - #error Force sensing is only available on the Arduino Zero or Chestnut PCB. - #endif +#if defined(ARDUINO_ARCH_SAMD) +#include "current_sense/samd_CurrentSense.h" +#else +#error Force sensing is only available on the Arduino Zero or Chestnut PCB. +#endif #endif - - - - // BOOLEANS -#define CLOSE 0 // movement direction -#define OPEN 1 // movement direction -#define RIGHT 1 // hand type -#define LEFT 2 // hand type +#define CLOSE 0 // movement direction +#define OPEN 1 // movement direction +#define RIGHT 1 // hand type +#define LEFT 2 // hand type // FINGERS -#define MAX_FINGERS 6 // maximum number of fingers -#define F0 0 -#define F1 1 -#define F2 2 -#define F3 3 -#define F4 4 -#define F5 5 +#define MAX_FINGERS 6 // maximum number of fingers +#define F0 0 +#define F1 1 +#define F2 2 +#define F3 3 +#define F4 4 +#define F5 5 // BOARD SETTINGS -#define MAX_ADC_RES 1023 // maximum resolution of the ADC for pos sensing +#define MAX_ADC_RES 1023 // maximum resolution of the ADC for pos sensing // SPEED -#define MAX_FINGER_PWM 255 // PWM. maximum motor speed -#define MIN_FINGER_PWM 0 // PWM. minimum motor speed -#define OFF_FINGER_PWM 0 // PWM. zero motor speed -#define MAX_FINGER_SPEED MAX_FINGER_PWM // backwards compatibility -#define MIN_FINGER_SPEED MIN_FINGER_PWM // backwards compatibility +#define MAX_FINGER_PWM 255 // PWM. maximum motor speed +#define MIN_FINGER_PWM 0 // PWM. minimum motor speed +#define OFF_FINGER_PWM 0 // PWM. zero motor speed +#define MAX_FINGER_SPEED MAX_FINGER_PWM // backwards compatibility +#define MIN_FINGER_SPEED MIN_FINGER_PWM // backwards compatibility -#define VEL_BUFF_SIZE 16 // number of values to store in velocity smoothing buffer (must be a power of 2) +#define VEL_BUFF_SIZE \ + 16 // number of values to store in velocity smoothing buffer (must be a power + // of 2) // POS LIMITS -#define MAX_POS_SENSOR_VAL MAX_ADC_RES // 10 bit ADC -#define MAX_FINGER_POS 973 // maximum motor position -#define MIN_FINGER_POS 50 // minimum motor position - -#define POS_REACHED_TOLERANCE 50 // tolerance for posReached() - -#if defined(FORCE_SENSE) - // STALL DETECTION - #define MAX_STALL_TIME_MS 750 // ms. maximum amount of time the motors can stall for before they are determined to be stalled - #define STALL_CURRENT_THRESH 50 // current threshold, value, above which, if the motor velocity == 0, the motor is deemed stalled - - // CURRENT - #define MAX_CURR_VAL MAX_ADC_RES - #define CURR_SPIKE_DUR_US (double) 50000 // us - duration of peak to discard - #define CURR_SENSE_BUFF_SIZE 64 // number of samples to store in the current sense circle buffer - - // FORCE - // force and current conversion values - #define MOTOR_DRIVER_CURRENT_LIMIT (float) 425 // mA - #define CURRENT_SENSE_SATURATION_VAL (float) 950 // max ADC value - #define ADC_VALS_PER_MA_DRAW (float) (CURRENT_SENSE_SATURATION_VAL/MOTOR_DRIVER_CURRENT_LIMIT) // number of ADC values per mA draw of motor - - // Y = MX + C, where Y = force, X = current draw - #define CURRENT_SENSE_CONST_M (float) 7.4833 // generated from the equation of the line from the force-current graph - #define CURRENT_SENSE_CONST_C (float) 46.444 // generated from the equation of the line from the force-current graph -#endif +#define MAX_POS_SENSOR_VAL MAX_ADC_RES // 10 bit ADC +#define MAX_FINGER_POS 973 // maximum motor position +#define MIN_FINGER_POS 50 // minimum motor position -// MACROS -#define window(amt,low,high) ((amt)<(low)?(0):((amt)>(high)?(0):(amt))) // limit amt to within low and high +#define POS_REACHED_TOLERANCE 50 // tolerance for posReached() +#if defined(FORCE_SENSE) +// STALL DETECTION +#define MAX_STALL_TIME_MS \ + 750 // ms. maximum amount of time the motors can stall for before they are + // determined to be stalled +#define STALL_CURRENT_THRESH \ + 50 // current threshold, value, above which, if the motor velocity == 0, the + // motor is deemed stalled + +// CURRENT +#define MAX_CURR_VAL MAX_ADC_RES +#define CURR_SPIKE_DUR_US (double)50000 // us - duration of peak to discard +#define CURR_SENSE_BUFF_SIZE \ + 64 // number of samples to store in the current sense circle buffer + +// FORCE +// force and current conversion values +#define MOTOR_DRIVER_CURRENT_LIMIT (float)425 // mA +#define CURRENT_SENSE_SATURATION_VAL (float)950 // max ADC value +#define ADC_VALS_PER_MA_DRAW \ + (float)(CURRENT_SENSE_SATURATION_VAL / \ + MOTOR_DRIVER_CURRENT_LIMIT) // number of ADC values per mA draw of + // motor + +// Y = MX + C, where Y = force, X = current draw +#define CURRENT_SENSE_CONST_M \ + (float)7.4833 // generated from the equation of the line from the + // force-current graph +#define CURRENT_SENSE_CONST_C \ + (float)46.444 // generated from the equation of the line from the + // force-current graph +#endif +// MACROS +#define window(amt, low, high) \ + ((amt) < (low) \ + ? (0) \ + : ((amt) > (high) ? (0) : (amt))) // limit amt to within low and high // FINGER PINS -typedef struct _FingerPin -{ - uint8_t dir[2]; - uint8_t posSns; +typedef struct _FingerPin { + uint8_t dir[2]; + uint8_t posSns; #ifdef FORCE_SENSE - uint8_t forceSns; + uint8_t forceSns; #endif } FingerPin; // VECTOR PROPERTIES -typedef struct _LimitProperties -{ - double min; - double max; - bool reached; +typedef struct _LimitProperties { + double min; + double max; + bool reached; } LimitProperties; -typedef struct _VectorProperties -{ - double prev; - double curr; - double targ; - double error; - double raw; - LimitProperties limit; +typedef struct _VectorProperties { + double prev; + double curr; + double targ; + double error; + double raw; + LimitProperties limit; } VectorProperties; - // FINGER CLASS -class Finger -{ - public: - // CONSTRUCTOR - Finger(); - - // INITIALISATION - uint8_t attach(uint8_t dir0, uint8_t dir1, uint8_t posSns); // attach pins to a finger using only position control - uint8_t attach(uint8_t dir0, uint8_t dir1, uint8_t posSns, bool inv); // attach pins to a finger using only position control, but allow the direction to be inverted - uint8_t attach(uint8_t dir0, uint8_t dir1, uint8_t posSns, uint8_t forceSns, bool inv); // attach pins to a finger using position control and force control, and allow the direction to be inverted - void detach(void); // deactivate the finger - bool attached(void); // return true if the current finger is attached and initialised correctly - void invertFingerDir(void); // set the motor to be inverted - - // LIMITS - void setPosLimits(int min, int max); // set the maximum and minimum position limits - void setPWMLimits(int min, int max); // set the maximum and minimum speed limits +class Finger { +public: + // CONSTRUCTOR + Finger(); + + // INITIALISATION + uint8_t + attach(uint8_t dir0, uint8_t dir1, + uint8_t posSns); // attach pins to a finger using only position control + uint8_t attach(uint8_t dir0, uint8_t dir1, uint8_t posSns, + bool inv); // attach pins to a finger using only position + // control, but allow the direction to be inverted + uint8_t + attach(uint8_t dir0, uint8_t dir1, uint8_t posSns, uint8_t forceSns, + bool inv); // attach pins to a finger using position control and force + // control, and allow the direction to be inverted + void detach(void); // deactivate the finger + bool attached(void); // return true if the current finger is attached and + // initialised correctly + void invertFingerDir(void); // set the motor to be inverted + + // LIMITS + void setPosLimits(int min, + int max); // set the maximum and minimum position limits + void setPWMLimits(int min, + int max); // set the maximum and minimum speed limits #ifdef FORCE_SENSE - void setForceLimits(int min, int max); // set the maximum and minimum force limits + void setForceLimits(int min, + int max); // set the maximum and minimum force limits #endif - // POS - void writePos(int value); // write a target position to the finger - void movePos(int value); // write a change in position to the finger - int16_t readPos(void); // return the current position - int16_t readPosError(void); // return the error between the current position and the target position - uint16_t readTargetPos(void); // return the target position - bool reachedPos(void); // returns true if position reached - bool reachedPos(uint16_t posErr); // returns true if position reached - - // DIR - void writeDir(int value); // write a target direction to the finger - uint8_t readDir(void); // return the current direction - void open(void); // open the finger - void close(void); // close the finger - void open_close(void); // toggle finger between open/closed - void open_close(boolean dir); // set finger to open/close - - // SPEED - void writeSpeed(int value); // write a target speed to the finger - float readSpeed(void); // return the current movement speed of the finger - float readTargetSpeed(void); // return the target movement speed - int readPWM(void); // return the current speed being written to the motor (PWM) - int readTargetPWM(void); // return the target motor speed (PWM) + // POS + void writePos(int value); // write a target position to the finger + void movePos(int value); // write a change in position to the finger + int16_t readPos(void); // return the current position + int16_t readPosError(void); // return the error between the current position + // and the target position + uint16_t readTargetPos(void); // return the target position + bool reachedPos(void); // returns true if position reached + bool reachedPos(uint16_t posErr); // returns true if position reached + + // DIR + void writeDir(int value); // write a target direction to the finger + uint8_t readDir(void); // return the current direction + void open(void); // open the finger + void close(void); // close the finger + void open_close(void); // toggle finger between open/closed + void open_close(boolean dir); // set finger to open/close + + // SPEED + void writeSpeed(int value); // write a target speed to the finger + float readSpeed(void); // return the current movement speed of the finger + float readTargetSpeed(void); // return the target movement speed + int readPWM( + void); // return the current speed being written to the motor (PWM) + int readTargetPWM(void); // return the target motor speed (PWM) #ifdef FORCE_SENSE - // FORCE - float readForce(void); // return the current force value. If force sense is not enabled, return blank (-1) - uint16_t readCurrent(void); // return the latest force sense ADC value - bool reachedForceLimit(void); // return true if the force limit has been reached - float convertADCToForce(int ADCVal); // convert ADC current sense value to force value (float) - int convertForceToADC(float force); // convert force value to ADC current sense value (int) + // FORCE + float readForce(void); // return the current force value. If force sense is + // not enabled, return blank (-1) + uint16_t readCurrent(void); // return the latest force sense ADC value + bool + reachedForceLimit(void); // return true if the force limit has been reached + float convertADCToForce( + int ADCVal); // convert ADC current sense value to force value (float) + int convertForceToADC( + float force); // convert force value to ADC current sense value (int) #endif - // STOP/START - void stopMotor(void); // stop the motor and hold position + // STOP/START + void stopMotor(void); // stop the motor and hold position - // ENABLE/DISABLE - void motorEnable(bool en); // set motor to be enabled/disabled - bool enabled(void); // return true if the motor is enabled + // ENABLE/DISABLE + void motorEnable(bool en); // set motor to be enabled/disabled + bool enabled(void); // return true if the motor is enabled #ifdef FORCE_SENSE - void forceSenseEnable(bool en); // set force sensing to be enabled/disabled + void forceSenseEnable(bool en); // set force sensing to be enabled/disabled #endif - void enableInterrupt(void); // enable timer interrupt for motor control - void disableInterrupt(void); // disable timer interrupt for motor control - - - - //// PRINT - //void printPos(void); // print the current position (no new line) - //void printPos(bool newL); // print the current position (new line) - //void printPosError(void); // print the current position error (no new line) - //void printPosError(bool newL); // print the current position error (new line) - - //void printDir(void); // print the current direction (no new line) - //void printDir(bool newL); // print the current direction (new line) - //void printReached(void); // print whether the target position has been reached (no new line) - //void printReached(bool newL); // print whether the target position has been reached (new line) - - //void printSpeed(void); // print the current speed (no new line) - //void printSpeed(bool newL); // print the current speed (new line) - // - //void printDetails(void); // print current position, direction, speed and whether the target position has been reached - //void printConfig(void); // print finger number, pins and limits - - - // CONTROL - void control(void); // run position (and force) controller (called via interrupt) + void enableInterrupt(void); // enable timer interrupt for motor control + void disableInterrupt(void); // disable timer interrupt for motor control + + //// PRINT + // void printPos(void); // print the current + // position (no new line) void printPos(bool newL); // print + // the current position (new line) void printPosError(void); + // // print the current position error (no new line) void printPosError(bool + // newL); // print the current position error (new line) + + // void printDir(void); // print the current + // direction (no new line) void printDir(bool newL); // print + // the current direction (new line) void printReached(void); + // // print whether the target position has been reached (no new line) void + // printReached(bool newL); // print whether the target position has + // been reached (new line) + + // void printSpeed(void); // print the current speed (no + // new line) void printSpeed(bool newL); // print the current + // speed (new line) + // + // void printDetails(void); // print current position, + // direction, speed and whether the target position has been reached void + // printConfig(void); // print finger number, pins and limits + + // CONTROL + void + control(void); // run position (and force) controller (called via interrupt) #ifdef FORCE_SENSE - void calcCurrentSns(void); // read the current force and discount the current spike (called via interrupt) + void calcCurrentSns(void); // read the current force and discount the current + // spike (called via interrupt) #endif - private: - +private: #ifdef USE_PID - // PID CONTROLLERS - PID_CONTROLLER _PID; + // PID CONTROLLERS + PID_CONTROLLER _PID; #endif - // BUFFERS - CIRCLE_BUFFER _velBuff; // velocity buffer + // BUFFERS + CIRCLE_BUFFER _velBuff; // velocity buffer #ifdef FORCE_SENSE - CIRCLE_BUFFER _IBuff; // current buffer + CIRCLE_BUFFER _IBuff; // current buffer #endif - // TIMERS - US_NB_TIMER _velTimer; // timer used to calculate the velocity + // TIMERS + US_NB_TIMER _velTimer; // timer used to calculate the velocity #ifdef FORCE_SENSE - US_NB_DELAY _currentSpikeTimer; // current spike rejection timer - MS_NB_DELAY _motorStallTimer; + US_NB_DELAY _currentSpikeTimer; // current spike rejection timer + MS_NB_DELAY _motorStallTimer; #endif - uint8_t _fingerIndex; // current finger number - FingerPin _pin; // finger pin struct (dir, pos, force) + uint8_t _fingerIndex; // current finger number + FingerPin _pin; // finger pin struct (dir, pos, force) - // PROPERTIES - VectorProperties _pos; // position properties (prev, curr, targ, error, limit) - VectorProperties _dir; // direction properties (curr) - VectorProperties _speed; // movement speed properties (prev, curr, targ, error, limit) - VectorProperties _PWM; // PWM speed properties (prev, curr, targ, error, limit) + // PROPERTIES + VectorProperties _pos; // position properties (prev, curr, targ, error, limit) + VectorProperties _dir; // direction properties (curr) + VectorProperties + _speed; // movement speed properties (prev, curr, targ, error, limit) + VectorProperties + _PWM; // PWM speed properties (prev, curr, targ, error, limit) #ifdef FORCE_SENSE - VectorProperties _force; // force properties (prev, curr, targ, error, limit) (all stored as ADC values instead of N) + VectorProperties _force; // force properties (prev, curr, targ, error, limit) + // (all stored as ADC values instead of N) #endif - // ENABLE FLAGS - bool _isActive = false; // flag to indicate if finger is enabled - bool _invert; // finger inversion flag - bool _motorEn; // motor enable flag - bool _interruptEn; // flag to set whether to use the timer interrupt for motor control + // ENABLE FLAGS + bool _isActive = false; // flag to indicate if finger is enabled + bool _invert; // finger inversion flag + bool _motorEn; // motor enable flag + bool _interruptEn; // flag to set whether to use the timer interrupt for motor + // control #ifdef FORCE_SENSE - bool _forceSnsEn = false; // force sense is enable flag + bool _forceSnsEn = false; // force sense is enable flag #endif - // CONTROLLERS - void positionController(void); // position controller (either PID or custom P) - void motorControl(int speed); // split the vectorised motor speed into direction and speed values and write to the motor + // CONTROLLERS + void positionController(void); // position controller (either PID or custom P) + void + motorControl(int speed); // split the vectorised motor speed into direction + // and speed values and write to the motor #ifdef FORCE_SENSE - bool stallDetection(void); // if the finger is drawing too much current whilst not moving, set the target pos to the current pos - void forceController(void); // stop the finger if the force limit is reached, or move to reach a target force + bool + stallDetection(void); // if the finger is drawing too much current whilst not + // moving, set the target pos to the current pos + void forceController(void); // stop the finger if the force limit is reached, + // or move to reach a target force #endif - // CALCULATIONS - void calcVel(void); - long _lastVelCal = 0; + // CALCULATIONS + void calcVel(void); + long _lastVelCal = 0; }; - // INTERRUPT HANDLERS -void _fingerControlCallback(void); // runs the control() function of a Finger instance at each call by the timer interrupt +void _fingerControlCallback( + void); // runs the control() function of a Finger instance at each call by + // the timer interrupt #ifdef FORCE_SENSE -void _currentSenseCallback(void); // runs the calcCurrentSns() function of a Finger instance at each call +void _currentSenseCallback(void); // runs the calcCurrentSns() function of a + // Finger instance at each call #endif // HARDWARE SPECIFIC FUNCTIONS #ifdef ARDUINO_AVR_MEGA2560 -void setPWMFreq(uint8_t pin, uint8_t value); // change the PWM timer frequency to be out of the audible range -#endif +void setPWMFreq(uint8_t pin, uint8_t value); // change the PWM timer frequency + // to be out of the audible range +#endif #endif /* FINGERLIB_H_ */ \ No newline at end of file diff --git a/src/buffer/CircleBuff.h b/src/buffer/CircleBuff.h index e1ea6ac..c184c83 100644 --- a/src/buffer/CircleBuff.h +++ b/src/buffer/CircleBuff.h @@ -1,17 +1,18 @@ /* Open Bionics - CircleBuff -* Author - Olly McBride -* Date - February 2017 -* -* This work is licensed under the Creative Commons Attribution-ShareAlike 4.0 International License. -* To view a copy of this license, visit http://creativecommons.org/licenses/by-sa/4.0/. -* -* Website - http://www.openbionics.com/ -* GitHub - https://github.com/Open-Bionics -* Email - ollymcbride@openbionics.com -* -* CircleBuff.h -* -*/ + * Author - Olly McBride + * Date - February 2017 + * + * This work is licensed under the Creative Commons + *Attribution-ShareAlike 4.0 International License. To view a copy of this + *license, visit http://creativecommons.org/licenses/by-sa/4.0/. + * + * Website - http://www.openbionics.com/ + * GitHub - https://github.com/Open-Bionics + * Email - ollymcbride@openbionics.com + * + * CircleBuff.h + * + */ #ifndef CIRCLE_BUFFER_H_ #define CIRCLE_BUFFER_H_ @@ -20,278 +21,239 @@ #include #ifndef max -#define max(a,b) (((a) > (b)) ? (a) : (b)) +#define max(a, b) (((a) > (b)) ? (a) : (b)) #endif #ifndef min -#define min(a,b) (((a) < (b)) ? (a) : (b)) +#define min(a, b) (((a) < (b)) ? (a) : (b)) #endif // MUST BE A POWER OF 2 -//#define C_BUFF_MAX_SIZE 64 // default buffer size (2^6) -//#define C_BUFF_MAX_SIZE 128 // default buffer size (2^7) -//#define C_BUFF_MAX_SIZE 255 // default buffer size (2^8) -//#define C_BUFF_MAX_SIZE 512 // default buffer size (2^9) -#define C_BUFF_MAX_SIZE 1024 // default buffer size (2^10) -//#define C_BUFF_MAX_SIZE 2048 // default buffer size (2^11) -//#define C_BUFF_MAX_SIZE 4096 // default buffer size (2^12) -//#define C_BUFF_MAX_SIZE 8192 // default buffer size (2^13) - +// #define C_BUFF_MAX_SIZE 64 // +// default buffer size (2^6) #define C_BUFF_MAX_SIZE 128 +// // default buffer size (2^7) #define C_BUFF_MAX_SIZE 255 +// // default buffer size (2^8) #define C_BUFF_MAX_SIZE 512 +// // default buffer size (2^9) +#define C_BUFF_MAX_SIZE 1024 // default buffer size (2^10) +// #define C_BUFF_MAX_SIZE 2048 // default +// buffer size (2^11) #define C_BUFF_MAX_SIZE 4096 +// // default buffer size (2^12) #define C_BUFF_MAX_SIZE 8192 +// // default buffer size (2^13) /** -* # Circle buffer class -* -* The circle buffer unit is a buffer in which data can be stored. The buffer is instantiated to a -* set type and initialised to a set size, where data can then be stored. Each new data entry is -* stored in the next buffer 'slot', and when the buffer is full, the buffer wraps around and overwrites -* the first data entry. Thus the buffer will not overflow. -* -* The circle buffer also calculates a running total, running mean, minimum and maximum value of -* all data entries. -* -*/ - -template -class CIRCLE_BUFFER -{ - public: - CIRCLE_BUFFER() - { - _buffSize = 0; /**< size of the buffer (must be a power of 2) */ - _buffSizeMask = 0; /**< mask used to calculate the wrap around point */ - _buff = nullptr; /**< data buffer */ - - _readIndex = 0; /**< index to read data from */ - _writeIndex = 0; /**< index to write new data to */ - - _total = 0; /**< running total */ - _mean = 0; /**< running mean */ - _min = (sizeof(buff_type) * UINT8_MAX);; /**< all-time minimum value */ - _max = 0; /**< all-time maximum value */ - } - - ~CIRCLE_BUFFER() - { - destroy(); - } - - /** Create an instance of a circular buffer of size 'size' (size must be a power or 2) - */ - bool begin(uint16_t size) - { - // if buffer size is not a power of 2, increase size until it is - while (!isPowerOfTwo(size)) - { - size++; - } - - // limit array size to maximum allocated size - if (size > C_BUFF_MAX_SIZE) - { - size = C_BUFF_MAX_SIZE; - } - - // allocate memory for buffer - _buff = (buff_type*) malloc(size * sizeof(buff_type)); - - // set circle buffer size - _buffSize = size; - _buffSizeMask = (_buffSize - 1); - - // reset read/write index - _readIndex = 0; - _writeIndex = 0; - - // clear maths variables - _total = 0; - _mean = 0; - - if (_buff == NULL) - { - return false; - } - else - { - // initialise all values to 0, so that when it is subtracted from the total on the first write nothing happens - fill(0); - - // reset the max & min values after the fill so that min != 0 - resetMaxMin(); - - return true; - } - } - - /** Fill buffer with an initial value - */ - void fill(buff_type value) - { - _total = (value * _buffSize); - _mean = value; - _min = value; - _max = value; - - for (uint16_t i = 0; i < _buffSize; i++) - { - _buff[i] = value; - } - } - - /** Read the first (oldest) value from the buffer and increment read index - */ - buff_type read(void) - { - if (_buff == NULL) - return (buff_type)0; - - return _buff[_readIndex++ & _buffSizeMask]; - } - - /** Read the first (oldest) value from the buffer and don't increment read index - */ - buff_type readGlimpse(void) - { - if (_buff == NULL) - return (buff_type)0; - - return _buff[_readIndex & _buffSizeMask]; - } - - /** Read a specific element within the buffer - */ - buff_type readElement(uint16_t index) - { - if (_buff == NULL) - return (buff_type)0; - - return _buff[index]; - } - - /** Read the mean of the buffer - */ - buff_type readMean(void) - { - return _mean; - } - - /** Read the total of the buffer - */ - long double readTotal(void) - { - return _total; - } - - /** Read the minimum value of the buffer - */ - buff_type readMin(void) - { - return _min; - } - - /** Read the maximum value of the buffer - */ - buff_type readMax(void) - { - return _max; - } - - /** Write to the buffer, recalculate the total and the mean (and std dev if enabled) - */ - bool write(buff_type value) - { - if (_buff == NULL) - return false; - - _min = min(_min, value); - _max = max(_max, value); - - // remove first value from total, then add new value and calculate mean - _total -= (long double)_buff[_writeIndex & _buffSizeMask]; // remove last value from total - _total += (long double) value; // add new value to total - - // calculate new mean - if (_total == 0) - { - _mean = 0; - } - else - { - _mean = (long double)_total / (long double)_buffSize; - } - - _buff[_writeIndex++ & _buffSizeMask] = value; // add new value to buffer - - // return true to indicate successful write - return true; - } - - - /** Get the actual buffer size (power of 2) - */ - uint16_t getSize(void) - { - return _buffSize; - } - - /** Get the current read position within the buffer - */ - uint16_t getReadIndex(void) - { - return _readIndex; - } - - /** Get the current write position within the buffer - */ - uint16_t getWriteIndex(void) - { - return _writeIndex; - } - - /** Reset the maximum & minimum values - */ - void resetMaxMin(void) - { - _min = (sizeof(buff_type) * UINT8_MAX); - _max = 0; - } - - /** Destroy the buffer by freeing the memory - */ - void destroy(void) - { - free(_buff); - - _buffSize = 0; - _total = 0; - _mean = 0; - _min = 0; - _max = 0; - - _readIndex = 0; - _writeIndex = 0; - - _buff = NULL; - } - - private: - uint16_t _buffSize; /**< size of the buffer (must be a power of 2) */ - uint16_t _buffSizeMask; /**< mask used to calculate the wrap around point */ - buff_type *_buff; /**< data buffer */ - - uint16_t _readIndex; /**< index to read data from */ - uint16_t _writeIndex; /**< index to write new data to */ - - long double _total; /**< running total */ - buff_type _mean; /**< running mean */ - buff_type _min; /**< all-time minimum value */ - buff_type _max; /**< all-time maximum value */ - - - bool isPowerOfTwo(uint16_t x) /**< return true if 'x' is a power of 2 */ - { - return (((uint16_t)x & ((uint16_t)x - (uint16_t)1)) == (uint16_t)0); - } + * # Circle buffer class + * + * The circle buffer unit is a buffer in which data can be stored. The buffer is + * instantiated to a set type and initialised to a set size, where data can then + * be stored. Each new data entry is stored in the next buffer 'slot', and when + * the buffer is full, the buffer wraps around and overwrites the first data + * entry. Thus the buffer will not overflow. + * + * The circle buffer also calculates a running total, running mean, minimum and + * maximum value of all data entries. + * + */ + +template class CIRCLE_BUFFER { +public: + CIRCLE_BUFFER() { + _buffSize = 0; /**< size of the buffer (must be a power of 2) */ + _buffSizeMask = 0; /**< mask used to calculate the wrap around point */ + _buff = nullptr; /**< data buffer */ + + _readIndex = 0; /**< index to read data from */ + _writeIndex = 0; /**< index to write new data to */ + + _total = 0; /**< running total */ + _mean = 0; /**< running mean */ + _min = (sizeof(buff_type) * UINT8_MAX); + ; /**< all-time minimum value */ + _max = 0; /**< all-time maximum value */ + } + + ~CIRCLE_BUFFER() { destroy(); } + + /** Create an instance of a circular buffer of size 'size' (size must be a + * power or 2) + */ + bool begin(uint16_t size) { + // if buffer size is not a power of 2, increase size until it is + while (!isPowerOfTwo(size)) { + size++; + } + + // limit array size to maximum allocated size + if (size > C_BUFF_MAX_SIZE) { + size = C_BUFF_MAX_SIZE; + } + + // allocate memory for buffer + _buff = (buff_type *)malloc(size * sizeof(buff_type)); + + // set circle buffer size + _buffSize = size; + _buffSizeMask = (_buffSize - 1); + + // reset read/write index + _readIndex = 0; + _writeIndex = 0; + + // clear maths variables + _total = 0; + _mean = 0; + + if (_buff == NULL) { + return false; + } else { + // initialise all values to 0, so that when it is subtracted from the + // total on the first write nothing happens + fill(0); + + // reset the max & min values after the fill so that min != 0 + resetMaxMin(); + + return true; + } + } + + /** Fill buffer with an initial value + */ + void fill(buff_type value) { + _total = (value * _buffSize); + _mean = value; + _min = value; + _max = value; + + for (uint16_t i = 0; i < _buffSize; i++) { + _buff[i] = value; + } + } + + /** Read the first (oldest) value from the buffer and increment read index + */ + buff_type read(void) { + if (_buff == NULL) + return (buff_type)0; + + return _buff[_readIndex++ & _buffSizeMask]; + } + + /** Read the first (oldest) value from the buffer and don't increment read + * index + */ + buff_type readGlimpse(void) { + if (_buff == NULL) + return (buff_type)0; + + return _buff[_readIndex & _buffSizeMask]; + } + + /** Read a specific element within the buffer + */ + buff_type readElement(uint16_t index) { + if (_buff == NULL) + return (buff_type)0; + + return _buff[index]; + } + + /** Read the mean of the buffer + */ + buff_type readMean(void) { return _mean; } + + /** Read the total of the buffer + */ + long double readTotal(void) { return _total; } + + /** Read the minimum value of the buffer + */ + buff_type readMin(void) { return _min; } + + /** Read the maximum value of the buffer + */ + buff_type readMax(void) { return _max; } + + /** Write to the buffer, recalculate the total and the mean (and std dev if + * enabled) + */ + bool write(buff_type value) { + if (_buff == NULL) + return false; + + _min = min(_min, value); + _max = max(_max, value); + + // remove first value from total, then add new value and calculate mean + _total -= (long double) + _buff[_writeIndex & _buffSizeMask]; // remove last value from total + _total += (long double)value; // add new value to total + + // calculate new mean + if (_total == 0) { + _mean = 0; + } else { + _mean = (long double)_total / (long double)_buffSize; + } + + _buff[_writeIndex++ & _buffSizeMask] = value; // add new value to buffer + + // return true to indicate successful write + return true; + } + + /** Get the actual buffer size (power of 2) + */ + uint16_t getSize(void) { return _buffSize; } + + /** Get the current read position within the buffer + */ + uint16_t getReadIndex(void) { return _readIndex; } + + /** Get the current write position within the buffer + */ + uint16_t getWriteIndex(void) { return _writeIndex; } + + /** Reset the maximum & minimum values + */ + void resetMaxMin(void) { + _min = (sizeof(buff_type) * UINT8_MAX); + _max = 0; + } + + /** Destroy the buffer by freeing the memory + */ + void destroy(void) { + free(_buff); + + _buffSize = 0; + _total = 0; + _mean = 0; + _min = 0; + _max = 0; + + _readIndex = 0; + _writeIndex = 0; + + _buff = NULL; + } + +private: + uint16_t _buffSize; /**< size of the buffer (must be a power of 2) */ + uint16_t _buffSizeMask; /**< mask used to calculate the wrap around point */ + buff_type *_buff; /**< data buffer */ + + uint16_t _readIndex; /**< index to read data from */ + uint16_t _writeIndex; /**< index to write new data to */ + + long double _total; /**< running total */ + buff_type _mean; /**< running mean */ + buff_type _min; /**< all-time minimum value */ + buff_type _max; /**< all-time maximum value */ + + bool isPowerOfTwo(uint16_t x) /**< return true if 'x' is a power of 2 */ + { + return (((uint16_t)x & ((uint16_t)x - (uint16_t)1)) == (uint16_t)0); + } }; - #endif // CIRCLE_BUFFER_H_ \ No newline at end of file diff --git a/src/current_sense/samd_CurrentSense.cpp b/src/current_sense/samd_CurrentSense.cpp index 82c09fc..f11e0ff 100644 --- a/src/current_sense/samd_CurrentSense.cpp +++ b/src/current_sense/samd_CurrentSense.cpp @@ -1,20 +1,21 @@ /* -* samd_CurrentSense.cpp -* -* Created: 16/09/2015 11:30:18 -* Author: Olly McBride -* -* This work is licensed under the Creative Commons Attribution 4.0 International License. -* To view a copy of this license, visit http://creativecommons.org/licenses/by/4.0/. -* -*/ + * samd_CurrentSense.cpp + * + * Created: 16/09/2015 11:30:18 + * Author: Olly McBride + * + * This work is licensed under the Creative Commons Attribution 4.0 + * International License. To view a copy of this license, visit + * http://creativecommons.org/licenses/by/4.0/. + * + */ #if defined(ARDUINO_ARCH_SAMD) #include "samd_CurrentSense.h" // list to hold pointers to each timer instance -const void* TCInstances[NUM_TIMERS] = { TCC0,TCC1,TCC2,TC3,TC4,TC5 }; +const void *TCInstances[NUM_TIMERS] = {TCC0, TCC1, TCC2, TC3, TC4, TC5}; // list to hold which timers have been enabled int PWMTimerList[NUM_TIMERS]; @@ -22,394 +23,341 @@ int PWMTimerList[NUM_TIMERS]; // the PWM timer being used to read the current uint8_t _currentSenseTimer = (uint8_t)(-1); -void(*_ptr2CurrSnsFunc)(void) = NULL; +void (*_ptr2CurrSnsFunc)(void) = NULL; int _ptr2CurrSnsFuncFlag = false; // attach current sense control interrupt, initialise and sync PWM timers -void initCurrentSense(uint8_t dir0, uint8_t dir1, void(*f)(void)) -{ - static bool _currSnsInit = false; // flag for current sense initialisation +void initCurrentSense(uint8_t dir0, uint8_t dir1, void (*f)(void)) { + static bool _currSnsInit = false; // flag for current sense initialisation + // attach current sense function + if (!_currSnsInit) { + _currSnsInit = true; - // attach current sense function - if (!_currSnsInit) - { - _currSnsInit = true; + _ptr2CurrSnsFunc = f; + _ptr2CurrSnsFuncFlag = true; - _ptr2CurrSnsFunc = f; - _ptr2CurrSnsFuncFlag = true; + setFastAnalogRead(); + } - setFastAnalogRead(); - } + // initalise PWM timers + uint8_t timer0 = initPWMtimer(dir0); + uint8_t timer1 = initPWMtimer(dir1); - // initalise PWM timers - uint8_t timer0 = initPWMtimer(dir0); - uint8_t timer1 = initPWMtimer(dir1); + // increment the list to show which timers are being used for PWM, so they can + // be synced later + if (timer0 != -1) + PWMTimerList[timer0]++; - // increment the list to show which timers are being used for PWM, so they can be synced later - if (timer0 != -1) - PWMTimerList[timer0]++; + if (timer1 != -1) + PWMTimerList[timer1]++; - if (timer1 != -1) - PWMTimerList[timer1]++; + // sync all attached PWM timers + syncPWMTimers(); +} - // sync all attached PWM timers - syncPWMTimers(); +// change the ADC clock prescaler to DIV (to reduce time in curr sense +// interrupt) +void setFastAnalogRead(void) { + ADC->CTRLB.bit.PRESCALER = ADC_CTRLB_PRESCALER_DIV128_Val; // ollyEdit + syncADC(); // ollyEdit } +// initialise the PWM timer with custom freq and prescaler +uint8_t initPWMtimer(uint32_t pin) { + PinDescription pinDesc = g_APinDescription[pin]; + uint32_t attr = pinDesc.ulPinAttribute; + // MYSERIAL.print("Pin: "); + // MYSERIAL.print(pin); -// change the ADC clock prescaler to DIV (to reduce time in curr sense interrupt) -void setFastAnalogRead(void) -{ - ADC->CTRLB.bit.PRESCALER = ADC_CTRLB_PRESCALER_DIV128_Val; // ollyEdit - syncADC(); // ollyEdit + // let arduino initialise timers & peripheral pins + analogWrite(pin, 0); -} + // if pin is a PWM pin + if ((attr & PIN_ATTR_PWM) == PIN_ATTR_PWM) { + uint32_t tcNum = GetTCNumber(pinDesc.ulPWMChannel); + uint8_t tcChannel = GetTCChannelNumber(pinDesc.ulPWMChannel); + static bool tcEnabled[TCC_INST_NUM + TC_INST_NUM]; -// initialise the PWM timer with custom freq and prescaler -uint8_t initPWMtimer(uint32_t pin) -{ - PinDescription pinDesc = g_APinDescription[pin]; - uint32_t attr = pinDesc.ulPinAttribute; - - //MYSERIAL.print("Pin: "); - //MYSERIAL.print(pin); - - // let arduino initialise timers & peripheral pins - analogWrite(pin, 0); - - // if pin is a PWM pin - if ((attr & PIN_ATTR_PWM) == PIN_ATTR_PWM) - { - uint32_t tcNum = GetTCNumber(pinDesc.ulPWMChannel); - uint8_t tcChannel = GetTCChannelNumber(pinDesc.ulPWMChannel); - static bool tcEnabled[TCC_INST_NUM + TC_INST_NUM]; - - // if timer has not already been configured, config timer - if (!tcEnabled[tcNum]) - { - tcEnabled[tcNum] = true; - - // configure peripheral according to device - if (attr & PIN_ATTR_TIMER) - { + // if timer has not already been configured, config timer + if (!tcEnabled[tcNum]) { + tcEnabled[tcNum] = true; + + // configure peripheral according to device + if (attr & PIN_ATTR_TIMER) { #if !(ARDUINO_SAMD_VARIANT_COMPLIANCE >= 10603) - // Compatibility for cores based on SAMD core <=1.6.2 - if (pinDesc.ulPinType == PIO_TIMER_ALT) - { - pinPeripheral(pin, PIO_TIMER_ALT); - } - else + // Compatibility for cores based on SAMD core <=1.6.2 + if (pinDesc.ulPinType == PIO_TIMER_ALT) { + pinPeripheral(pin, PIO_TIMER_ALT); + } else #endif - { - pinPeripheral(pin, PIO_TIMER); - } - } - else - { - // We suppose that attr has PIN_ATTR_TIMER_ALT bit set... - pinPeripheral(pin, PIO_TIMER_ALT); - } - - // assign timer to GCLK - uint16_t GCLK_CLKCTRL_IDs[] = - { - GCLK_CLKCTRL_ID(GCM_TCC0_TCC1), // TCC0 - GCLK_CLKCTRL_ID(GCM_TCC0_TCC1), // TCC1 - GCLK_CLKCTRL_ID(GCM_TCC2_TC3), // TCC2 - GCLK_CLKCTRL_ID(GCM_TCC2_TC3), // TC3 - GCLK_CLKCTRL_ID(GCM_TC4_TC5), // TC4 - GCLK_CLKCTRL_ID(GCM_TC4_TC5), // TC5 - GCLK_CLKCTRL_ID(GCM_TC6_TC7), // TC6 - GCLK_CLKCTRL_ID(GCM_TC6_TC7), // TC7 - }; - GCLK->CLKCTRL.reg = (uint16_t)(GCLK_CLKCTRL_CLKEN | GCLK_CLKCTRL_GEN_GCLK0 | GCLK_CLKCTRL_IDs[tcNum]); - while (GCLK->STATUS.bit.SYNCBUSY == 1); - - // configure timer for PWM - if (tcNum < TCC_INST_NUM) // if TCCx - { - // -- Configure TCC - Tcc* TCCx = (Tcc*)GetTC(pinDesc.ulPWMChannel); - // Disable TCCx - TCCx->CTRLA.bit.ENABLE = 0; - syncTCC(TCCx); - // Set prescaler to 1/256 - //TCCx->CTRLA.reg |= TCC_CTRLA_PRESCALER_DIV256; - TCCx->CTRLA.reg |= TCC_CTRLA_PRESCALER_DIV8; - //syncTCC(TCCx); - // Set TCx as normal PWM - TCCx->WAVE.reg |= TCC_WAVE_WAVEGEN_NPWM; - syncTCC(TCCx); - // Set the initial value to 0 - TCCx->CC[tcChannel].reg = (uint32_t)0x00; - syncTCC(TCCx); - // Set PER to maximum counter value (resolution : 0xFF) - TCCx->PER.reg = 0xFF; - syncTCC(TCCx); - - // Interrupts - TCCx->INTENSET.reg = 0; // disable all interrupts - TCCx->INTENSET.bit.OVF = 1; // enable overfollow - TCCx->INTENSET.bit.MC0 = 1; // enable compare match to CC0 - - // Enable TCCx - TCCx->CTRLA.bit.ENABLE = 1; - syncTCC(TCCx); - } - else // if TCx - { - // -- Configure TC - Tc* TCx = (Tc*)GetTC(pinDesc.ulPWMChannel); - // Disable TCx - TCx->COUNT8.CTRLA.bit.ENABLE = 0; - syncTC_8(TCx); - // Set Timer counter Mode to 8 bits, normal PWM, prescaler 1/256 - //TCx->COUNT8.CTRLA.reg |= TC_CTRLA_MODE_COUNT8 | TC_CTRLA_WAVEGEN_NPWM | TC_CTRLA_PRESCALER_DIV256; - TCx->COUNT8.CTRLA.reg |= TC_CTRLA_MODE_COUNT8 | TC_CTRLA_WAVEGEN_NPWM | TC_CTRLA_PRESCALER_DIV8; - syncTC_8(TCx); - // Set the initial value to 0 - TCx->COUNT8.CC[tcChannel].reg = (uint8_t)0x00; - syncTC_8(TCx); - // Set PER to maximum counter value (resolution : 0xFF) - TCx->COUNT8.PER.reg = 0xFF; - syncTC_8(TCx); - // Enable TCx - TCx->COUNT8.CTRLA.bit.ENABLE = 1; - syncTC_8(TCx); - } - } - else - { - if (tcNum >= TCC_INST_NUM) - { - Tc* TCx = (Tc*)GetTC(pinDesc.ulPWMChannel); - TCx->COUNT8.CC[tcChannel].reg = (uint8_t)0x00; - syncTC_8(TCx); - } - else { - Tcc* TCCx = (Tcc*)GetTC(pinDesc.ulPWMChannel); - TCCx->CTRLBSET.bit.LUPD = 1; - syncTCC(TCCx); - TCCx->CCB[tcChannel].reg = (uint32_t)0x00; - syncTCC(TCCx); - TCCx->CTRLBCLR.bit.LUPD = 1; - syncTCC(TCCx); - } - } - - // return the PWM timer used for the pin - return tcNum; - } - else - { - // if pin is not a PWM pin, return -1 - return (uint8_t)(-1); - } + { + pinPeripheral(pin, PIO_TIMER); + } + } else { + // We suppose that attr has PIN_ATTR_TIMER_ALT bit set... + pinPeripheral(pin, PIO_TIMER_ALT); + } + + // assign timer to GCLK + uint16_t GCLK_CLKCTRL_IDs[] = { + GCLK_CLKCTRL_ID(GCM_TCC0_TCC1), // TCC0 + GCLK_CLKCTRL_ID(GCM_TCC0_TCC1), // TCC1 + GCLK_CLKCTRL_ID(GCM_TCC2_TC3), // TCC2 + GCLK_CLKCTRL_ID(GCM_TCC2_TC3), // TC3 + GCLK_CLKCTRL_ID(GCM_TC4_TC5), // TC4 + GCLK_CLKCTRL_ID(GCM_TC4_TC5), // TC5 + GCLK_CLKCTRL_ID(GCM_TC6_TC7), // TC6 + GCLK_CLKCTRL_ID(GCM_TC6_TC7), // TC7 + }; + GCLK->CLKCTRL.reg = + (uint16_t)(GCLK_CLKCTRL_CLKEN | GCLK_CLKCTRL_GEN_GCLK0 | + GCLK_CLKCTRL_IDs[tcNum]); + while (GCLK->STATUS.bit.SYNCBUSY == 1) + ; + + // configure timer for PWM + if (tcNum < TCC_INST_NUM) // if TCCx + { + // -- Configure TCC + Tcc *TCCx = (Tcc *)GetTC(pinDesc.ulPWMChannel); + // Disable TCCx + TCCx->CTRLA.bit.ENABLE = 0; + syncTCC(TCCx); + // Set prescaler to 1/256 + // TCCx->CTRLA.reg |= TCC_CTRLA_PRESCALER_DIV256; + TCCx->CTRLA.reg |= TCC_CTRLA_PRESCALER_DIV8; + // syncTCC(TCCx); + // Set TCx as normal PWM + TCCx->WAVE.reg |= TCC_WAVE_WAVEGEN_NPWM; + syncTCC(TCCx); + // Set the initial value to 0 + TCCx->CC[tcChannel].reg = (uint32_t)0x00; + syncTCC(TCCx); + // Set PER to maximum counter value (resolution : 0xFF) + TCCx->PER.reg = 0xFF; + syncTCC(TCCx); + + // Interrupts + TCCx->INTENSET.reg = 0; // disable all interrupts + TCCx->INTENSET.bit.OVF = 1; // enable overfollow + TCCx->INTENSET.bit.MC0 = 1; // enable compare match to CC0 + + // Enable TCCx + TCCx->CTRLA.bit.ENABLE = 1; + syncTCC(TCCx); + } else // if TCx + { + // -- Configure TC + Tc *TCx = (Tc *)GetTC(pinDesc.ulPWMChannel); + // Disable TCx + TCx->COUNT8.CTRLA.bit.ENABLE = 0; + syncTC_8(TCx); + // Set Timer counter Mode to 8 bits, normal PWM, prescaler 1/256 + // TCx->COUNT8.CTRLA.reg |= TC_CTRLA_MODE_COUNT8 | TC_CTRLA_WAVEGEN_NPWM + // | TC_CTRLA_PRESCALER_DIV256; + TCx->COUNT8.CTRLA.reg |= TC_CTRLA_MODE_COUNT8 | TC_CTRLA_WAVEGEN_NPWM | + TC_CTRLA_PRESCALER_DIV8; + syncTC_8(TCx); + // Set the initial value to 0 + TCx->COUNT8.CC[tcChannel].reg = (uint8_t)0x00; + syncTC_8(TCx); + // Set PER to maximum counter value (resolution : 0xFF) + TCx->COUNT8.PER.reg = 0xFF; + syncTC_8(TCx); + // Enable TCx + TCx->COUNT8.CTRLA.bit.ENABLE = 1; + syncTC_8(TCx); + } + } else { + if (tcNum >= TCC_INST_NUM) { + Tc *TCx = (Tc *)GetTC(pinDesc.ulPWMChannel); + TCx->COUNT8.CC[tcChannel].reg = (uint8_t)0x00; + syncTC_8(TCx); + } else { + Tcc *TCCx = (Tcc *)GetTC(pinDesc.ulPWMChannel); + TCCx->CTRLBSET.bit.LUPD = 1; + syncTCC(TCCx); + TCCx->CCB[tcChannel].reg = (uint32_t)0x00; + syncTCC(TCCx); + TCCx->CTRLBCLR.bit.LUPD = 1; + syncTCC(TCCx); + } + } + + // return the PWM timer used for the pin + return tcNum; + } else { + // if pin is not a PWM pin, return -1 + return (uint8_t)(-1); + } } - - - -// count through every timer, if it is being used for PWM (timerList[i] > 0) disable timer and reset all counters to 0 -// when every PWM timer is clear, re-enable all PWM timers and return number of synced PWM timers -uint8_t syncPWMTimers(void) -{ - uint8_t nTimers = 0; - - Tcc* TCC[TC_INST_NUM]; - Tc* TC[TC_INST_NUM]; - - // disable all active PWM timers, clear timer counters and prescale counters - for (int i = 0; i < NUM_TIMERS; i++) - { - if (!PWMTimerList[i]) // if timer is not used for PWM, continue - continue; - - if (i < TCC_INST_NUM) // if TCCx - { - TCC[i] = (Tcc*)TCInstances[i]; - // Disable TCCx - TCC[i]->CTRLA.bit.ENABLE = 0; - syncTCC(TCC[i]); - - // Clear the prescaler counter - TCC[i]->CTRLA.reg |= TCC_CTRLA_PRESCSYNC_RESYNC; - syncTCC(TCC[i]); - - // Clear the timer counter - TCC[i]->COUNT.reg = 0x00; - syncTCC(TCC[i]); - - } - else // if TCx - { - int j = i - TCC_INST_NUM; - TC[j] = (Tc*)TCInstances[i]; - - // Disable TCCx - TC[j]->COUNT8.CTRLA.bit.ENABLE = 0; - syncTC_8(TC[j]); - - // Clear the prescaler counter - TC[j]->COUNT8.CTRLA.reg |= TC_CTRLA_PRESCSYNC_RESYNC; - syncTC_8(TC[j]); - - // Clear the timer counter - TC[j]->COUNT8.COUNT.reg = 0x00; - syncTC_8(TC[j]); - } - } - - - // use lowest timer for current sense - for (int i = 0; i < NUM_TIMERS; i++) - { - if (!PWMTimerList[i]) // if timer is not used for PWM, continue - continue; - - enableCurrentSnsTimer(i); - break; - } - - // re-enable all PWM timers - for (int i = 0; i < NUM_TIMERS; i++) - { - if (!PWMTimerList[i]) // if timer is not used for PWM, continue - continue; - - if (i < TCC_INST_NUM) // if TCCx - { - // Enable TCCx - TCC[i]->CTRLA.bit.ENABLE = 1; - syncTCC(TCC[i]); - } - else // if TCx - { - int j = i - TCC_INST_NUM; - - // Enable TCx - TC[j]->COUNT8.CTRLA.bit.ENABLE = 1; - syncTC_8(TC[j]); - } - - nTimers++; // if PWM timers, increment counter to show sync success - } - - return nTimers; // return number of PWM timers synced +// count through every timer, if it is being used for PWM (timerList[i] > 0) +// disable timer and reset all counters to 0 when every PWM timer is clear, +// re-enable all PWM timers and return number of synced PWM timers +uint8_t syncPWMTimers(void) { + uint8_t nTimers = 0; + + Tcc *TCC[TC_INST_NUM]; + Tc *TC[TC_INST_NUM]; + + // disable all active PWM timers, clear timer counters and prescale counters + for (int i = 0; i < NUM_TIMERS; i++) { + if (!PWMTimerList[i]) // if timer is not used for PWM, continue + continue; + + if (i < TCC_INST_NUM) // if TCCx + { + TCC[i] = (Tcc *)TCInstances[i]; + // Disable TCCx + TCC[i]->CTRLA.bit.ENABLE = 0; + syncTCC(TCC[i]); + + // Clear the prescaler counter + TCC[i]->CTRLA.reg |= TCC_CTRLA_PRESCSYNC_RESYNC; + syncTCC(TCC[i]); + + // Clear the timer counter + TCC[i]->COUNT.reg = 0x00; + syncTCC(TCC[i]); + + } else // if TCx + { + int j = i - TCC_INST_NUM; + TC[j] = (Tc *)TCInstances[i]; + + // Disable TCCx + TC[j]->COUNT8.CTRLA.bit.ENABLE = 0; + syncTC_8(TC[j]); + + // Clear the prescaler counter + TC[j]->COUNT8.CTRLA.reg |= TC_CTRLA_PRESCSYNC_RESYNC; + syncTC_8(TC[j]); + + // Clear the timer counter + TC[j]->COUNT8.COUNT.reg = 0x00; + syncTC_8(TC[j]); + } + } + + // use lowest timer for current sense + for (int i = 0; i < NUM_TIMERS; i++) { + if (!PWMTimerList[i]) // if timer is not used for PWM, continue + continue; + + enableCurrentSnsTimer(i); + break; + } + + // re-enable all PWM timers + for (int i = 0; i < NUM_TIMERS; i++) { + if (!PWMTimerList[i]) // if timer is not used for PWM, continue + continue; + + if (i < TCC_INST_NUM) // if TCCx + { + // Enable TCCx + TCC[i]->CTRLA.bit.ENABLE = 1; + syncTCC(TCC[i]); + } else // if TCx + { + int j = i - TCC_INST_NUM; + + // Enable TCx + TC[j]->COUNT8.CTRLA.bit.ENABLE = 1; + syncTC_8(TC[j]); + } + + nTimers++; // if PWM timers, increment counter to show sync success + } + + return nTimers; // return number of PWM timers synced } // enable timer interrupt for current reading -void enableCurrentSnsTimer(uint8_t tNum) -{ - // list of Interrupt Requests for timers - IRQn_Type timeIRQnList[NUM_TIMERS] = - { - TCC0_IRQn, - TCC1_IRQn, - TCC2_IRQn, - TC3_IRQn, - TC4_IRQn, - TC5_IRQn - }; - - // enable Interrupt Vector - NVIC_EnableIRQ(timeIRQnList[tNum]); - - // set sense timer number - _currentSenseTimer = tNum; -} - - +void enableCurrentSnsTimer(uint8_t tNum) { + // list of Interrupt Requests for timers + IRQn_Type timeIRQnList[NUM_TIMERS] = {TCC0_IRQn, TCC1_IRQn, TCC2_IRQn, + TC3_IRQn, TC4_IRQn, TC5_IRQn}; + // enable Interrupt Vector + NVIC_EnableIRQ(timeIRQnList[tNum]); + // set sense timer number + _currentSenseTimer = tNum; +} // PWM timer interrupt handlers -void TCC0_Handler() -{ - const uint8_t tmrNum = 0; // TCC0 - - Tcc* TCC = (Tcc*)TCC0; // get timer struct - if (TCC->INTFLAG.bit.OVF == 1) // A overflow caused the interrupt - { +void TCC0_Handler() { + const uint8_t tmrNum = 0; // TCC0 - if (_currentSenseTimer == tmrNum) - { - if (_ptr2CurrSnsFuncFlag) - { - _ptr2CurrSnsFunc(); - } - } + Tcc *TCC = (Tcc *)TCC0; // get timer struct + if (TCC->INTFLAG.bit.OVF == 1) // A overflow caused the interrupt + { + if (_currentSenseTimer == tmrNum) { + if (_ptr2CurrSnsFuncFlag) { + _ptr2CurrSnsFunc(); + } + } - TCC->INTFLAG.bit.OVF = 1; // writing a one clears the flag ovf flag - } + TCC->INTFLAG.bit.OVF = 1; // writing a one clears the flag ovf flag + } } -void TCC1_Handler() -{ - const uint8_t tmrNum = 1; // TCC1 +void TCC1_Handler() { + const uint8_t tmrNum = 1; // TCC1 - Tcc* TCC = (Tcc*)TCC1; // get timer struct - if (TCC->INTFLAG.bit.OVF == 1) // A overflow caused the interrupt - { + Tcc *TCC = (Tcc *)TCC1; // get timer struct + if (TCC->INTFLAG.bit.OVF == 1) // A overflow caused the interrupt + { - if (_currentSenseTimer == tmrNum) - { - if (_ptr2CurrSnsFuncFlag) - { - _ptr2CurrSnsFunc(); - } - } + if (_currentSenseTimer == tmrNum) { + if (_ptr2CurrSnsFuncFlag) { + _ptr2CurrSnsFunc(); + } + } - - TCC->INTFLAG.bit.OVF = 1; // writing a one clears the flag ovf flag - } + TCC->INTFLAG.bit.OVF = 1; // writing a one clears the flag ovf flag + } } -void TCC2_Handler() -{ - const uint8_t tmrNum = 2; // TCC2 - - Tcc* TCC = (Tcc*)TCC2; // get timer struct - if (TCC->INTFLAG.bit.OVF == 1) // A overflow caused the interrupt - { +void TCC2_Handler() { + const uint8_t tmrNum = 2; // TCC2 - if (_currentSenseTimer == tmrNum) - { - if (_ptr2CurrSnsFuncFlag) - { - _ptr2CurrSnsFunc(); - } - } + Tcc *TCC = (Tcc *)TCC2; // get timer struct + if (TCC->INTFLAG.bit.OVF == 1) // A overflow caused the interrupt + { + if (_currentSenseTimer == tmrNum) { + if (_ptr2CurrSnsFuncFlag) { + _ptr2CurrSnsFunc(); + } + } - TCC->INTFLAG.bit.OVF = 1; // writing a one clears the flag ovf flag - } + TCC->INTFLAG.bit.OVF = 1; // writing a one clears the flag ovf flag + } } -void TC3_Handler() -{ - const uint8_t tmrNum = 3; // TC3 - - TcCount8* TC = (TcCount8*)TC3; // get timer struct - if (TC->INTFLAG.bit.OVF == 1) - { - if (_currentSenseTimer == tmrNum) - { - if (_ptr2CurrSnsFuncFlag) - { - _ptr2CurrSnsFunc(); - } - } - - TC->INTFLAG.bit.OVF = 1; // writing a one clears the flag ovf flag - } +void TC3_Handler() { + const uint8_t tmrNum = 3; // TC3 + + TcCount8 *TC = (TcCount8 *)TC3; // get timer struct + if (TC->INTFLAG.bit.OVF == 1) { + if (_currentSenseTimer == tmrNum) { + if (_ptr2CurrSnsFuncFlag) { + _ptr2CurrSnsFunc(); + } + } + + TC->INTFLAG.bit.OVF = 1; // writing a one clears the flag ovf flag + } } // being used for position control -//void TC4_Handler() +// void TC4_Handler() //{ // const uint8_t tmrNum = 4; // TC4 // @@ -421,28 +369,24 @@ void TC3_Handler() // readCurrSense(); // } // -// TC->INTFLAG.bit.OVF = 1; // writing a one clears the flag ovf flag +// TC->INTFLAG.bit.OVF = 1; // writing a one clears the flag ovf +//flag // } //} -void TC5_Handler() -{ - const uint8_t tmrNum = 5; // TC5 - - TcCount8* TC = (TcCount8*)TC5; // get timer struct - if (TC->INTFLAG.bit.OVF == 1) - { - if (_currentSenseTimer == tmrNum) - { - if (_ptr2CurrSnsFuncFlag) - { - _ptr2CurrSnsFunc(); - } - } - - TC->INTFLAG.bit.OVF = 1; // writing a one clears the flag ovf flag - } -} +void TC5_Handler() { + const uint8_t tmrNum = 5; // TC5 + TcCount8 *TC = (TcCount8 *)TC5; // get timer struct + if (TC->INTFLAG.bit.OVF == 1) { + if (_currentSenseTimer == tmrNum) { + if (_ptr2CurrSnsFuncFlag) { + _ptr2CurrSnsFunc(); + } + } + + TC->INTFLAG.bit.OVF = 1; // writing a one clears the flag ovf flag + } +} #endif /* defined(ARDUINO_ARCH_SAMD) */ \ No newline at end of file diff --git a/src/current_sense/samd_CurrentSense.h b/src/current_sense/samd_CurrentSense.h index e3c2bd9..53cd813 100644 --- a/src/current_sense/samd_CurrentSense.h +++ b/src/current_sense/samd_CurrentSense.h @@ -4,10 +4,11 @@ * Created: 16/09/2015 11:30:18 * Author: Olly McBride * - * This work is licensed under the Creative Commons Attribution 4.0 International License. - * To view a copy of this license, visit http://creativecommons.org/licenses/by/4.0/. + * This work is licensed under the Creative Commons Attribution 4.0 + * International License. To view a copy of this license, visit + * http://creativecommons.org/licenses/by/4.0/. * - */ + */ #if defined(ARDUINO_ARCH_SAMD) @@ -15,58 +16,55 @@ #define SAMD_CURRENTSENSE_H_ #include -#include // for pinPeripheral +#include // for pinPeripheral -#include "FingerLib.h" // for MAX_FINGERS +#include "FingerLib.h" // for MAX_FINGERS - -#define NUM_TIMERS (TCC_INST_NUM + TC_INST_NUM) +#define NUM_TIMERS (TCC_INST_NUM + TC_INST_NUM) // Wait for synchronization of registers between the clock domains -static __inline__ void syncTC_8(Tc* TCx) __attribute__((always_inline, unused)); -static void syncTC_8(Tc* TCx) -{ - while (TCx->COUNT8.STATUS.bit.SYNCBUSY); +static __inline__ void syncTC_8(Tc *TCx) __attribute__((always_inline, unused)); +static void syncTC_8(Tc *TCx) { + while (TCx->COUNT8.STATUS.bit.SYNCBUSY) + ; } // Wait for synchronization of registers between the clock domains -static __inline__ void syncTCC(Tcc* TCCx) __attribute__((always_inline, unused)); -static void syncTCC(Tcc* TCCx) -{ - while (TCCx->SYNCBUSY.reg & TCC_SYNCBUSY_MASK); +static __inline__ void syncTCC(Tcc *TCCx) + __attribute__((always_inline, unused)); +static void syncTCC(Tcc *TCCx) { + while (TCCx->SYNCBUSY.reg & TCC_SYNCBUSY_MASK) + ; } // Wait for synchronization of registers between the clock domains static __inline__ void syncADC() __attribute__((always_inline, unused)); -static void syncADC() -{ - while (ADC->STATUS.bit.SYNCBUSY == 1) - ; +static void syncADC() { + while (ADC->STATUS.bit.SYNCBUSY == 1) + ; } - - // attach current sense control interrupt, initialise and sync PWM timers -void initCurrentSense(uint8_t dir0, uint8_t dir1, void(*f)(void)); +void initCurrentSense(uint8_t dir0, uint8_t dir1, void (*f)(void)); // initialise the PWM timer with custom freq and prescaler uint8_t initPWMtimer(uint32_t pin); -// count through every timer, if it is being used for PWM (timerList[i] > 0) disable timer and reset all counters to 0 -// when every PWM timer is clear, re-enable all PWM timers and return number of synced PWM timers +// count through every timer, if it is being used for PWM (timerList[i] > 0) +// disable timer and reset all counters to 0 when every PWM timer is clear, +// re-enable all PWM timers and return number of synced PWM timers uint8_t syncPWMTimers(void); -void enableCurrentSnsTimer(uint8_t tNum); // enable timer interrupt for current reading -void setFastAnalogRead(void); // change the ADC clock prescaler to DIV (to reduce time in curr sense interrupt) - - +void enableCurrentSnsTimer( + uint8_t tNum); // enable timer interrupt for current reading +void setFastAnalogRead(void); // change the ADC clock prescaler to DIV (to + // reduce time in curr sense interrupt) // list to hold pointers to each timer instance -extern const void* TCInstances[NUM_TIMERS]; +extern const void *TCInstances[NUM_TIMERS]; // list to hold which timers have been enabled extern int PWMTimerList[NUM_TIMERS]; - #endif /* SAMD_CURRENTSENSE_H_ */ #endif /* defined(ARDUINO_ARCH_SAMD) */ \ No newline at end of file diff --git a/src/pid/pid_controller.cpp b/src/pid/pid_controller.cpp index 5af156b..1609c49 100644 --- a/src/pid/pid_controller.cpp +++ b/src/pid/pid_controller.cpp @@ -1,140 +1,116 @@ /* -* pid_controller.cpp -* -* Created: 29/09/2016 13:48:12 -* Author: Olly McBride -* -* This work is licensed under the Creative Commons Attribution 4.0 International License. -* To view a copy of this license, visit http://creativecommons.org/licenses/by/4.0/. -* -*/ + * pid_controller.cpp + * + * Created: 29/09/2016 13:48:12 + * Author: Olly McBride + * + * This work is licensed under the Creative Commons Attribution 4.0 + * International License. To view a copy of this license, visit + * http://creativecommons.org/licenses/by/4.0/. + * + */ #include "pid_controller.h" +PID_CONTROLLER::PID_CONTROLLER() { + _dInput = 0; + _prev = 0; + _integral = 0; // clear PID history -PID_CONTROLLER::PID_CONTROLLER() -{ - _dInput = 0; - _prev = 0; - _integral = 0; // clear PID history + _rampLim = 0; // disable rate limit + _prevOutput = 0; - _rampLim = 0; // disable rate limit - _prevOutput = 0; + setLimits(DEFAULT_LIMIT_MIN, DEFAULT_LIMIT_MAX); + setGains(DEFAULT_GAIN_P, DEFAULT_GAIN_I, DEFAULT_GAIN_D); - setLimits(DEFAULT_LIMIT_MIN, DEFAULT_LIMIT_MAX); - setGains(DEFAULT_GAIN_P, DEFAULT_GAIN_I, DEFAULT_GAIN_D); - - enable(true); + enable(true); } // enable the PID controller (default) -void PID_CONTROLLER::enable(void) -{ - enable(true); -} +void PID_CONTROLLER::enable(void) { enable(true); } // disable the PID controller -void PID_CONTROLLER::disable(void) -{ - enable(false); -} +void PID_CONTROLLER::disable(void) { enable(false); } // set the PID controller to be enabled/disabled -void PID_CONTROLLER::enable(bool en) -{ - _en = en; -} +void PID_CONTROLLER::enable(bool en) { _en = en; } -void PID_CONTROLLER::reset(void) -{ - _dInput = 0; - _prev = 0; - _integral = 0; +void PID_CONTROLLER::reset(void) { + _dInput = 0; + _prev = 0; + _integral = 0; } -double PID_CONTROLLER::run(double targ, double curr) -{ - if (!_en) - { - return 0; - } - - double output = 0; - double error = targ - curr; - double sampleTime = (_sampleTimer.stop() / 1000); // sampleTime in ms - - if (sampleTime > 0) - { - // integral summation allows _Ki to change without causing unwanted disturbances - _integral += _Ki * error * sampleTime; - _integral = constrain(_integral, _min, _max); - - // store the change in input to remove the derivative spike - double _dInput = (curr - _prev) / sampleTime; - - // calculate and constrain output - output = (_Kp * error) + _integral - (_Kd * _dInput); - output = constrain(output, _min, _max); - - // if a rate limit is set - if (_rampLim != 0) - { - double change = (output - _prevOutput); - - // if the change in outputs exceeds the rate limit - if (change > _rampLim) - { - output = _prevOutput + _rampLim; // just change by the ramp rate - } - else if (change < -_rampLim) - { - output = _prevOutput - _rampLim; // just change by the ramp rate - } - } - - // store history - _prev = curr; // previous pos/vel - _prevOutput = output; // store the previous output - } - - // restart the sample timer - _sampleTimer.start(); - - return output; +double PID_CONTROLLER::run(double targ, double curr) { + if (!_en) { + return 0; + } + + double output = 0; + double error = targ - curr; + double sampleTime = (_sampleTimer.stop() / 1000); // sampleTime in ms + + if (sampleTime > 0) { + // integral summation allows _Ki to change without causing unwanted + // disturbances + _integral += _Ki * error * sampleTime; + _integral = constrain(_integral, _min, _max); + + // store the change in input to remove the derivative spike + double _dInput = (curr - _prev) / sampleTime; + + // calculate and constrain output + output = (_Kp * error) + _integral - (_Kd * _dInput); + output = constrain(output, _min, _max); + + // if a rate limit is set + if (_rampLim != 0) { + double change = (output - _prevOutput); + + // if the change in outputs exceeds the rate limit + if (change > _rampLim) { + output = _prevOutput + _rampLim; // just change by the ramp rate + } else if (change < -_rampLim) { + output = _prevOutput - _rampLim; // just change by the ramp rate + } + } + + // store history + _prev = curr; // previous pos/vel + _prevOutput = output; // store the previous output + } + + // restart the sample timer + _sampleTimer.start(); + + return output; } // set the output value limits -void PID_CONTROLLER::setLimits(double min, double max) -{ - _min = min; - _max = max; +void PID_CONTROLLER::setLimits(double min, double max) { + _min = min; + _max = max; } // get the output value limits -void PID_CONTROLLER::getLimits(double *min, double *max) -{ - *min = _min; - *max = _max; +void PID_CONTROLLER::getLimits(double *min, double *max) { + *min = _min; + *max = _max; } // set the maximum change in output limit -void PID_CONTROLLER::setRampRate(double ramp) -{ - _rampLim = ramp; -} +void PID_CONTROLLER::setRampRate(double ramp) { _rampLim = ramp; } // set the controller gains -void PID_CONTROLLER::setGains(float Kp, float Ki, float Kd) -{ - _Kp = Kp; - _Ki = Ki; - _Kd = Kd; +void PID_CONTROLLER::setGains(float Kp, float Ki, float Kd) { + _Kp = Kp; + _Ki = Ki; + _Kd = Kd; } // get the gains of the controller -void PID_CONTROLLER::getGains(float *Kp, float *Ki, float *Kd) -{ - *Kp = _Kp; - *Ki = _Ki; - *Kd = _Kd; +void PID_CONTROLLER::getGains(float *Kp, float *Ki, float *Kd) { + *Kp = _Kp; + *Ki = _Ki; + *Kd = _Kd; } \ No newline at end of file diff --git a/src/pid/pid_controller.h b/src/pid/pid_controller.h index df6ea9d..648e88d 100644 --- a/src/pid/pid_controller.h +++ b/src/pid/pid_controller.h @@ -1,86 +1,79 @@ /* -* pid_controller.h -* -* Created: 29/09/2016 13:48:12 -* Author: Olly McBride -* -* This work is licensed under the Creative Commons Attribution 4.0 International License. -* To view a copy of this license, visit http://creativecommons.org/licenses/by/4.0/. -* -*/ - + * pid_controller.h + * + * Created: 29/09/2016 13:48:12 + * Author: Olly McBride + * + * This work is licensed under the Creative Commons Attribution 4.0 + * International License. To view a copy of this license, visit + * http://creativecommons.org/licenses/by/4.0/. + * + */ #ifndef PID_CONTROLLER_H_ #define PID_CONTROLLER_H_ -#include #include "../timers/timer_and_delay.h" +#include +#define DEFAULT_LIMIT_MAX 255 +#define DEFAULT_LIMIT_MIN -255 -#define DEFAULT_LIMIT_MAX 255 -#define DEFAULT_LIMIT_MIN -255 - -//#define DEFAULT_GAIN_P 1.9 -//#define DEFAULT_GAIN_P 1.2 -//#define DEFAULT_GAIN_I 0 -//#define DEFAULT_GAIN_D 0 - -//#define DEFAULT_GAIN_P 5 -//#define DEFAULT_GAIN_I 0.001 -//#define DEFAULT_GAIN_D 0 - -//#define DEFAULT_GAIN_P 3 -//#define DEFAULT_GAIN_I 0.0013 -//#define DEFAULT_GAIN_D 0.1 - -//#define DEFAULT_GAIN_P 2.5 -//#define DEFAULT_GAIN_I 0.0003 -//#define DEFAULT_GAIN_D 0.1 +// #define DEFAULT_GAIN_P 1.9 +// #define DEFAULT_GAIN_P 1.2 +// #define DEFAULT_GAIN_I 0 +// #define DEFAULT_GAIN_D 0 -#define DEFAULT_GAIN_P 2 -#define DEFAULT_GAIN_I 0 -#define DEFAULT_GAIN_D 10 +// #define DEFAULT_GAIN_P 5 +// #define DEFAULT_GAIN_I 0.001 +// #define DEFAULT_GAIN_D 0 +// #define DEFAULT_GAIN_P 3 +// #define DEFAULT_GAIN_I 0.0013 +// #define DEFAULT_GAIN_D 0.1 +// #define DEFAULT_GAIN_P 2.5 +// #define DEFAULT_GAIN_I 0.0003 +// #define DEFAULT_GAIN_D 0.1 -class PID_CONTROLLER -{ - public: - PID_CONTROLLER(); +#define DEFAULT_GAIN_P 2 +#define DEFAULT_GAIN_I 0 +#define DEFAULT_GAIN_D 10 - void enable(void); // enable the PID controller (default) - void disable(void); // disable the PID controller - void enable(bool en); // set the PID controller to be enabled/disabled - void reset(void); +class PID_CONTROLLER { +public: + PID_CONTROLLER(); - double run(double targ, double curr); // run the PID computation + void enable(void); // enable the PID controller (default) + void disable(void); // disable the PID controller + void enable(bool en); // set the PID controller to be enabled/disabled + void reset(void); - void setLimits(double min, double max); // set the output value limits - void getLimits(double *min, double *max); // get the output value limits - void setRampRate(double ramp); // set the maximum change in output limit + double run(double targ, double curr); // run the PID computation - void setGains(float Kp, float Ki, float Kd); // set the controller gains - void getGains(float *Kp, float *Ki, float *Kd); // get the controller gains + void setLimits(double min, double max); // set the output value limits + void getLimits(double *min, double *max); // get the output value limits + void setRampRate(double ramp); // set the maximum change in output limit - private: - bool _en; // enable/disable flag + void setGains(float Kp, float Ki, float Kd); // set the controller gains + void getGains(float *Kp, float *Ki, float *Kd); // get the controller gains - US_NB_TIMER _sampleTimer; // duration of sample in us +private: + bool _en; // enable/disable flag - double _min, _max; // output limits - double _rampLim; // ramp rate limit + US_NB_TIMER _sampleTimer; // duration of sample in us - double _prevOutput; // previous output (used + double _min, _max; // output limits + double _rampLim; // ramp rate limit - float _Kp, _Ki, _Kd; // PID controller gains + double _prevOutput; // previous output (used - double _dInput; // change in input over time - double _prev; // previous input + float _Kp, _Ki, _Kd; // PID controller gains - double _integral; // integral value + double _dInput; // change in input over time + double _prev; // previous input + double _integral; // integral value }; - - #endif /* PID_CONTROLLER_H_ */ \ No newline at end of file diff --git a/src/timers/avr_FingerTimer.cpp b/src/timers/avr_FingerTimer.cpp index 428119d..9099eb9 100644 --- a/src/timers/avr_FingerTimer.cpp +++ b/src/timers/avr_FingerTimer.cpp @@ -4,138 +4,127 @@ * Created: 02/11/2015 11:12:48 * Author: Olly McBride * - * This work is licensed under the Creative Commons Attribution 4.0 International License. - * To view a copy of this license, visit http://creativecommons.org/licenses/by/4.0/. + * This work is licensed under the Creative Commons Attribution 4.0 + * International License. To view a copy of this license, visit + * http://creativecommons.org/licenses/by/4.0/. * - */ + */ #if defined(ARDUINO_ARCH_AVR) #include "avr_FingerTimer.h" // create global pointers to functions and flags -void(*_ptr2MotorFunc)(void) = NULL; -void(*_ptr2PiggybackFunc)(void) = NULL; +void (*_ptr2MotorFunc)(void) = NULL; +void (*_ptr2PiggybackFunc)(void) = NULL; // function to receive pointer to motor and assign to global pointer -void _passMotorPtr(void(*f)(void)) -{ - _ptr2MotorFunc = f; -} +void _passMotorPtr(void (*f)(void)) { _ptr2MotorFunc = f; } -// function to receive pointer for timer piggybacking and assign to global pointer -void _attachFuncToTimer(void(*f)(void)) -{ - _ptr2PiggybackFunc = f; -} +// function to receive pointer for timer piggybacking and assign to global +// pointer +void _attachFuncToTimer(void (*f)(void)) { _ptr2PiggybackFunc = f; } #if defined(ARDUINO_AVR_MEGA2560) // initialise timer registers for position control timer -void _posCtrlTimerSetup(void) -{ - _changePWMFreq(); // change PWM to ~31KHz, so it is out of the audible range - - cli(); //stop interrupts - - // Timer5 - TCCR5A = 0; // set entire TCCR5A register to 0 - TCCR5B = 0; // same for TCCR5B - TCNT5 = 0; //initialize counter value to 0 - OCR5A = CC_REG_VAL; // set compare match register for 5khz increments - TCCR5B |= (1 << WGM52); // turn on CTC mode - TCCR5B |= (1 << CS50); // Set CS50 bit for no prescaler for maximum precision - TIMSK5 |= (1 << OCIE5A); // enable timer compare interrupt - - sei(); //allow interrupts +void _posCtrlTimerSetup(void) { + _changePWMFreq(); // change PWM to ~31KHz, so it is out of the audible range + + cli(); // stop interrupts + + // Timer5 + TCCR5A = 0; // set entire TCCR5A register to 0 + TCCR5B = 0; // same for TCCR5B + TCNT5 = 0; // initialize counter value to 0 + OCR5A = CC_REG_VAL; // set compare match register for 5khz increments + TCCR5B |= (1 << WGM52); // turn on CTC mode + TCCR5B |= (1 << CS50); // Set CS50 bit for no prescaler for maximum precision + TIMSK5 |= (1 << OCIE5A); // enable timer compare interrupt + + sei(); // allow interrupts } -// Timer5 -ISR(TIMER5_COMPA_vect) -{ - static long timer5cnt = 0; // main timer counter increments every call of the interrupt - static long motorCount = 0; // time instance variable for motor position control - static long mSecCount = 0; // time instance variable for millisecond counter - - timer5cnt++; // increment timer counter every - - // triggered once a millisecond - if ((timer5cnt - mSecCount) >= MILLI_TIME) - { - mSecCount = timer5cnt; - - if (_ptr2PiggybackFunc) - { - _ptr2PiggybackFunc(); - } - } - - // position control for a single motor - if ((timer5cnt - motorCount) >= MOTOR_CTRL_TIME) - { - motorCount = timer5cnt; - - if (_ptr2MotorFunc) - { - _ptr2MotorFunc(); - } - } +// Timer5 +ISR(TIMER5_COMPA_vect) { + static long timer5cnt = + 0; // main timer counter increments every call of the interrupt + static long motorCount = + 0; // time instance variable for motor position control + static long mSecCount = 0; // time instance variable for millisecond counter + + timer5cnt++; // increment timer counter every + + // triggered once a millisecond + if ((timer5cnt - mSecCount) >= MILLI_TIME) { + mSecCount = timer5cnt; + + if (_ptr2PiggybackFunc) { + _ptr2PiggybackFunc(); + } + } + + // position control for a single motor + if ((timer5cnt - motorCount) >= MOTOR_CTRL_TIME) { + motorCount = timer5cnt; + + if (_ptr2MotorFunc) { + _ptr2MotorFunc(); + } + } } -void _changePWMFreq(void) -{ - TCCR1B = (TCCR1B & 0b11111000) | 0x01; - TCCR2B = (TCCR2B & 0b11111000) | 0x01; - TCCR3B = (TCCR3B & 0b11111000) | 0x01; - TCCR4B = (TCCR4B & 0b11111000) | 0x01; +void _changePWMFreq(void) { + TCCR1B = (TCCR1B & 0b11111000) | 0x01; + TCCR2B = (TCCR2B & 0b11111000) | 0x01; + TCCR3B = (TCCR3B & 0b11111000) | 0x01; + TCCR4B = (TCCR4B & 0b11111000) | 0x01; } #elif defined(ARDUINO_AVR_UNO) // initialise timer registers for position control timer -void _posCtrlTimerSetup(void) -{ +void _posCtrlTimerSetup(void) { - cli(); //stop interrupts + cli(); // stop interrupts - // Timer1 - TCCR1A = 0; // set entire TCCR1A register to 0 - TCCR1B = 0; // same for TCCR1B - TCNT1 = 0; //initialize counter value to 0 - OCR1A = CC_REG_VAL; // set compare match register to the value set by avr_FingerTimer.h - TCCR1B |= (1 << WGM12); // turn on CTC mode - TCCR1B |= (1 << CS10); // Set CS50 bit for no prescaler for maximum precision - TIMSK1 |= (1 << OCIE1A);// enable timer compare interrupt + // Timer1 + TCCR1A = 0; // set entire TCCR1A register to 0 + TCCR1B = 0; // same for TCCR1B + TCNT1 = 0; // initialize counter value to 0 + OCR1A = CC_REG_VAL; // set compare match register to the value set by + // avr_FingerTimer.h + TCCR1B |= (1 << WGM12); // turn on CTC mode + TCCR1B |= (1 << CS10); // Set CS50 bit for no prescaler for maximum precision + TIMSK1 |= (1 << OCIE1A); // enable timer compare interrupt - sei(); //allow interrupts + sei(); // allow interrupts } // Timer1 -ISR(TIMER1_COMPA_vect) -{ - static long timer1cnt = 0; // main timer counter increments every call of the interrupt - static long motorCount = 0; // time instance variable for motor position control - static long mSecCount = 0; // time instance variable for millisecond counter - - timer1cnt++; // increment timer counter - - // triggered once a millisecond - if ((timer1cnt - mSecCount) >= MILLI_TIME) - { - mSecCount = timer1cnt; - - if (_ptr2PiggybackFunc) - { - _ptr2PiggybackFunc(); - } - } - - // position control for a single motor - if ((timer1cnt - motorCount) >= MOTOR_CTRL_TIME) - { - motorCount = timer1cnt; - _ptr2MotorFunc(); - } +ISR(TIMER1_COMPA_vect) { + static long timer1cnt = + 0; // main timer counter increments every call of the interrupt + static long motorCount = + 0; // time instance variable for motor position control + static long mSecCount = 0; // time instance variable for millisecond counter + + timer1cnt++; // increment timer counter + + // triggered once a millisecond + if ((timer1cnt - mSecCount) >= MILLI_TIME) { + mSecCount = timer1cnt; + + if (_ptr2PiggybackFunc) { + _ptr2PiggybackFunc(); + } + } + + // position control for a single motor + if ((timer1cnt - motorCount) >= MOTOR_CTRL_TIME) { + motorCount = timer1cnt; + _ptr2MotorFunc(); + } } #endif diff --git a/src/timers/avr_FingerTimer.h b/src/timers/avr_FingerTimer.h index f035df7..cd70c08 100644 --- a/src/timers/avr_FingerTimer.h +++ b/src/timers/avr_FingerTimer.h @@ -4,10 +4,11 @@ * Created: 02/11/2015 11:12:48 * Author: Olly McBride * - * This work is licensed under the Creative Commons Attribution 4.0 International License. - * To view a copy of this license, visit http://creativecommons.org/licenses/by/4.0/. + * This work is licensed under the Creative Commons Attribution 4.0 + * International License. To view a copy of this license, visit + * http://creativecommons.org/licenses/by/4.0/. * - */ + */ #if defined(ARDUINO_ARCH_AVR) @@ -17,43 +18,51 @@ #include // select timer frequency -//#define TIMER_10KHZ -//#define TIMER_5KHZ +// #define TIMER_10KHZ +// #define TIMER_5KHZ #define TIMER_2KHZ -//#define TIMER_1KHZ +// #define TIMER_1KHZ #if defined(TIMER_10KHZ) -#define TIMER_FREQ 10 // timer frequency in KHz -//#define CC_REG_VAL 319 // compare capture reg val -#define CC_REG_VAL 1500 // compare capture reg val +#define TIMER_FREQ 10 // timer frequency in KHz +// #define CC_REG_VAL 319 // compare capture reg val +#define CC_REG_VAL 1500 // compare capture reg val #elif defined(TIMER_5KHZ) -#define TIMER_FREQ 5 // timer frequency in KHz -//#define CC_REG_VAL 640 // compare capture reg val -#define CC_REG_VAL 3000 // compare capture reg val +#define TIMER_FREQ 5 // timer frequency in KHz +// #define CC_REG_VAL 640 // compare capture reg val +#define CC_REG_VAL 3000 // compare capture reg val #elif defined(TIMER_2KHZ) -#define TIMER_FREQ 2 // timer frequency in KHz -//#define CC_REG_VAL 1599 // compare capture reg val -#define CC_REG_VAL 7500 // compare capture reg val +#define TIMER_FREQ 2 // timer frequency in KHz +// #define CC_REG_VAL 1599 // compare capture reg val +#define CC_REG_VAL 7500 // compare capture reg val #elif defined(TIMER_1KHZ) -#define TIMER_FREQ 1 // timer frequency in KHz -//#define CC_REG_VAL 3198 // compare capture reg val -#define CC_REG_VAL 15000 // compare capture reg val +#define TIMER_FREQ 1 // timer frequency in KHz +// #define CC_REG_VAL 3198 // compare capture reg val +#define CC_REG_VAL 15000 // compare capture reg val #endif -#define ms(val) ((val)*(TIMER_FREQ)) // number of timer ticks per ms +#define ms(val) ((val) * (TIMER_FREQ)) // number of timer ticks per ms // calculate number of timer ticks per ms for each timer function -#define MILLI_TIME ms(1) // 1ms 1kHz -#define MOTOR_CTRL_TIME ms(5) // 5ms 200Hz +#define MILLI_TIME ms(1) // 1ms 1kHz +#define MOTOR_CTRL_TIME ms(5) // 5ms 200Hz -#define NOT_A_PWM_PIN (-1) -#define PWM_pin_to_timer(p) ( (((p) == 4) || ((p) == 13)) ? 0 : \ - ( (((p) == 11) || ((p) == 12)) ? 1 : \ - ( (((p) == 9) || ((p) == 10)) ? 2 : \ - ( (((p) == 2) || ((p) == 3) || ((p) == 5)) ? 3 : \ - ( (((p) == 6) || ((p) == 7) || ((p) == 8)) ? 4 : \ - ( (((p) == 44) || ((p) == 45) || ((p) == 46)) ? 5 : \ - NOT_A_PWM_PIN ) ) ) ) ) ) +#define NOT_A_PWM_PIN (-1) +#define PWM_pin_to_timer(p) \ + ((((p) == 4) || ((p) == 13)) \ + ? 0 \ + : ((((p) == 11) || ((p) == 12)) \ + ? 1 \ + : ((((p) == 9) || ((p) == 10)) \ + ? 2 \ + : ((((p) == 2) || ((p) == 3) || ((p) == 5)) \ + ? 3 \ + : ((((p) == 6) || ((p) == 7) || ((p) == 8)) \ + ? 4 \ + : ((((p) == 44) || ((p) == 45) || \ + ((p) == 46)) \ + ? 5 \ + : NOT_A_PWM_PIN)))))) // function prototypes void _posCtrlTimerSetup(void); diff --git a/src/timers/samd_FingerTimer.cpp b/src/timers/samd_FingerTimer.cpp index bb03466..b305d35 100644 --- a/src/timers/samd_FingerTimer.cpp +++ b/src/timers/samd_FingerTimer.cpp @@ -4,114 +4,114 @@ * Created: 02/11/2015 11:12:48 * Author: Olly McBride * - * This work is licensed under the Creative Commons Attribution 4.0 International License. - * To view a copy of this license, visit http://creativecommons.org/licenses/by/4.0/. + * This work is licensed under the Creative Commons Attribution 4.0 + * International License. To view a copy of this license, visit + * http://creativecommons.org/licenses/by/4.0/. * - */ - + */ #if defined(ARDUINO_ARCH_SAMD) #include "samd_FingerTimer.h" // used for customMillis() -//unsigned long _milliSeconds = 0; +// unsigned long _milliSeconds = 0; // create global pointers to functions and flags void (*_ptr2MotorFunc)(void) = NULL; void (*_ptr2PiggybackFunc)(void) = NULL; // function to receive pointer to motor and assign to global pointer -void _passMotorPtr(void (*f)(void)) -{ - _ptr2MotorFunc = f; -} +void _passMotorPtr(void (*f)(void)) { _ptr2MotorFunc = f; } -// function to receive pointer for timer piggybacking and assign to global pointer -void _attachFuncToTimer(void (*f)(void)) -{ - _ptr2PiggybackFunc = f; -} +// function to receive pointer for timer piggybacking and assign to global +// pointer +void _attachFuncToTimer(void (*f)(void)) { _ptr2PiggybackFunc = f; } // initialise timer registers for 5KHz timer (200uS) -void _posCtrlTimerSetup(void) -{ - // Enable clock for TC4 - REG_GCLK_CLKCTRL = (uint16_t) (GCLK_CLKCTRL_CLKEN | GCLK_CLKCTRL_GEN_GCLK0 | GCLK_CLKCTRL_ID_TC4_TC5) ; - while ( GCLK->STATUS.bit.SYNCBUSY == 1 ); // wait for sync - - // The type cast must fit with the selected timer mode - TcCount16* TC = (TcCount16*) TC4; // get timer struct - - TC->CTRLA.reg &= ~TC_CTRLA_ENABLE; // Disable TC - while (TC->STATUS.bit.SYNCBUSY == 1); // wait for sync - - TC->CTRLA.reg |= TC_CTRLA_MODE_COUNT16; // Set Timer counter Mode to 16 bits - while (TC->STATUS.bit.SYNCBUSY == 1); // wait for sync - TC->CTRLA.reg |= TC_CTRLA_WAVEGEN_MFRQ; // Set TC Freq - while (TC->STATUS.bit.SYNCBUSY == 1); // wait for sync - - TC->CTRLA.reg |= TC_CTRLA_PRESCALER_DIV2; // Set prescaler - while (TC->STATUS.bit.SYNCBUSY == 1); // wait for sync - - TC->CC[0].reg = CC_REG_VAL; // timer frequency - while (TC->STATUS.bit.SYNCBUSY == 1); // wait for sync - - // Interrupts - TC->INTENSET.reg = 0; // disable all interrupts - TC->INTENSET.bit.OVF = 1; // enable overflow - TC->INTENSET.bit.MC0 = 1; // enable compare match to CC0 - - // Enable InterruptVector - NVIC_EnableIRQ(TC4_IRQn); - - // Enable TC - TC->CTRLA.reg |= TC_CTRLA_ENABLE; - while (TC->STATUS.bit.SYNCBUSY == 1); // wait for sync - +void _posCtrlTimerSetup(void) { + // Enable clock for TC4 + REG_GCLK_CLKCTRL = (uint16_t)(GCLK_CLKCTRL_CLKEN | GCLK_CLKCTRL_GEN_GCLK0 | + GCLK_CLKCTRL_ID_TC4_TC5); + while (GCLK->STATUS.bit.SYNCBUSY == 1) + ; // wait for sync + + // The type cast must fit with the selected timer mode + TcCount16 *TC = (TcCount16 *)TC4; // get timer struct + + TC->CTRLA.reg &= ~TC_CTRLA_ENABLE; // Disable TC + while (TC->STATUS.bit.SYNCBUSY == 1) + ; // wait for sync + + TC->CTRLA.reg |= TC_CTRLA_MODE_COUNT16; // Set Timer counter Mode to 16 bits + while (TC->STATUS.bit.SYNCBUSY == 1) + ; // wait for sync + TC->CTRLA.reg |= TC_CTRLA_WAVEGEN_MFRQ; // Set TC Freq + while (TC->STATUS.bit.SYNCBUSY == 1) + ; // wait for sync + + TC->CTRLA.reg |= TC_CTRLA_PRESCALER_DIV2; // Set prescaler + while (TC->STATUS.bit.SYNCBUSY == 1) + ; // wait for sync + + TC->CC[0].reg = CC_REG_VAL; // timer frequency + while (TC->STATUS.bit.SYNCBUSY == 1) + ; // wait for sync + + // Interrupts + TC->INTENSET.reg = 0; // disable all interrupts + TC->INTENSET.bit.OVF = 1; // enable overflow + TC->INTENSET.bit.MC0 = 1; // enable compare match to CC0 + + // Enable InterruptVector + NVIC_EnableIRQ(TC4_IRQn); + + // Enable TC + TC->CTRLA.reg |= TC_CTRLA_ENABLE; + while (TC->STATUS.bit.SYNCBUSY == 1) + ; // wait for sync } // TC4 -void TC4_Handler() // also used as PWM timer for D16 & D17 (not used on any OB Samd hand) +void TC4_Handler() // also used as PWM timer for D16 & D17 (not used on any OB + // Samd hand) { - static long timerCount = 0; // main timer counter increments every call of the interrupt - static long motorCount = 0; // time instance variable for motor position control - static long mSecCount = 0; // time instance variable for millisecond counter - - TcCount16* TC = (TcCount16*) TC4; // get timer struct - if (TC->INTFLAG.bit.OVF == 1) // An overflow caused the interrupt - { - TC->INTFLAG.bit.OVF = 1; // writing a one clears the flag ovf flag - - timerCount++; // increment timer counter every period - - // triggered once a millisecond - if((timerCount - mSecCount) >= MILLI_TIME) - { - mSecCount = timerCount; - - if (_ptr2PiggybackFunc) - { - _ptr2PiggybackFunc(); - } - } - - // position control for a single motor - if((timerCount - motorCount) >= MOTOR_CTRL_TIME) - { - motorCount = timerCount; - - if (_ptr2MotorFunc != NULL) - { - _ptr2MotorFunc(); - } - } - } - - if (TC->INTFLAG.bit.MC0 == 1) // a compare to cc0 caused the interrupt - { - TC->INTFLAG.bit.MC0 = 1; // writing a one clears the flag ovf flag - } + static long timerCount = + 0; // main timer counter increments every call of the interrupt + static long motorCount = + 0; // time instance variable for motor position control + static long mSecCount = 0; // time instance variable for millisecond counter + + TcCount16 *TC = (TcCount16 *)TC4; // get timer struct + if (TC->INTFLAG.bit.OVF == 1) // An overflow caused the interrupt + { + TC->INTFLAG.bit.OVF = 1; // writing a one clears the flag ovf flag + + timerCount++; // increment timer counter every period + + // triggered once a millisecond + if ((timerCount - mSecCount) >= MILLI_TIME) { + mSecCount = timerCount; + + if (_ptr2PiggybackFunc) { + _ptr2PiggybackFunc(); + } + } + + // position control for a single motor + if ((timerCount - motorCount) >= MOTOR_CTRL_TIME) { + motorCount = timerCount; + + if (_ptr2MotorFunc != NULL) { + _ptr2MotorFunc(); + } + } + } + + if (TC->INTFLAG.bit.MC0 == 1) // a compare to cc0 caused the interrupt + { + TC->INTFLAG.bit.MC0 = 1; // writing a one clears the flag ovf flag + } } #endif /* defined(ARDUINO_ARCH_SAMD) */ \ No newline at end of file diff --git a/src/timers/samd_FingerTimer.h b/src/timers/samd_FingerTimer.h index 851bc25..d8f39ba 100644 --- a/src/timers/samd_FingerTimer.h +++ b/src/timers/samd_FingerTimer.h @@ -4,10 +4,11 @@ * Created: 02/11/2015 11:12:48 * Author: Olly McBride * - * This work is licensed under the Creative Commons Attribution 4.0 International License. - * To view a copy of this license, visit http://creativecommons.org/licenses/by/4.0/. + * This work is licensed under the Creative Commons Attribution 4.0 + * International License. To view a copy of this license, visit + * http://creativecommons.org/licenses/by/4.0/. * - */ + */ #if defined(ARDUINO_ARCH_SAMD) @@ -17,40 +18,39 @@ #include // select timer frequency -//#define TIMER_10KHZ -//#define TIMER_5KHZ +// #define TIMER_10KHZ +// #define TIMER_5KHZ #define TIMER_2KHZ -//#define TIMER_1KHZ -//#define TIMER_500HZ - +// #define TIMER_1KHZ +// #define TIMER_500HZ #if defined(TIMER_10KHZ) -#define TIMER_FREQ 10 // timer frequency in KHz -#define CC_REG_VAL 2397 // compare capture reg val +#define TIMER_FREQ 10 // timer frequency in KHz +#define CC_REG_VAL 2397 // compare capture reg val #elif defined(TIMER_5KHZ) -#define TIMER_FREQ 5 // timer frequency in KHz -#define CC_REG_VAL 4794 // compare capture reg val +#define TIMER_FREQ 5 // timer frequency in KHz +#define CC_REG_VAL 4794 // compare capture reg val #elif defined(TIMER_2KHZ) -#define TIMER_FREQ 2 // timer frequency in KHz -#define CC_REG_VAL 11985 // compare capture reg val +#define TIMER_FREQ 2 // timer frequency in KHz +#define CC_REG_VAL 11985 // compare capture reg val #elif defined(TIMER_1KHZ) -#define TIMER_FREQ 1 // timer frequency in KHz -#define CC_REG_VAL 23970 // compare capture reg val +#define TIMER_FREQ 1 // timer frequency in KHz +#define CC_REG_VAL 23970 // compare capture reg val #elif defined(TIMER_500HZ) -#define TIMER_FREQ 0.5 // timer frequency in KHz -#define CC_REG_VAL 47940 // compare capture reg val +#define TIMER_FREQ 0.5 // timer frequency in KHz +#define CC_REG_VAL 47940 // compare capture reg val #endif -#define ms(val) ((val)*(TIMER_FREQ)) // number of timer ticks per ms +#define ms(val) ((val) * (TIMER_FREQ)) // number of timer ticks per ms // calculate number of timer ticks per ms for each timer function -#define MILLI_TIME ms(1) // 1ms 1kHz -#define MOTOR_CTRL_TIME ms(5) // 5ms 200Hz +#define MILLI_TIME ms(1) // 1ms 1kHz +#define MOTOR_CTRL_TIME ms(5) // 5ms 200Hz // function prototypes void _posCtrlTimerSetup(void); void TC4_Handler(); -//long customMillis(void); +// long customMillis(void); void _passMotorPtr(void (*f)(void)); void _attachFuncToTimer(void (*f)(void)); diff --git a/src/timers/timer_and_delay.cpp b/src/timers/timer_and_delay.cpp index 810f2f5..a44e13a 100644 --- a/src/timers/timer_and_delay.cpp +++ b/src/timers/timer_and_delay.cpp @@ -1,235 +1,177 @@ /* -* timer_and_delay.cpp -* -* Created: 28/09/2016 10:57:02 -* Author: Olly McBride -* -* This work is licensed under the Creative Commons Attribution 4.0 International License. -* To view a copy of this license, visit http://creativecommons.org/licenses/by/4.0/. -* -*/ + * timer_and_delay.cpp + * + * Created: 28/09/2016 10:57:02 + * Author: Olly McBride + * + * This work is licensed under the Creative Commons Attribution 4.0 + * International License. To view a copy of this license, visit + * http://creativecommons.org/licenses/by/4.0/. + * + */ #include "timer_and_delay.h" ////////////////////// NON_BLOCKING DELAY CLASS ////////////////////////// -NB_DELAY_CLASS::NB_DELAY_CLASS() -{ - _started = false; - _startTime = 0; +NB_DELAY_CLASS::NB_DELAY_CLASS() { + _started = false; + _startTime = 0; } -NB_DELAY_CLASS::~NB_DELAY_CLASS() -{ - -} +NB_DELAY_CLASS::~NB_DELAY_CLASS() {} // start non-blocking delay -void NB_DELAY_CLASS::start(long delVal) -{ - _startTime = ticker(); - _interval = delVal; - _started = true; +void NB_DELAY_CLASS::start(long delVal) { + _startTime = ticker(); + _interval = delVal; + _started = true; } // returns true if _interval has elapsed -bool NB_DELAY_CLASS::finished(void) -{ - if (((ticker() - _startTime) >= _interval) && _started) - { - _started = false; - return true; - } - else - { - return false; - } +bool NB_DELAY_CLASS::finished(void) { + if (((ticker() - _startTime) >= _interval) && _started) { + _started = false; + return true; + } else { + return false; + } } // returns true if timer is running -bool NB_DELAY_CLASS::started(void) -{ - return _started; -} +bool NB_DELAY_CLASS::started(void) { return _started; } // returns elapsed time, if not started return 0 -long NB_DELAY_CLASS::now(void) -{ - if (!_started) - { - return 0; - } - else - { - return (ticker() - _startTime); - } +long NB_DELAY_CLASS::now(void) { + if (!_started) { + return 0; + } else { + return (ticker() - _startTime); + } } // stop the timer and return elapsed time -long NB_DELAY_CLASS::stop(void) -{ - long elapsed = now(); +long NB_DELAY_CLASS::stop(void) { + long elapsed = now(); - _started = false; + _started = false; - return elapsed; + return elapsed; } // run the timer constantly, return true every time the delVal has elapsed -bool NB_DELAY_CLASS::timeElapsed(long delVal) -{ - if (!started()) // if timer is not started (first run of timer) - { - start(delVal); // start timer - return false; // duration has not elapsed, so return false - } - if (finished()) // if timer is finished - { - start(delVal); // restart timer - return true; // duration has elapsed, so restart timer and return true - } - else // else if timer has started but not finished - { - return false; // duration has not elapsed, so return false - } +bool NB_DELAY_CLASS::timeElapsed(long delVal) { + if (!started()) // if timer is not started (first run of timer) + { + start(delVal); // start timer + return false; // duration has not elapsed, so return false + } + if (finished()) // if timer is finished + { + start(delVal); // restart timer + return true; // duration has elapsed, so restart timer and return true + } else // else if timer has started but not finished + { + return false; // duration has not elapsed, so return false + } } // return the interval/delVal currently being used -long NB_DELAY_CLASS::getInterval(void) -{ - return _interval; -} - -// function used to increment the timer after a particular period (us, ms, seconds etc) -unsigned long MS_NB_DELAY::ticker(void) -{ - return millis(); -} - -// function used to increment the timer after a particular period (us, ms, seconds etc) -unsigned long US_NB_DELAY::ticker(void) -{ - return micros(); -} +long NB_DELAY_CLASS::getInterval(void) { return _interval; } +// function used to increment the timer after a particular period (us, ms, +// seconds etc) +unsigned long MS_NB_DELAY::ticker(void) { return millis(); } +// function used to increment the timer after a particular period (us, ms, +// seconds etc) +unsigned long US_NB_DELAY::ticker(void) { return micros(); } ////////////////////// NON_BLOCKING TIMER CLASS ////////////////////////// -NB_TIMER_CLASS::NB_TIMER_CLASS() -{ - _startTime = 0; - _started = false; +NB_TIMER_CLASS::NB_TIMER_CLASS() { + _startTime = 0; + _started = false; - _pauseTime = 0; - _paused = false; + _pauseTime = 0; + _paused = false; } -NB_TIMER_CLASS::~NB_TIMER_CLASS() -{ - -} +NB_TIMER_CLASS::~NB_TIMER_CLASS() {} // start non-blocking timer -void NB_TIMER_CLASS::start(void) -{ - _startTime = ticker(); - _started = true; - _paused = false; +void NB_TIMER_CLASS::start(void) { + _startTime = ticker(); + _started = true; + _paused = false; } -// returns true if delay timer is running -bool NB_TIMER_CLASS::started(void) -{ - return _started; -} +// returns true if delay timer is running +bool NB_TIMER_CLASS::started(void) { return _started; } // returns how much time has elapsed since the start, if not started return 0 -long NB_TIMER_CLASS::now(void) -{ - if (!_started) - { - return 0; - } - else if (_paused) - { - return (_pauseTime - _startTime); - } - else - { - return (ticker() - _startTime); - } +long NB_TIMER_CLASS::now(void) { + if (!_started) { + return 0; + } else if (_paused) { + return (_pauseTime - _startTime); + } else { + return (ticker() - _startTime); + } } // stop the timer and return elapsed time -long NB_TIMER_CLASS::stop(void) -{ - long elapsed = now(); +long NB_TIMER_CLASS::stop(void) { + long elapsed = now(); - _started = false; + _started = false; - return elapsed; + return elapsed; } -// stop the timer, return the elapsed time and start the timer again -long NB_TIMER_CLASS::restart(void) -{ - long elapsed = stop(); - start(); - return elapsed; +// stop the timer, return the elapsed time and start the timer again +long NB_TIMER_CLASS::restart(void) { + long elapsed = stop(); + start(); + return elapsed; } // check whether a certain interval has elapsed, keep timer running -bool NB_TIMER_CLASS::timeElapsed(long interval) -{ - if (now() >= interval) - { - return true; - } - else - { - return false; - } +bool NB_TIMER_CLASS::timeElapsed(long interval) { + if (now() >= interval) { + return true; + } else { + return false; + } } // pause the timer -long NB_TIMER_CLASS::pause(void) -{ - if (!_started) - { - return 0; - } +long NB_TIMER_CLASS::pause(void) { + if (!_started) { + return 0; + } - _pauseTime = ticker(); - _paused = true; + _pauseTime = ticker(); + _paused = true; - return (_pauseTime - _startTime); + return (_pauseTime - _startTime); } // return true of the timer is currently paused -bool NB_TIMER_CLASS::paused() -{ - return _paused; -} +bool NB_TIMER_CLASS::paused() { return _paused; } // resume the timer -long NB_TIMER_CLASS::resume(void) -{ - _startTime += (ticker() - _pauseTime); - _paused = false; +long NB_TIMER_CLASS::resume(void) { + _startTime += (ticker() - _pauseTime); + _paused = false; - return now(); + return now(); } +// function used to increment the timer after a particular period (us, ms, +// seconds etc) +unsigned long MS_NB_TIMER::ticker(void) { return millis(); } -// function used to increment the timer after a particular period (us, ms, seconds etc) -unsigned long MS_NB_TIMER::ticker(void) -{ - return millis(); -} - -// function used to increment the timer after a particular period (us, ms, seconds etc) -unsigned long US_NB_TIMER::ticker(void) -{ - return micros(); -} \ No newline at end of file +// function used to increment the timer after a particular period (us, ms, +// seconds etc) +unsigned long US_NB_TIMER::ticker(void) { return micros(); } \ No newline at end of file diff --git a/src/timers/timer_and_delay.h b/src/timers/timer_and_delay.h index d2468d8..b4c70a0 100644 --- a/src/timers/timer_and_delay.h +++ b/src/timers/timer_and_delay.h @@ -4,143 +4,142 @@ * Created: 28/09/2016 10:57:02 * Author: Olly McBride * - * This work is licensed under the Creative Commons Attribution 4.0 International License. - * To view a copy of this license, visit http://creativecommons.org/licenses/by/4.0/. + * This work is licensed under the Creative Commons Attribution 4.0 + * International License. To view a copy of this license, visit + * http://creativecommons.org/licenses/by/4.0/. * - */ - + */ #ifndef TIMER_AND_DELAY_H_ #define TIMER_AND_DELAY_H_ #include - - - /** +/** * # Non-blocking delay base class * - * The timer and delay unit is designed to indicate elapsed duration, and can be used as - * a non-blocking delay function or to measure the amount of time a process has taken. - * The timer and delay classes are compatible with both milliseconds and microseconds by - * creating instances of any of the following; + * The timer and delay unit is designed to indicate elapsed duration, and can be + * used as a non-blocking delay function or to measure the amount of time a + * process has taken. The timer and delay classes are compatible with both + * milliseconds and microseconds by creating instances of any of the following; * * 1. MS_NB_DELAY - millisecond delay * 2. US_NB_DELAY - microsecond delay * 3. MS_NB_TIMER - millisecond timer * 4. US_NB_TIMER - microsecond timer * - * The timer and delay classes use a virtual 'ticker()' function that is used for the - * duration calculation. This ticker() function is set to return a timestamp in millisecond - * or microseconds, but can also be spoofed by the unit test process. + * The timer and delay classes use a virtual 'ticker()' function that is used + * for the duration calculation. This ticker() function is set to return a + * timestamp in millisecond or microseconds, but can also be spoofed by the unit + * test process. * */ - -class NB_DELAY_CLASS -{ - public: - NB_DELAY_CLASS(); - ~NB_DELAY_CLASS(); - - void start(long delVal); /**< start non-blocking delay */ - bool finished(void); /**< returns true if delVal has elapsed */ - bool started(void); /**< returns true if delay timer is running */ - long now(void); /**< returns elapsed time, if not started return 0 */ - long stop(void); /**< stop the timer and return elapsed time */ - - bool timeElapsed(long delVal); /**< run the timer constantly, return true every time the delVal has elapsed */ - - long getInterval(void); /**< return the interval/delVal currently being used */ - - private: - long _startTime; /**< time at which the delay was started */ - long _interval; /**< amount of time the delay will last for */ - bool _started; /**< flag to indicate if the delay has started */ - - virtual unsigned long ticker(void) /**< virtual function used to track the change in time (us, ms, seconds etc) */ - { - return 0; - } +class NB_DELAY_CLASS { +public: + NB_DELAY_CLASS(); + ~NB_DELAY_CLASS(); + + void start(long delVal); /**< start non-blocking delay */ + bool finished(void); /**< returns true if delVal has elapsed */ + bool started(void); /**< returns true if delay timer is running */ + long now(void); /**< returns elapsed time, if not started return 0 */ + long stop(void); /**< stop the timer and return elapsed time */ + + bool timeElapsed(long delVal); /**< run the timer constantly, return true + every time the delVal has elapsed */ + + long + getInterval(void); /**< return the interval/delVal currently being used */ + +private: + long _startTime; /**< time at which the delay was started */ + long _interval; /**< amount of time the delay will last for */ + bool _started; /**< flag to indicate if the delay has started */ + + virtual unsigned long ticker(void) /**< virtual function used to track the + change in time (us, ms, seconds etc) */ + { + return 0; + } }; /*! Millisecond non-blocking delay class */ -class MS_NB_DELAY : public NB_DELAY_CLASS -{ - public: - unsigned long ticker(void); +class MS_NB_DELAY : public NB_DELAY_CLASS { +public: + unsigned long ticker(void); }; /*! Microsecond non-blocking delay class */ -class US_NB_DELAY : public NB_DELAY_CLASS -{ - public: - unsigned long ticker(void); +class US_NB_DELAY : public NB_DELAY_CLASS { +public: + unsigned long ticker(void); }; - /** -* # Non-blocking timer base class -* -* The timer and delay unit is designed to indicate elapsed duration, and can be used as -* a non-blocking delay function or to measure the amount of time a process has taken. -* The timer and delay classes are compatible with both milliseconds and microseconds by -* creating instances of any of the following; -* -* 1. MS_NB_DELAY - millisecond delay -* 2. US_NB_DELAY - microsecond delay -* 3. MS_NB_TIMER - millisecond timer -* 4. US_NB_TIMER - microsecond timer -* -* The timer and delay classes use a virtual 'ticker()' function that is used for the -* duration calculation. This ticker() function is set to return a timestamp in millisecond -* or microseconds, but can also be spoofed by the unit test process. -* -*/ -class NB_TIMER_CLASS -{ - public: - NB_TIMER_CLASS(); - ~NB_TIMER_CLASS(); - - void start(void); /**< start non-blocking timer */ - bool started(void); /**< returns true if delay timer is running */ - long now(void); /**< returns how much time has elapsed since the start, if not started return 0 */ - long stop(void); /**< stop the timer and return elapsed time */ - long restart(void); /**< stop the timer, return the elapsed time and start the timer again */ - - bool timeElapsed(long interval);/**< check whether a certain interval has elapsed, keep timer running */ - - long pause(void); /**< pause the timer */ - bool paused(void); /**< return true of the timer is currently paused */ - long resume(void); /**< resume the timer */ - - private: - long _startTime; /**< time at which the timer was started */ - bool _started = false; /**< flag to indicate if the timer has started */ - - - long _pauseTime; /**< time at which the timer was paused */ - bool _paused; /**< flag to indicate if the timer is paused */ - - virtual unsigned long ticker(void) /**< function used to increment the timer after a particular period (us, ms, seconds etc) */ - { - return 0; - } + * # Non-blocking timer base class + * + * The timer and delay unit is designed to indicate elapsed duration, and can be + * used as a non-blocking delay function or to measure the amount of time a + * process has taken. The timer and delay classes are compatible with both + * milliseconds and microseconds by creating instances of any of the following; + * + * 1. MS_NB_DELAY - millisecond delay + * 2. US_NB_DELAY - microsecond delay + * 3. MS_NB_TIMER - millisecond timer + * 4. US_NB_TIMER - microsecond timer + * + * The timer and delay classes use a virtual 'ticker()' function that is used + * for the duration calculation. This ticker() function is set to return a + * timestamp in millisecond or microseconds, but can also be spoofed by the unit + * test process. + * + */ +class NB_TIMER_CLASS { +public: + NB_TIMER_CLASS(); + ~NB_TIMER_CLASS(); + + void start(void); /**< start non-blocking timer */ + bool started(void); /**< returns true if delay timer is running */ + long now(void); /**< returns how much time has elapsed since the start, if not + started return 0 */ + long stop(void); /**< stop the timer and return elapsed time */ + long restart(void); /**< stop the timer, return the elapsed time and start the + timer again */ + + bool timeElapsed(long interval); /**< check whether a certain interval has + elapsed, keep timer running */ + + long pause(void); /**< pause the timer */ + bool paused(void); /**< return true of the timer is currently paused */ + long resume(void); /**< resume the timer */ + +private: + long _startTime; /**< time at which the timer was started */ + bool _started = false; /**< flag to indicate if the timer has started */ + + long _pauseTime; /**< time at which the timer was paused */ + bool _paused; /**< flag to indicate if the timer is paused */ + + virtual unsigned long + ticker(void) /**< function used to increment the timer after a particular + period (us, ms, seconds etc) */ + { + return 0; + } }; /*! Millisecond non-blocking timer class */ -class MS_NB_TIMER : public NB_TIMER_CLASS -{ - public: - unsigned long ticker(void); +class MS_NB_TIMER : public NB_TIMER_CLASS { +public: + unsigned long ticker(void); }; /*! Microsecond non-blocking timer class */ -class US_NB_TIMER : public NB_TIMER_CLASS -{ - public: - unsigned long ticker(void); +class US_NB_TIMER : public NB_TIMER_CLASS { +public: + unsigned long ticker(void); }; #endif /* TIMER_AND_DELAY_H_ */ \ No newline at end of file