├── keywords.txt ├── library.json ├── .gitignore ├── README.md ├── LICENSE ├── NXTServo.h └── NXTServo.cpp /keywords.txt: -------------------------------------------------------------------------------- 1 | NXTServo KEYWORD1 2 | update KEYWORD2 3 | setSampleTime KEYWORD2 4 | tuneSpeedPID KEYWORD2 5 | tuneAnglePID KEYWORD2 6 | turnAt KEYWORD2 7 | brake KEYWORD2 8 | goTo KEYWORD2 9 | hold KEYWORD2 10 | reset KEYWORD2 11 | position KEYWORD2 12 | -------------------------------------------------------------------------------- /library.json: -------------------------------------------------------------------------------- 1 | { 2 | "name": "NXTMotor", 3 | "keywords": "NXTMotor, motor, pid, position, control, speed", 4 | "description": "NXTMotor control library", 5 | "repository": 6 | { 7 | "type": "git", 8 | "url": "https://github.com/sebgiles/nxtmotor" 9 | } 10 | } 11 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Compiled Object files 2 | *.slo 3 | *.lo 4 | *.o 5 | *.obj 6 | 7 | # Precompiled Headers 8 | *.gch 9 | *.pch 10 | 11 | # Compiled Dynamic libraries 12 | *.so 13 | *.dylib 14 | *.dll 15 | 16 | # Fortran module files 17 | *.mod 18 | 19 | # Compiled Static libraries 20 | *.lai 21 | *.la 22 | *.a 23 | *.lib 24 | 25 | # Executables 26 | *.exe 27 | *.out 28 | *.app 29 | 30 | # OS generated files # 31 | ###################### 32 | .DS_Store 33 | .DS_Store? 34 | ._* 35 | .Spotlight-V100 36 | .Trashes 37 | ehthumbs.db 38 | Thumbs.db 39 | 40 | *.un~ 41 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # NXTServo 2 | Arduino library to handle Lego Mindstorms NXT servo motors. 3 | Features precise position control and speed control. 4 | 5 | Relies on the PJRC Encoder library, install separetely: https://www.pjrc.com/teensy/td_libs_Encoder.html 6 | 7 | Each motor requires 4 pins on Arduino: 8 | 2 digital inputs for the encoder signal 9 | (at least one should be an interrupt pin, more info: https://www.pjrc.com/teensy/td_libs_Encoder.html#polling) 10 | 2 pwm outputs 11 | 12 | H-bridge (L293 etc.) or motor shield is required for powering the motor with 9V. Used channels must be enabled, pwm is sent directly to the control pins. No extra wiring or circuits are necessary. 13 | 14 | Please refer to this table: http://www.personal.psu.edu/jpm5375/MotorPinout.png 15 | Pins 1 and 2 are connected to the H-bridge/shield output 16 | Pins 5 and 6 carry the encoder signal to the arduino 17 | Pin 3 is GND 18 | Pin 4 should be +4.5V to power the optical encoder, I use 5V without any problem, do at your own risk 19 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | The MIT License (MIT) 2 | 3 | Copyright (c) 2015 Sebastian Giles 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | 23 | -------------------------------------------------------------------------------- /NXTServo.h: -------------------------------------------------------------------------------- 1 | // 2 | // NXTServo.h 3 | // 4 | // 5 | // Created by Sebastian Giles on 12/12/14. 6 | // 7 | // 8 | 9 | #ifndef ____NXTServo__ 10 | #define ____NXTServo__ 11 | 12 | #include "Arduino.h" 13 | #include "Motor.h" 14 | #include "PID.h" 15 | #include "Encoder.h" 16 | 17 | #define DEFAULT_SAMPLE_TIME 20 18 | #define DEFAULT_VOLTAGE 9 19 | 20 | #define SKP 0 21 | #define SKI 0 22 | #define SKD 0 23 | 24 | #define AKP 0 25 | #define AKI 0 26 | #define AKD 0 27 | 28 | class NXTServo : public Motor, public PID, private Encoder{ 29 | public: 30 | 31 | NXTServo(const int &white, const int &black, const int &yellow, const int &blue); 32 | 33 | //returns true if something changes meaning sampleTime has passed 34 | //should be run very frequently 35 | bool update(); 36 | 37 | //takes integer value in millis 38 | void setSampleTime(const int &newSampleTime); 39 | 40 | void tuneSpeedPID(const float &kp, const float &ki, const float &kd); 41 | 42 | void tuneAnglePID(const float &kp, const float &ki, const float &kd); 43 | 44 | //Gets motor cruising at specified speed 45 | void turnAt(const float &speed); //in rpm 46 | 47 | //stops the motor as fast as possible 48 | void brake(); 49 | 50 | //Turns motor to given position 51 | void goTo(const float &angle); //in degrees 52 | 53 | //holds current position 54 | void hold(); 55 | 56 | //return current position in degrees 57 | float position(); 58 | 59 | //uses current position as new 0 reference point 60 | void reset(); 61 | 62 | //override is necessasry to disable pid 63 | void turn(const float &power); 64 | 65 | //returns struct from PID object 66 | PIDData* getData(); 67 | 68 | private: 69 | 70 | PID speedController; 71 | PID angleController; 72 | 73 | PIDData data; 74 | 75 | enum mode {OFF, SPEED, ANGLE}; 76 | 77 | mode currentState; 78 | 79 | unsigned int sampleTime; 80 | 81 | unsigned long lastTime; //in millis, used to check if updating is needed 82 | float lastAngle; //both are used to compute speed 83 | 84 | }; 85 | 86 | 87 | #endif /* defined(____NXTServo__) */ 88 | -------------------------------------------------------------------------------- /NXTServo.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // NXTServo.cpp 3 | // 4 | // 5 | // Created by Sebastian Giles on 12/12/14. 6 | // 7 | // 8 | 9 | #include "NXTServo.h" 10 | 11 | NXTServo::NXTServo( const int &fwdPin, const int &revPin, 12 | const int &interruptPin, const int &quadraturePin): 13 | Motor(fwdPin, revPin, DEFAULT_VOLTAGE), 14 | Encoder(interruptPin, quadraturePin) 15 | { 16 | currentState = OFF; 17 | sampleTime = DEFAULT_SAMPLE_TIME; 18 | //Initial angle is established at this moment 19 | //so we can calculate the speed even on the first call of update() 20 | angleController.tune(AKP,AKI,AKD); 21 | speedController.tune(SKP,SKI,SKD); 22 | speedController.tolerate(1); 23 | angleController.tolerate(1); 24 | lastAngle=position(); 25 | lastTime=millis(); 26 | data.target=0; 27 | } 28 | 29 | boolean NXTServo::update(){ 30 | 31 | if(millis()-lastTime<=sampleTime){ 32 | return false; 33 | } 34 | 35 | long ms=millis(); 36 | 37 | float angle = position(); 38 | 39 | data.time = ms/1000.0; 40 | 41 | if(currentState==SPEED) { 42 | data.input = ((angle-lastAngle)/360.0) / 43 | ((data.time-lastTime/1000.0)/60.0); 44 | 45 | Motor::turn(speedController.compute(data)); 46 | 47 | } else if(currentState==ANGLE) { 48 | data.input=angle; 49 | Motor::turn(angleController.compute(data)); 50 | } 51 | 52 | lastTime = ms; 53 | lastAngle = angle; 54 | 55 | return true; 56 | } 57 | 58 | float NXTServo::position(){ 59 | long pos=read(); 60 | return (float)pos/2.0; 61 | } 62 | 63 | void NXTServo::setSampleTime(const int &time){ 64 | sampleTime=time; 65 | } 66 | 67 | void NXTServo::tuneSpeedPID(const float &kp, const float &ki, const float &kd){ 68 | speedController.tune(kp,ki,kd); 69 | } 70 | 71 | void NXTServo::tuneAnglePID(const float &kp, const float &ki, const float &kd){ 72 | angleController.tune(kp,ki,kd); 73 | } 74 | 75 | 76 | void NXTServo::turnAt(const float &speed){ 77 | data.target=speed; 78 | currentState=SPEED; 79 | } 80 | 81 | void NXTServo::brake(){ 82 | turnAt(0); 83 | } 84 | 85 | void NXTServo::goTo(const float &angle){ 86 | data.target=angle; 87 | currentState=ANGLE; 88 | } 89 | 90 | void NXTServo::hold(){ 91 | goTo(position()); 92 | } 93 | 94 | void NXTServo::reset(){ 95 | currentState=OFF; 96 | write(0); 97 | lastAngle=0; 98 | lastTime=millis(); 99 | update(); 100 | } 101 | 102 | void NXTServo::turn(const float &power){ 103 | currentState=OFF; 104 | Motor::turn(power); 105 | } 106 | 107 | PIDData* NXTServo::getData(){ 108 | return &data; 109 | } 110 | --------------------------------------------------------------------------------