├── .gitattributes ├── .gitignore ├── Libraries ├── Dagu4Motor │ ├── Dagu4Motor.cpp │ ├── Dagu4Motor.h │ ├── Encoder.cpp │ ├── Encoder.h │ ├── keywords.txt │ └── util │ │ ├── direct_pin_read.h │ │ ├── interrupt_config.h │ │ └── interrupt_pins.h └── README.md └── README.md /.gitattributes: -------------------------------------------------------------------------------- 1 | # Auto detect text files and perform LF normalization 2 | * text=auto 3 | 4 | # Custom for Visual Studio 5 | *.cs diff=csharp 6 | *.sln merge=union 7 | *.csproj merge=union 8 | *.vbproj merge=union 9 | *.fsproj merge=union 10 | *.dbproj merge=union 11 | 12 | # Standard to msysgit 13 | *.doc diff=astextplain 14 | *.DOC diff=astextplain 15 | *.docx diff=astextplain 16 | *.DOCX diff=astextplain 17 | *.dot diff=astextplain 18 | *.DOT diff=astextplain 19 | *.pdf diff=astextplain 20 | *.PDF diff=astextplain 21 | *.rtf diff=astextplain 22 | *.RTF diff=astextplain 23 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | ################# 2 | ## Eclipse 3 | ################# 4 | 5 | *.pydevproject 6 | .project 7 | .metadata 8 | bin/ 9 | tmp/ 10 | *.tmp 11 | *.bak 12 | *.swp 13 | *~.nib 14 | local.properties 15 | .classpath 16 | .settings/ 17 | .loadpath 18 | 19 | # External tool builders 20 | .externalToolBuilders/ 21 | 22 | # Locally stored "Eclipse launch configurations" 23 | *.launch 24 | 25 | # CDT-specific 26 | .cproject 27 | 28 | # PDT-specific 29 | .buildpath 30 | 31 | 32 | ################# 33 | ## Visual Studio 34 | ################# 35 | 36 | ## Ignore Visual Studio temporary files, build results, and 37 | ## files generated by popular Visual Studio add-ons. 38 | 39 | # User-specific files 40 | *.suo 41 | *.user 42 | *.sln.docstates 43 | 44 | # Build results 45 | 46 | [Dd]ebug/ 47 | [Rr]elease/ 48 | x64/ 49 | build/ 50 | [Bb]in/ 51 | [Oo]bj/ 52 | 53 | # MSTest test Results 54 | [Tt]est[Rr]esult*/ 55 | [Bb]uild[Ll]og.* 56 | 57 | *_i.c 58 | *_p.c 59 | *.ilk 60 | *.meta 61 | *.obj 62 | *.pch 63 | *.pdb 64 | *.pgc 65 | *.pgd 66 | *.rsp 67 | *.sbr 68 | *.tlb 69 | *.tli 70 | *.tlh 71 | *.tmp 72 | *.tmp_proj 73 | *.log 74 | *.vspscc 75 | *.vssscc 76 | .builds 77 | *.pidb 78 | *.log 79 | *.scc 80 | 81 | # Visual C++ cache files 82 | ipch/ 83 | *.aps 84 | *.ncb 85 | *.opensdf 86 | *.sdf 87 | *.cachefile 88 | 89 | # Visual Studio profiler 90 | *.psess 91 | *.vsp 92 | *.vspx 93 | 94 | # Guidance Automation Toolkit 95 | *.gpState 96 | 97 | # ReSharper is a .NET coding add-in 98 | _ReSharper*/ 99 | *.[Rr]e[Ss]harper 100 | 101 | # TeamCity is a build add-in 102 | _TeamCity* 103 | 104 | # DotCover is a Code Coverage Tool 105 | *.dotCover 106 | 107 | # NCrunch 108 | *.ncrunch* 109 | .*crunch*.local.xml 110 | 111 | # Installshield output folder 112 | [Ee]xpress/ 113 | 114 | # DocProject is a documentation generator add-in 115 | DocProject/buildhelp/ 116 | DocProject/Help/*.HxT 117 | DocProject/Help/*.HxC 118 | DocProject/Help/*.hhc 119 | DocProject/Help/*.hhk 120 | DocProject/Help/*.hhp 121 | DocProject/Help/Html2 122 | DocProject/Help/html 123 | 124 | # Click-Once directory 125 | publish/ 126 | 127 | # Publish Web Output 128 | *.Publish.xml 129 | *.pubxml 130 | 131 | # NuGet Packages Directory 132 | ## TODO: If you have NuGet Package Restore enabled, uncomment the next line 133 | #packages/ 134 | 135 | # Windows Azure Build Output 136 | csx 137 | *.build.csdef 138 | 139 | # Windows Store app package directory 140 | AppPackages/ 141 | 142 | # Others 143 | sql/ 144 | *.Cache 145 | ClientBin/ 146 | [Ss]tyle[Cc]op.* 147 | ~$* 148 | *~ 149 | *.dbmdl 150 | *.[Pp]ublish.xml 151 | *.pfx 152 | *.publishsettings 153 | 154 | # RIA/Silverlight projects 155 | Generated_Code/ 156 | 157 | # Backup & report files from converting an old project file to a newer 158 | # Visual Studio version. Backup files are not needed, because we have git ;-) 159 | _UpgradeReport_Files/ 160 | Backup*/ 161 | UpgradeLog*.XML 162 | UpgradeLog*.htm 163 | 164 | # SQL Server files 165 | App_Data/*.mdf 166 | App_Data/*.ldf 167 | 168 | ############# 169 | ## Windows detritus 170 | ############# 171 | 172 | # Windows image file caches 173 | Thumbs.db 174 | ehthumbs.db 175 | 176 | # Folder config file 177 | Desktop.ini 178 | 179 | # Recycle Bin used on file shares 180 | $RECYCLE.BIN/ 181 | 182 | # Mac crap 183 | .DS_Store 184 | 185 | 186 | ############# 187 | ## Python 188 | ############# 189 | 190 | *.py[co] 191 | 192 | # Packages 193 | *.egg 194 | *.egg-info 195 | dist/ 196 | build/ 197 | eggs/ 198 | parts/ 199 | var/ 200 | sdist/ 201 | develop-eggs/ 202 | .installed.cfg 203 | 204 | # Installer logs 205 | pip-log.txt 206 | 207 | # Unit test / coverage reports 208 | .coverage 209 | .tox 210 | 211 | #Translations 212 | *.mo 213 | 214 | #Mr Developer 215 | .mr.developer.cfg 216 | -------------------------------------------------------------------------------- /Libraries/Dagu4Motor/Dagu4Motor.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Dagu4Motor.cpp - Library for driving the Dagu4Motor Driver code. 3 | Created by William Garrdio 4 | Created on: 02/03/2012 5 | 02/25/2012 - Rewrite class to handle 1 motor per instance 6 | 7 | */ 8 | #include "Arduino.h" 9 | #include "Dagu4Motor.h" 10 | 11 | 12 | Dagu4Motor::Dagu4Motor(int pwmPin, int dirPin, int currPin, int encAPin, int encBPin) : enc(encAPin, encBPin) 13 | { 14 | _pwm = pwmPin; 15 | _dir = dirPin; 16 | _curr = currPin; 17 | _currRate = 0; 18 | _encA = encAPin; 19 | _encB = encBPin; 20 | _distance = 0; 21 | _speed = 0; 22 | 23 | } 24 | 25 | void Dagu4Motor::begin() 26 | { 27 | 28 | 29 | pinMode(_pwm, OUTPUT); 30 | pinMode(_dir, OUTPUT); 31 | pinMode(_curr, INPUT); 32 | 33 | } 34 | 35 | void Dagu4Motor::stopMotors() 36 | { 37 | digitalWrite(_pwm, LOW); 38 | setSpeed(0); 39 | } 40 | 41 | void Dagu4Motor::setSpeed(int speedMotor) 42 | { 43 | speedMotor = constrain(speedMotor, 0, 255); 44 | 45 | analogWrite(_pwm, speedMotor); 46 | _speed=speedMotor; 47 | 48 | } 49 | 50 | void Dagu4Motor::setMotorDirection(bool isMotor) 51 | { 52 | 53 | if(isMotor) 54 | { 55 | digitalWrite(_dir, LOW); //Set motor direction, 1 low, 2 high 56 | } 57 | else 58 | { 59 | digitalWrite(_dir, HIGH); //Reverse motor direction, 1 high, 2 low 60 | } 61 | 62 | 63 | } 64 | 65 | int Dagu4Motor::getCurrent() 66 | { 67 | 68 | _currRate = analogRead(_curr); 69 | 70 | return _currRate; 71 | } 72 | 73 | float Dagu4Motor::getDistance() 74 | { 75 | 76 | _distance = enc.read(); 77 | _distance = (((_distance/333.33)*7.8)/12); 78 | 79 | return _distance; 80 | } 81 | 82 | 83 | int Dagu4Motor::getSpeed() 84 | { 85 | 86 | 87 | return _speed; 88 | } 89 | 90 | long int Dagu4Motor::getTicks() 91 | { 92 | 93 | _ticks = enc.read(); 94 | 95 | return _ticks; 96 | } 97 | 98 | -------------------------------------------------------------------------------- /Libraries/Dagu4Motor/Dagu4Motor.h: -------------------------------------------------------------------------------- 1 | /* 2 | ************************************************************************************* 3 | Dagu4Motor.h - Library for driving the Dagu4Motor Driver code. 4 | Created by William Garrdio 5 | Created: 02/03/2012 6 | ************************************************************************************* 7 | Change Log: 8 | 02/25/2012 - WG - Added new class for encoders 9 | 02/25/2012 - WG - Rewrite class to handle 1 motor per instance 10 | 02/28/2020 - WG - Add calculation to distance function, added ticks function 11 | ************************************************************************************* 12 | Todo: 13 | Comment functions 14 | Speed function 15 | ************************************************************************************* 16 | Notes 17 | pwmPin: Digital pin to set motor speed 18 | dirPin: Digital pin to set motor direction 19 | currPin: Analog pin to monitor current usage 20 | encA: Digital pin for encoder A, should be interrupt pin 21 | encB: Digital pin for encoder B 22 | ************************************************************************************* 23 | */ 24 | #ifndef Dagu4Motor_h 25 | #define Dagu4Motor_h 26 | //#define ENCODER_OPTIMIZE_INTERRUPTS //Fails to compile, vector errors 27 | #include "Encoder.h" 28 | 29 | #include "Arduino.h" 30 | 31 | class Dagu4Motor 32 | { 33 | public: 34 | Dagu4Motor(int pwmPin, int dirPin, int currPin, int encAPin, int encBPin); 35 | void begin(); 36 | void stopMotors(); 37 | void setSpeed(int speedMotor); 38 | void setMotorDirection(bool isMotor); 39 | int getCurrent(); 40 | float getDistance(); 41 | int getSpeed(); 42 | long int getTicks(); 43 | 44 | 45 | 46 | private: 47 | int _pwm; 48 | int _dir; 49 | int _curr; 50 | int _currRate; 51 | int _encA; 52 | int _encB; 53 | float _distance; 54 | int _speed; 55 | long int _ticks; 56 | Encoder enc; 57 | 58 | 59 | 60 | }; 61 | 62 | 63 | #endif -------------------------------------------------------------------------------- /Libraries/Dagu4Motor/Encoder.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "Encoder.h" 3 | 4 | // Yes, all the code is in the header file, to provide the user 5 | // configure options with #define (before they include it), and 6 | // to facilitate some crafty optimizations! 7 | 8 | Encoder_internal_state_t * Encoder::interruptArgs[]; 9 | 10 | 11 | -------------------------------------------------------------------------------- /Libraries/Dagu4Motor/Encoder.h: -------------------------------------------------------------------------------- 1 | /* Encoder Library, for measuring quadrature encoded signals 2 | * http://www.pjrc.com/teensy/td_libs_Encoder.html 3 | * Copyright (c) 2011 PJRC.COM, LLC - Paul Stoffregen 4 | * 5 | * Version 1.0 6 | * 7 | * Permission is hereby granted, free of charge, to any person obtaining a copy 8 | * of this software and associated documentation files (the "Software"), to deal 9 | * in the Software without restriction, including without limitation the rights 10 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 11 | * copies of the Software, and to permit persons to whom the Software is 12 | * furnished to do so, subject to the following conditions: 13 | * 14 | * The above copyright notice and this permission notice shall be included in 15 | * all copies or substantial portions of the Software. 16 | * 17 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 22 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 23 | * THE SOFTWARE. 24 | */ 25 | 26 | 27 | #ifndef Encoder_h_ 28 | #define Encoder_h_ 29 | 30 | #if defined(ARDUINO) && ARDUINO >= 100 31 | #include "Arduino.h" 32 | #elif defined(WIRING) 33 | #include "Wiring.h" 34 | #else 35 | #include "WProgram.h" 36 | #include "pins_arduino.h" 37 | #endif 38 | 39 | #include "util/direct_pin_read.h" 40 | 41 | #if defined(ENCODER_USE_INTERRUPTS) || !defined(ENCODER_DO_NOT_USE_INTERRUPTS) 42 | #define ENCODER_USE_INTERRUPTS 43 | #define ENCODER_ARGLIST_SIZE CORE_NUM_INTERRUPT 44 | #include "util/interrupt_pins.h" 45 | #ifdef ENCODER_OPTIMIZE_INTERRUPTS 46 | #include "util/interrupt_config.h" 47 | #endif 48 | #else 49 | #define ENCODER_ARGLIST_SIZE 0 50 | #endif 51 | 52 | 53 | 54 | // All the data needed by interrupts is consolidated into this ugly struct 55 | // to facilitate assembly language optimizing of the speed critical update. 56 | // The assembly code uses auto-incrementing addressing modes, so the struct 57 | // must remain in exactly this order. 58 | typedef struct { 59 | volatile IO_REG_TYPE * pin1_register; 60 | volatile IO_REG_TYPE * pin2_register; 61 | IO_REG_TYPE pin1_bitmask; 62 | IO_REG_TYPE pin2_bitmask; 63 | uint8_t state; 64 | int32_t position; 65 | } Encoder_internal_state_t; 66 | 67 | class Encoder 68 | { 69 | public: 70 | Encoder(uint8_t pin1, uint8_t pin2) { 71 | #ifdef INPUT_PULLUP 72 | pinMode(pin1, INPUT_PULLUP); 73 | pinMode(pin2, INPUT_PULLUP); 74 | #else 75 | pinMode(pin1, INPUT); 76 | digitalWrite(pin1, HIGH); 77 | pinMode(pin2, INPUT); 78 | digitalWrite(pin2, HIGH); 79 | #endif 80 | encoder.pin1_register = PIN_TO_BASEREG(pin1); 81 | encoder.pin1_bitmask = PIN_TO_BITMASK(pin1); 82 | encoder.pin2_register = PIN_TO_BASEREG(pin2); 83 | encoder.pin2_bitmask = PIN_TO_BITMASK(pin2); 84 | encoder.position = 0; 85 | // allow time for a passive R-C filter to charge 86 | // through the pullup resistors, before reading 87 | // the initial state 88 | delayMicroseconds(2000); 89 | uint8_t s = 0; 90 | if (DIRECT_PIN_READ(encoder.pin1_register, encoder.pin1_bitmask)) s |= 1; 91 | if (DIRECT_PIN_READ(encoder.pin2_register, encoder.pin2_bitmask)) s |= 2; 92 | encoder.state = s; 93 | #ifdef ENCODER_USE_INTERRUPTS 94 | interrupts_in_use = 0; 95 | switch (pin1) { 96 | #ifdef CORE_INT0_PIN 97 | case CORE_INT0_PIN: 98 | interruptArgs[0] = &encoder; 99 | attachInterrupt(0, isr0, CHANGE); 100 | break; 101 | #endif 102 | #ifdef CORE_INT1_PIN 103 | case CORE_INT1_PIN: 104 | interruptArgs[1] = &encoder; 105 | attachInterrupt(1, isr1, CHANGE); 106 | break; 107 | #endif 108 | #ifdef CORE_INT2_PIN 109 | case CORE_INT2_PIN: 110 | interruptArgs[2] = &encoder; 111 | attachInterrupt(2, isr2, CHANGE); 112 | break; 113 | #endif 114 | #ifdef CORE_INT3_PIN 115 | case CORE_INT3_PIN: 116 | interruptArgs[3] = &encoder; 117 | attachInterrupt(3, isr3, CHANGE); 118 | break; 119 | #endif 120 | #ifdef CORE_INT4_PIN 121 | case CORE_INT4_PIN: 122 | interruptArgs[4] = &encoder; 123 | attachInterrupt(4, isr4, CHANGE); 124 | break; 125 | #endif 126 | #ifdef CORE_INT5_PIN 127 | case CORE_INT5_PIN: 128 | interruptArgs[5] = &encoder; 129 | attachInterrupt(5, isr5, CHANGE); 130 | break; 131 | #endif 132 | #ifdef CORE_INT6_PIN 133 | case CORE_INT6_PIN: 134 | interruptArgs[6] = &encoder; 135 | attachInterrupt(6, isr6, CHANGE); 136 | break; 137 | #endif 138 | #ifdef CORE_INT7_PIN 139 | case CORE_INT7_PIN: 140 | interruptArgs[7] = &encoder; 141 | attachInterrupt(7, isr7, CHANGE); 142 | break; 143 | #endif 144 | default: 145 | return; 146 | } 147 | interrupts_in_use++; 148 | 149 | switch (pin2) { 150 | #ifdef CORE_INT0_PIN 151 | case CORE_INT0_PIN: 152 | interruptArgs[0] = &encoder; 153 | attachInterrupt(0, isr0, CHANGE); 154 | break; 155 | #endif 156 | #ifdef CORE_INT1_PIN 157 | case CORE_INT1_PIN: 158 | interruptArgs[1] = &encoder; 159 | attachInterrupt(1, isr1, CHANGE); 160 | break; 161 | #endif 162 | #ifdef CORE_INT2_PIN 163 | case CORE_INT2_PIN: 164 | interruptArgs[2] = &encoder; 165 | attachInterrupt(2, isr2, CHANGE); 166 | break; 167 | #endif 168 | #ifdef CORE_INT3_PIN 169 | case CORE_INT3_PIN: 170 | interruptArgs[3] = &encoder; 171 | attachInterrupt(3, isr3, CHANGE); 172 | break; 173 | #endif 174 | #ifdef CORE_INT4_PIN 175 | case CORE_INT4_PIN: 176 | interruptArgs[4] = &encoder; 177 | attachInterrupt(4, isr4, CHANGE); 178 | break; 179 | #endif 180 | #ifdef CORE_INT5_PIN 181 | case CORE_INT5_PIN: 182 | interruptArgs[5] = &encoder; 183 | attachInterrupt(5, isr5, CHANGE); 184 | break; 185 | #endif 186 | #ifdef CORE_INT6_PIN 187 | case CORE_INT6_PIN: 188 | interruptArgs[6] = &encoder; 189 | attachInterrupt(6, isr6, CHANGE); 190 | break; 191 | #endif 192 | #ifdef CORE_INT7_PIN 193 | case CORE_INT7_PIN: 194 | interruptArgs[7] = &encoder; 195 | attachInterrupt(7, isr7, CHANGE); 196 | break; 197 | default: 198 | return; 199 | #endif 200 | } 201 | interrupts_in_use++; 202 | #endif 203 | //update_finishup(); // to force linker to include the code 204 | } 205 | #ifdef ENCODER_USE_INTERRUPTS 206 | inline int32_t read() { 207 | if (interrupts_in_use < 2) { 208 | noInterrupts(); 209 | update(&encoder); 210 | } else { 211 | noInterrupts(); 212 | } 213 | int32_t ret = encoder.position; 214 | interrupts(); 215 | return ret; 216 | } 217 | inline void write(int32_t p) { 218 | noInterrupts(); 219 | encoder.position = p; 220 | interrupts(); 221 | } 222 | #else 223 | inline int32_t read() { 224 | update(&encoder); 225 | return encoder.position; 226 | } 227 | inline void write(int32_t p) { 228 | encoder.position = p; 229 | } 230 | #endif 231 | private: 232 | Encoder_internal_state_t encoder; 233 | #ifdef ENCODER_USE_INTERRUPTS 234 | uint8_t interrupts_in_use; 235 | #endif 236 | public: 237 | static Encoder_internal_state_t * interruptArgs[ENCODER_ARGLIST_SIZE]; 238 | 239 | // _______ _______ 240 | // Pin1 ______| |_______| |______ Pin1 241 | // negative <--- _______ _______ __ --> positive 242 | // Pin2 __| |_______| |_______| Pin2 243 | 244 | // new new old old 245 | // pin2 pin1 pin2 pin1 Result 246 | // ---- ---- ---- ---- ------ 247 | // 0 0 0 0 no movement 248 | // 0 0 0 1 +1 249 | // 0 0 1 0 -1 250 | // 0 0 1 1 +2 (assume pin1 edges only) 251 | // 0 1 0 0 -1 252 | // 0 1 0 1 no movement 253 | // 0 1 1 0 -2 (assume pin1 edges only) 254 | // 0 1 1 1 +1 255 | // 1 0 0 0 +1 256 | // 1 0 0 1 -2 (assume pin1 edges only) 257 | // 1 0 1 0 no movement 258 | // 1 0 1 1 -1 259 | // 1 1 0 0 +2 (assume pin1 edges only) 260 | // 1 1 0 1 -1 261 | // 1 1 1 0 +1 262 | // 1 1 1 1 no movement 263 | /* 264 | // Simple, easy-to-read "documentation" version :-) 265 | // 266 | void update(void) { 267 | uint8_t s = state & 3; 268 | if (digitalRead(pin1)) s |= 4; 269 | if (digitalRead(pin2)) s |= 8; 270 | switch (s) { 271 | case 0: case 5: case 10: case 15: 272 | break; 273 | case 1: case 7: case 8: case 14: 274 | position++; break; 275 | case 2: case 4: case 11: case 13: 276 | position--; break; 277 | case 3: case 12: 278 | position += 2; break; 279 | default: 280 | position -= 2; break; 281 | } 282 | state = (s >> 2); 283 | } 284 | */ 285 | static void update(Encoder_internal_state_t *arg) { 286 | #if defined(__AVR__) 287 | // The compiler believes this is just 1 line of code, so 288 | // it will inline this function into each interrupt 289 | // handler. That's a tiny bit faster, but grows the code. 290 | // Especially when used with ENCODER_OPTIMIZE_INTERRUPTS, 291 | // the inline nature allows the ISR prologue and epilogue 292 | // to only save/restore necessary registers, for very nice 293 | // speed increase. 294 | asm volatile ( 295 | "ld r30, X+" "\n\t" 296 | "ld r31, X+" "\n\t" 297 | "ld r24, Z" "\n\t" // r24 = pin1 input 298 | "ld r30, X+" "\n\t" 299 | "ld r31, X+" "\n\t" 300 | "ld r25, Z" "\n\t" // r25 = pin2 input 301 | "ld r30, X+" "\n\t" // r30 = pin1 mask 302 | "ld r31, X+" "\n\t" // r31 = pin2 mask 303 | "ld r22, X" "\n\t" // r22 = state 304 | "andi r22, 3" "\n\t" 305 | "and r24, r30" "\n\t" 306 | "breq L%=1" "\n\t" // if (pin1) 307 | "ori r22, 4" "\n\t" // state |= 4 308 | "L%=1:" "and r25, r31" "\n\t" 309 | "breq L%=2" "\n\t" // if (pin2) 310 | "ori r22, 8" "\n\t" // state |= 8 311 | "L%=2:" "ldi r30, lo8(pm(L%=table))" "\n\t" 312 | "ldi r31, hi8(pm(L%=table))" "\n\t" 313 | // "L%=2:" "ldi r30, lo8(pm(Ltable))" "\n\t" 314 | // "ldi r31, hi8(pm(Ltable))" "\n\t" 315 | "add r30, r22" "\n\t" 316 | "adc r31, __zero_reg__" "\n\t" 317 | "asr r22" "\n\t" 318 | "asr r22" "\n\t" 319 | "st X+, r22" "\n\t" // store new state 320 | "ld r22, X+" "\n\t" 321 | "ld r23, X+" "\n\t" 322 | "ld r24, X+" "\n\t" 323 | "ld r25, X+" "\n\t" 324 | "ijmp" "\n\t" // jumps to update_finishup() 325 | // TODO move this table to another static function, 326 | // so it doesn't get needlessly duplicated. Easier 327 | // said than done, due to linker issues and inlining 328 | "L%=table:" "\n\t" 329 | "rjmp L%=end" "\n\t" // 0 330 | "rjmp L%=plus1" "\n\t" // 1 331 | "rjmp L%=minus1" "\n\t" // 2 332 | "rjmp L%=plus2" "\n\t" // 3 333 | "rjmp L%=minus1" "\n\t" // 4 334 | "rjmp L%=end" "\n\t" // 5 335 | "rjmp L%=minus2" "\n\t" // 6 336 | "rjmp L%=plus1" "\n\t" // 7 337 | "rjmp L%=plus1" "\n\t" // 8 338 | "rjmp L%=minus2" "\n\t" // 9 339 | "rjmp L%=end" "\n\t" // 10 340 | "rjmp L%=minus1" "\n\t" // 11 341 | "rjmp L%=plus2" "\n\t" // 12 342 | "rjmp L%=minus1" "\n\t" // 13 343 | "rjmp L%=plus1" "\n\t" // 14 344 | "rjmp L%=end" "\n\t" // 15 345 | "L%=minus2:" "\n\t" 346 | "subi r22, 2" "\n\t" 347 | "sbci r23, 0" "\n\t" 348 | "sbci r24, 0" "\n\t" 349 | "sbci r25, 0" "\n\t" 350 | "rjmp L%=store" "\n\t" 351 | "L%=minus1:" "\n\t" 352 | "subi r22, 1" "\n\t" 353 | "sbci r23, 0" "\n\t" 354 | "sbci r24, 0" "\n\t" 355 | "sbci r25, 0" "\n\t" 356 | "rjmp L%=store" "\n\t" 357 | "L%=plus2:" "\n\t" 358 | "subi r22, 254" "\n\t" 359 | "rjmp L%=z" "\n\t" 360 | "L%=plus1:" "\n\t" 361 | "subi r22, 255" "\n\t" 362 | "L%=z:" "sbci r23, 255" "\n\t" 363 | "sbci r24, 255" "\n\t" 364 | "sbci r25, 255" "\n\t" 365 | "L%=store:" "\n\t" 366 | "st -X, r25" "\n\t" 367 | "st -X, r24" "\n\t" 368 | "st -X, r23" "\n\t" 369 | "st -X, r22" "\n\t" 370 | "L%=end:" "\n" 371 | : : "x" (arg) : "r22", "r23", "r24", "r25", "r30", "r31"); 372 | #else 373 | uint8_t p1val = DIRECT_PIN_READ(arg->pin1_register, arg->pin1_bitmask); 374 | uint8_t p2val = DIRECT_PIN_READ(arg->pin2_register, arg->pin2_bitmask); 375 | uint8_t state = arg->state & 3; 376 | if (p1val) state |= 4; 377 | if (p2val) state |= 8; 378 | arg->state = (state >> 2); 379 | switch (state) { 380 | case 1: case 7: case 8: case 14: 381 | arg->position++; 382 | return; 383 | case 2: case 4: case 11: case 13: 384 | arg->position--; 385 | return; 386 | case 3: case 12: 387 | arg->position += 2; 388 | return; 389 | case 6: case 9: 390 | arg->position += 2; 391 | return; 392 | } 393 | #endif 394 | } 395 | /* 396 | #if defined(__AVR__) 397 | // TODO: this must be a no inline function 398 | // even noinline does not seem to solve difficult 399 | // problems with this. Oh well, it was only meant 400 | // to shrink code size - there's no performance 401 | // improvement in this, only code size reduction. 402 | __attribute__((noinline)) void update_finishup(void) { 403 | asm volatile ( 404 | "ldi r30, lo8(pm(Ltable))" "\n\t" 405 | "ldi r31, hi8(pm(Ltable))" "\n\t" 406 | "Ltable:" "\n\t" 407 | "rjmp L%=end" "\n\t" // 0 408 | "rjmp L%=plus1" "\n\t" // 1 409 | "rjmp L%=minus1" "\n\t" // 2 410 | "rjmp L%=plus2" "\n\t" // 3 411 | "rjmp L%=minus1" "\n\t" // 4 412 | "rjmp L%=end" "\n\t" // 5 413 | "rjmp L%=minus2" "\n\t" // 6 414 | "rjmp L%=plus1" "\n\t" // 7 415 | "rjmp L%=plus1" "\n\t" // 8 416 | "rjmp L%=minus2" "\n\t" // 9 417 | "rjmp L%=end" "\n\t" // 10 418 | "rjmp L%=minus1" "\n\t" // 11 419 | "rjmp L%=plus2" "\n\t" // 12 420 | "rjmp L%=minus1" "\n\t" // 13 421 | "rjmp L%=plus1" "\n\t" // 14 422 | "rjmp L%=end" "\n\t" // 15 423 | "L%=minus2:" "\n\t" 424 | "subi r22, 2" "\n\t" 425 | "sbci r23, 0" "\n\t" 426 | "sbci r24, 0" "\n\t" 427 | "sbci r25, 0" "\n\t" 428 | "rjmp L%=store" "\n\t" 429 | "L%=minus1:" "\n\t" 430 | "subi r22, 1" "\n\t" 431 | "sbci r23, 0" "\n\t" 432 | "sbci r24, 0" "\n\t" 433 | "sbci r25, 0" "\n\t" 434 | "rjmp L%=store" "\n\t" 435 | "L%=plus2:" "\n\t" 436 | "subi r22, 254" "\n\t" 437 | "rjmp L%=z" "\n\t" 438 | "L%=plus1:" "\n\t" 439 | "subi r22, 255" "\n\t" 440 | "L%=z:" "sbci r23, 255" "\n\t" 441 | "sbci r24, 255" "\n\t" 442 | "sbci r25, 255" "\n\t" 443 | "L%=store:" "\n\t" 444 | "st -X, r25" "\n\t" 445 | "st -X, r24" "\n\t" 446 | "st -X, r23" "\n\t" 447 | "st -X, r22" "\n\t" 448 | "L%=end:" "\n" 449 | : : : "r22", "r23", "r24", "r25", "r30", "r31"); 450 | } 451 | #endif 452 | */ 453 | 454 | #if defined(ENCODER_USE_INTERRUPTS) && !defined(ENCODER_OPTIMIZE_INTERRUPTS) 455 | #ifdef CORE_INT0_PIN 456 | static void isr0(void) { update(interruptArgs[0]); } 457 | #endif 458 | #ifdef CORE_INT1_PIN 459 | static void isr1(void) { update(interruptArgs[1]); } 460 | #endif 461 | #ifdef CORE_INT2_PIN 462 | static void isr2(void) { update(interruptArgs[2]); } 463 | #endif 464 | #ifdef CORE_INT3_PIN 465 | static void isr3(void) { update(interruptArgs[3]); } 466 | #endif 467 | #ifdef CORE_INT4_PIN 468 | static void isr4(void) { update(interruptArgs[4]); } 469 | #endif 470 | #ifdef CORE_INT5_PIN 471 | static void isr5(void) { update(interruptArgs[5]); } 472 | #endif 473 | #ifdef CORE_INT6_PIN 474 | static void isr6(void) { update(interruptArgs[6]); } 475 | #endif 476 | #ifdef CORE_INT7_PIN 477 | static void isr7(void) { update(interruptArgs[7]); } 478 | #endif 479 | #endif 480 | }; 481 | 482 | #if defined(ENCODER_USE_INTERRUPTS) && defined(ENCODER_OPTIMIZE_INTERRUPTS) 483 | #if defined(__AVR__) 484 | #if defined(INT0_vect) && CORE_NUM_INTERRUPT > 0 485 | ISR(INT0_vect) { Encoder::update(Encoder::interruptArgs[SCRAMBLE_INT_ORDER(0)]); } 486 | #endif 487 | #if defined(INT1_vect) && CORE_NUM_INTERRUPT > 1 488 | ISR(INT1_vect) { Encoder::update(Encoder::interruptArgs[SCRAMBLE_INT_ORDER(1)]); } 489 | #endif 490 | #if defined(INT2_vect) && CORE_NUM_INTERRUPT > 2 491 | ISR(INT2_vect) { Encoder::update(Encoder::interruptArgs[SCRAMBLE_INT_ORDER(2)]); } 492 | #endif 493 | #if defined(INT3_vect) && CORE_NUM_INTERRUPT > 3 494 | ISR(INT3_vect) { Encoder::update(Encoder::interruptArgs[SCRAMBLE_INT_ORDER(3)]); } 495 | #endif 496 | #if defined(INT4_vect) && CORE_NUM_INTERRUPT > 4 497 | ISR(INT4_vect) { Encoder::update(Encoder::interruptArgs[SCRAMBLE_INT_ORDER(4)]); } 498 | #endif 499 | #if defined(INT5_vect) && CORE_NUM_INTERRUPT > 5 500 | ISR(INT5_vect) { Encoder::update(Encoder::interruptArgs[SCRAMBLE_INT_ORDER(5)]); } 501 | #endif 502 | #if defined(INT6_vect) && CORE_NUM_INTERRUPT > 6 503 | ISR(INT6_vect) { Encoder::update(Encoder::interruptArgs[SCRAMBLE_INT_ORDER(6)]); } 504 | #endif 505 | #if defined(INT7_vect) && CORE_NUM_INTERRUPT > 7 506 | ISR(INT7_vect) { Encoder::update(Encoder::interruptArgs[SCRAMBLE_INT_ORDER(7)]); } 507 | #endif 508 | #endif // AVR 509 | #endif // ENCODER_OPTIMIZE_INTERRUPTS 510 | 511 | 512 | #endif 513 | -------------------------------------------------------------------------------- /Libraries/Dagu4Motor/keywords.txt: -------------------------------------------------------------------------------- 1 | ####################################### 2 | # Syntax Coloring Map for Dagu4Motor 3 | # by William Garrido 4 | ####################################### 5 | 6 | ####################################### 7 | # Datatypes (KEYWORD1) 8 | ####################################### 9 | 10 | Dagu4Motor KEYWORD1 11 | 12 | 13 | ####################################### 14 | # Methods and Functions (KEYWORD2) 15 | ####################################### 16 | 17 | setMotorDirection KEYWORD2 18 | setSpeed KEYWORD2 19 | stopMotors KEYWORD2 20 | getCurrent KEYWORD2 21 | getDistance KEYWORD2 22 | getSpeed KEYWORD2 23 | 24 | 25 | 26 | 27 | ENCODER_USE_INTERRUPTS LITERAL1 28 | ENCODER_OPTIMIZE_INTERRUPTS LITERAL1 29 | ENCODER_DO_NOT_USE_INTERRUPTS LITERAL1 30 | Encoder KEYWORD1 31 | -------------------------------------------------------------------------------- /Libraries/Dagu4Motor/util/direct_pin_read.h: -------------------------------------------------------------------------------- 1 | #ifndef direct_pin_read_h_ 2 | #define direct_pin_read_h_ 3 | 4 | #if defined(__AVR__) 5 | 6 | #define IO_REG_TYPE uint8_t 7 | #define PIN_TO_BASEREG(pin) (portInputRegister(digitalPinToPort(pin))) 8 | #define PIN_TO_BITMASK(pin) (digitalPinToBitMask(pin)) 9 | #define DIRECT_PIN_READ(base, mask) (((*(base)) & (mask)) ? 1 : 0) 10 | 11 | #elif defined(__PIC32MX__) 12 | 13 | #define IO_REG_TYPE uint32_t 14 | #define PIN_TO_BASEREG(pin) (portModeRegister(digitalPinToPort(pin))) 15 | #define PIN_TO_BITMASK(pin) (digitalPinToBitMask(pin)) 16 | #define DIRECT_PIN_READ(base, mask) (((*(base+4)) & (mask)) ? 1 : 0) 17 | 18 | #endif 19 | 20 | #endif 21 | -------------------------------------------------------------------------------- /Libraries/Dagu4Motor/util/interrupt_config.h: -------------------------------------------------------------------------------- 1 | #if defined(__AVR__) 2 | 3 | #include 4 | #include 5 | 6 | #define attachInterrupt(num, func, mode) enableInterrupt(num) 7 | #if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) 8 | #define SCRAMBLE_INT_ORDER(num) ((num < 4) ? num + 2 : ((num < 6) ? num - 4 : num)) 9 | #define DESCRAMBLE_INT_ORDER(num) ((num < 2) ? num + 4 : ((num < 6) ? num - 2 : num)) 10 | #else 11 | #define SCRAMBLE_INT_ORDER(num) (num) 12 | #define DESCRAMBLE_INT_ORDER(num) (num) 13 | #endif 14 | 15 | static void enableInterrupt(uint8_t num) 16 | { 17 | switch (DESCRAMBLE_INT_ORDER(num)) { 18 | #if defined(EICRA) && defined(EIMSK) 19 | case 0: 20 | EICRA = (EICRA & 0xFC) | 0x01; 21 | EIMSK |= 0x01; 22 | return; 23 | case 1: 24 | EICRA = (EICRA & 0xF3) | 0x04; 25 | EIMSK |= 0x02; 26 | return; 27 | case 2: 28 | EICRA = (EICRA & 0xCF) | 0x10; 29 | EIMSK |= 0x04; 30 | return; 31 | case 3: 32 | EICRA = (EICRA & 0x3F) | 0x40; 33 | EIMSK |= 0x08; 34 | return; 35 | #elif defined(MCUCR) && defined(GICR) 36 | case 0: 37 | MCUCR = (MCUCR & ~((1 << ISC00) | (1 << ISC01))) | (mode << ISC00); 38 | GICR |= (1 << INT0); 39 | return; 40 | case 1: 41 | MCUCR = (MCUCR & ~((1 << ISC10) | (1 << ISC11))) | (mode << ISC10); 42 | GICR |= (1 << INT1); 43 | return; 44 | #elif defined(MCUCR) && defined(GIMSK) 45 | case 0: 46 | MCUCR = (MCUCR & ~((1 << ISC00) | (1 << ISC01))) | (mode << ISC00); 47 | GIMSK |= (1 << INT0); 48 | return; 49 | case 1: 50 | MCUCR = (MCUCR & ~((1 << ISC10) | (1 << ISC11))) | (mode << ISC10); 51 | GIMSK |= (1 << INT1); 52 | return; 53 | #endif 54 | #if defined(EICRB) && defined(EIMSK) 55 | case 4: 56 | EICRB = (EICRB & 0xFC) | 0x01; 57 | EIMSK |= 0x10; 58 | return; 59 | case 5: 60 | EICRB = (EICRB & 0xF3) | 0x04; 61 | EIMSK |= 0x20; 62 | return; 63 | case 6: 64 | EICRB = (EICRB & 0xCF) | 0x10; 65 | EIMSK |= 0x40; 66 | return; 67 | case 7: 68 | EICRB = (EICRB & 0x3F) | 0x40; 69 | EIMSK |= 0x80; 70 | return; 71 | #endif 72 | } 73 | } 74 | 75 | #elif defined(__PIC32MX__) 76 | 77 | #ifdef ENCODER_OPTIMIZE_INTERRUPTS 78 | #undef ENCODER_OPTIMIZE_INTERRUPTS 79 | #endif 80 | 81 | #else 82 | 83 | #ifdef ENCODER_OPTIMIZE_INTERRUPTS 84 | #undef ENCODER_OPTIMIZE_INTERRUPTS 85 | #endif 86 | 87 | #endif 88 | -------------------------------------------------------------------------------- /Libraries/Dagu4Motor/util/interrupt_pins.h: -------------------------------------------------------------------------------- 1 | // interrupt pins for known boards 2 | 3 | // Teensy (and maybe others) define these automatically 4 | #if !defined(CORE_NUM_INTERRUPT) 5 | 6 | // Wiring boards 7 | #if defined(WIRING) 8 | #define CORE_NUM_INTERRUPT NUM_EXTERNAL_INTERRUPTS 9 | #if NUM_EXTERNAL_INTERRUPTS > 0 10 | #define CORE_INT0_PIN EI0 11 | #endif 12 | #if NUM_EXTERNAL_INTERRUPTS > 1 13 | #define CORE_INT1_PIN EI1 14 | #endif 15 | #if NUM_EXTERNAL_INTERRUPTS > 2 16 | #define CORE_INT2_PIN EI2 17 | #endif 18 | #if NUM_EXTERNAL_INTERRUPTS > 3 19 | #define CORE_INT3_PIN EI3 20 | #endif 21 | #if NUM_EXTERNAL_INTERRUPTS > 4 22 | #define CORE_INT4_PIN EI4 23 | #endif 24 | #if NUM_EXTERNAL_INTERRUPTS > 5 25 | #define CORE_INT5_PIN EI5 26 | #endif 27 | #if NUM_EXTERNAL_INTERRUPTS > 6 28 | #define CORE_INT6_PIN EI6 29 | #endif 30 | #if NUM_EXTERNAL_INTERRUPTS > 7 31 | #define CORE_INT7_PIN EI7 32 | #endif 33 | 34 | // Arduino Uno, Duemilanove, Diecimila, LilyPad, Mini, Fio, etc... 35 | #elif defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega8__) 36 | #define CORE_NUM_INTERRUPT 2 37 | #define CORE_INT0_PIN 2 38 | #define CORE_INT1_PIN 3 39 | 40 | // Arduino Mega 41 | #elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) 42 | #define CORE_NUM_INTERRUPT 6 43 | #define CORE_INT0_PIN 2 44 | #define CORE_INT1_PIN 3 45 | #define CORE_INT2_PIN 21 46 | #define CORE_INT3_PIN 20 47 | #define CORE_INT4_PIN 19 48 | #define CORE_INT5_PIN 18 49 | 50 | // Sanguino 51 | #elif defined(__AVR_ATmega644P__) || defined(__AVR_ATmega644__) 52 | #define CORE_NUM_INTERRUPT 3 53 | #define CORE_INT0_PIN 10 54 | #define CORE_INT1_PIN 11 55 | #define CORE_INT2_PIN 2 56 | 57 | // Chipkit Uno32 - attachInterrupt may not support CHANGE option 58 | #elif defined(__PIC32MX__) && defined(_BOARD_UNO_) 59 | #define CORE_NUM_INTERRUPT 5 60 | #define CORE_INT0_PIN 38 61 | #define CORE_INT1_PIN 2 62 | #define CORE_INT2_PIN 7 63 | #define CORE_INT3_PIN 8 64 | #define CORE_INT4_PIN 35 65 | 66 | // Chipkit Uno32 - attachInterrupt may not support CHANGE option 67 | #elif defined(__PIC32MX__) && defined(_BOARD_MEGA_) 68 | #define CORE_NUM_INTERRUPT 5 69 | #define CORE_INT0_PIN 3 70 | #define CORE_INT1_PIN 2 71 | #define CORE_INT2_PIN 7 72 | #define CORE_INT3_PIN 21 73 | #define CORE_INT4_PIN 20 74 | 75 | // http://hlt.media.mit.edu/?p=1229 76 | #elif defined(__AVR_ATtiny45__) || defined(__AVR_ATtiny85__) 77 | #define CORE_NUM_INTERRUPT 1 78 | #define CORE_INT0_PIN 2 79 | #endif 80 | #endif 81 | 82 | #if !defined(CORE_NUM_INTERRUPT) 83 | #error "Interrupts are unknown for this board, please add to this code" 84 | #endif 85 | #if CORE_NUM_INTERRUPT <= 0 86 | #error "Encoder requires interrupt pins, but this board does not have any :(" 87 | #error "You could try defining ENCODER_DO_NOT_USE_INTERRUPTS as a kludge." 88 | #endif 89 | 90 | -------------------------------------------------------------------------------- /Libraries/README.md: -------------------------------------------------------------------------------- 1 | This library is for Arduino to run the Dagu 4 Channel motor controller designed for the Dagu Rover 5 platform 2 | 3 | You can find the Rover 5 and controller at SparkFun. 4 | 5 | [Motor Driver Board](https://www.sparkfun.com/products/11593) 6 | [Rover 5](https://www.sparkfun.com/products/10336) 7 | [Arduino Uno](https://www.sparkfun.com/products/11224) 8 | 9 | This library needs work but in its basic form it does work and a good starting point. 10 | 11 | It also supports the encoders using the library from Teensy. 12 | 13 | [Teensy Encoder Library](http://www.pjrc.com/teensy/td_libs_Encoder.html) 14 | 15 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | Rover5 Motor Driver Board 2 | ================ 3 | 4 | [![Rover 5 Motor Driver Board](https://dlnmh9ip6v2uc.cloudfront.net/images/products/1/1/5/9/3/11593-02_medium.jpg) 5 | *Rover 5 Motor Driver Board(ROB-11593)*](https://www.sparkfun.com/products/11593) 6 | 7 | The Rover 5 Motor Driver Board was originally designed by Dagu for the Rover 5 platform, but is ideal for any small 4-wheel driven 8 | robotic vehicle. It includes 4 motor outputs, four encoder inputs, and current sensing for each motor, and works well with omni or 9 | mechanum wheel implementations. 10 | 11 | 12 | Repository Contents 13 | ------------------- 14 | * **/Libraries** - Code library for Arduino to run the board. 15 | 16 | 17 | 18 | License Information 19 | -------------------- 20 | All source code and designs are released under Creative Commons-By-ShareAlike CC BY-SA 21 | [CC BY-SA](http://i.creativecommons.org/l/by-sa/3.0/88x31.png) 22 | 23 | Collaboration 24 | -------------- 25 | This library was originally written by William from FriedCircuits.us. 26 | You can find his original repository [here](https://github.com/FriedCircuits/Dagu4Motor-Library). --------------------------------------------------------------------------------