├── pictures └── model.jpg ├── Electronics └── RW2_circuit.png ├── CAD_ReactionWheel ├── STL_files │ ├── RW_Base.STL │ ├── RW_Flywheel.STL │ ├── RW_Satellite-Disk.STL │ ├── RW_Satellite-MotorHolder_26mm.STL │ └── RW_Satellite-MotorHolder_40mm.STL └── SolidWorks_src │ ├── RW_Base.SLDPRT │ ├── RW_Assembly.STEP │ ├── RW_Flywheel.SLDPRT │ ├── RW_Satellite.SLDPRT │ └── Assembly │ ├── RW_Assembly.SLDASM │ ├── 8mm_threaded_rod.SLDPRT │ ├── 8mm Bearing SKF 608 (RS 286-7530).SLDPRT │ └── E3D Slimline Stepper Motor 0.9deg V1.1.SLDPRT ├── LICENSE ├── README.md └── Arduino_ReactionWheel ├── PID.h ├── Arduino_ReactionWheel.ino ├── AccelStepper.cpp └── AccelStepper.h /pictures/model.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CGrassin/reaction_wheel/HEAD/pictures/model.jpg -------------------------------------------------------------------------------- /Electronics/RW2_circuit.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CGrassin/reaction_wheel/HEAD/Electronics/RW2_circuit.png -------------------------------------------------------------------------------- /CAD_ReactionWheel/STL_files/RW_Base.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CGrassin/reaction_wheel/HEAD/CAD_ReactionWheel/STL_files/RW_Base.STL -------------------------------------------------------------------------------- /CAD_ReactionWheel/STL_files/RW_Flywheel.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CGrassin/reaction_wheel/HEAD/CAD_ReactionWheel/STL_files/RW_Flywheel.STL -------------------------------------------------------------------------------- /CAD_ReactionWheel/SolidWorks_src/RW_Base.SLDPRT: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CGrassin/reaction_wheel/HEAD/CAD_ReactionWheel/SolidWorks_src/RW_Base.SLDPRT -------------------------------------------------------------------------------- /CAD_ReactionWheel/STL_files/RW_Satellite-Disk.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CGrassin/reaction_wheel/HEAD/CAD_ReactionWheel/STL_files/RW_Satellite-Disk.STL -------------------------------------------------------------------------------- /CAD_ReactionWheel/SolidWorks_src/RW_Assembly.STEP: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CGrassin/reaction_wheel/HEAD/CAD_ReactionWheel/SolidWorks_src/RW_Assembly.STEP -------------------------------------------------------------------------------- /CAD_ReactionWheel/SolidWorks_src/RW_Flywheel.SLDPRT: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CGrassin/reaction_wheel/HEAD/CAD_ReactionWheel/SolidWorks_src/RW_Flywheel.SLDPRT -------------------------------------------------------------------------------- /CAD_ReactionWheel/SolidWorks_src/RW_Satellite.SLDPRT: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CGrassin/reaction_wheel/HEAD/CAD_ReactionWheel/SolidWorks_src/RW_Satellite.SLDPRT -------------------------------------------------------------------------------- /CAD_ReactionWheel/STL_files/RW_Satellite-MotorHolder_26mm.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CGrassin/reaction_wheel/HEAD/CAD_ReactionWheel/STL_files/RW_Satellite-MotorHolder_26mm.STL -------------------------------------------------------------------------------- /CAD_ReactionWheel/STL_files/RW_Satellite-MotorHolder_40mm.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CGrassin/reaction_wheel/HEAD/CAD_ReactionWheel/STL_files/RW_Satellite-MotorHolder_40mm.STL -------------------------------------------------------------------------------- /CAD_ReactionWheel/SolidWorks_src/Assembly/RW_Assembly.SLDASM: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CGrassin/reaction_wheel/HEAD/CAD_ReactionWheel/SolidWorks_src/Assembly/RW_Assembly.SLDASM -------------------------------------------------------------------------------- /CAD_ReactionWheel/SolidWorks_src/Assembly/8mm_threaded_rod.SLDPRT: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CGrassin/reaction_wheel/HEAD/CAD_ReactionWheel/SolidWorks_src/Assembly/8mm_threaded_rod.SLDPRT -------------------------------------------------------------------------------- /CAD_ReactionWheel/SolidWorks_src/Assembly/8mm Bearing SKF 608 (RS 286-7530).SLDPRT: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CGrassin/reaction_wheel/HEAD/CAD_ReactionWheel/SolidWorks_src/Assembly/8mm Bearing SKF 608 (RS 286-7530).SLDPRT -------------------------------------------------------------------------------- /CAD_ReactionWheel/SolidWorks_src/Assembly/E3D Slimline Stepper Motor 0.9deg V1.1.SLDPRT: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CGrassin/reaction_wheel/HEAD/CAD_ReactionWheel/SolidWorks_src/Assembly/E3D Slimline Stepper Motor 0.9deg V1.1.SLDPRT -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2021 Charles Grassin 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 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Reaction Wheel model 2 | 3 | This repository contains the files (code and STL) to build a reaction wheel controlled satellite model. 4 | 5 | ![The satellite model](pictures/model.jpg) 6 | 7 | This project is described in this article (including test results): https://charleslabs.fr/en/project-Reaction+Wheel+Attitude+Control 8 | 9 | ## Material 10 | 11 | There are 4 parts to 3D-print: base, satellite disk, motor holder and flywheel. The only part requiring support is the motor holder. 12 | 13 | Additionnal hardware is required: 14 | 15 | | Quantity | Item | 16 | |----------------|-----------------------| 17 | | 1 | NEMA 17 (23 mm or 40 mm length) | 18 | | 2 | 608 bearing | 19 | | 1 | M8 threaded rod (65 mm) | 20 | | 4 | M8 nut | 21 | | 8 | M3×6 mm screw | 22 | | 4 | M3 nut | 23 | | 1 | Arduino Nano board | 24 | | 1 | DRV8825 driver module | 25 | | 1 | Level shifter module | 26 | | 1 | MPU6050 sensor module | 27 | | 1 | 100 µF 16 V capacitor | 28 | | 1 | 2S LiPo Battery (300 mAh) | 29 | 30 | Additionnal M8 hardware is required to tune the moment of inertia of the wheel. In my case, I used 3 M8×20 mm bolts and 3 M8 nuts. 31 | 32 | See the article for more detailed build instructions. 33 | 34 | ## License 35 | 36 | This model is released under the terms of the MIT License. -------------------------------------------------------------------------------- /Arduino_ReactionWheel/PID.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is an ultra light/simple PID controller 3 | * library for Arduino. 4 | * 5 | * Charles GRASSIN, 2019 6 | * MIT License 7 | */ 8 | 9 | #ifndef PID_CONTROLLER_H 10 | #define PID_CONTROLLER_H 11 | /* A standard PID controller class */ 12 | class PIDController { 13 | private: 14 | double kp, ki, kd, lastErr, cumulErr; 15 | 16 | protected: 17 | virtual double calcError(double setPoint, double current){ 18 | return setPoint - current; 19 | } 20 | 21 | public: 22 | PIDController(double kp, double ki, double kd) : kp(kp), ki(ki), kd(kd), lastErr(0), cumulErr(0){} 23 | 24 | double compute(double setPoint,double current, long detaT){ 25 | /*Compute all the working error variables*/ 26 | double error = calcError(setPoint, current); 27 | double dErr = (detaT==0)?0:calcError(error, lastErr) / detaT; 28 | 29 | /*Remember some variables for next time*/ 30 | lastErr = error; 31 | cumulErr += error * detaT; 32 | 33 | /*Compute PID Output*/ 34 | return kp * error + ki * cumulErr + kd * dErr; 35 | } 36 | 37 | void reset(){ 38 | lastErr=0;cumulErr=0; 39 | } 40 | }; 41 | 42 | /* A PID controller to control angles*/ 43 | class PIDAngleController: public PIDController{ 44 | 45 | public: 46 | PIDAngleController(double kp, double ki, double kd) : PIDController(kp, ki, kd) {} 47 | 48 | protected: 49 | virtual double calcError(double setPoint, double current){ 50 | double distance = fmod(setPoint - current , 360.0); 51 | if (distance < -180) 52 | return distance + 360; 53 | else if (distance > 179) 54 | return distance - 360; 55 | return distance; 56 | } 57 | }; 58 | #endif 59 | -------------------------------------------------------------------------------- /Arduino_ReactionWheel/Arduino_ReactionWheel.ino: -------------------------------------------------------------------------------- 1 | /* 2 | * This is the controller code for the reaction wheel satellite model published 3 | * at https://charleslabs.fr/en/project-Reaction+Wheel+Attitude+Control 4 | * 5 | * The electronics diagram and the mechanical build is described in this article. 6 | * 7 | * You can set the PID controller terms and various other parameters hereunder. 8 | * 9 | * Charles GRASSIN, 2021 10 | * MIT License 11 | */ 12 | 13 | #define SERIAL_DEBUG_ENABLE 1 /* 0 = disable, 1 = enable */ 14 | #define MPU6050_CALIBRATION 0 /* 0 = disable, 1 = enable */ 15 | #define CONTROLLER_MODE 0 /* 0 = Speed stabilization only, 1 = Speed and Attitude stabilization, 2 = same as 1, but change set point every N secondes */ 16 | 17 | // -------PINS------- 18 | #define PIN_DIR 2 19 | #define PIN_STEP 3 20 | #define PIN_SLEEP 4 21 | #define PIN_LED 13 22 | // ------------------ 23 | 24 | // -----STEPPER------ 25 | #include "AccelStepper.h" // This is a hacked version of this library to allow 26 | // speed control instead of position control. 27 | AccelStepper myStepper(1, PIN_STEP, PIN_DIR); 28 | double motorSpeed = 0; 29 | #define MICROSTEPPING 4 /* 1 or 2 or 4 or 8 or 16 or 32 */ 30 | #define ACCELERATION 1750 /* Steps per s */ 31 | #define MAX_SPEED (600 * MICROSTEPPING) 32 | // ------------------ 33 | 34 | // -------PID-------- 35 | #include "PID.h" 36 | const double P_TERM = 0.050 * MICROSTEPPING; 37 | const double I_TERM = 0.000 * MICROSTEPPING; 38 | const double D_TERM = 0.017 * MICROSTEPPING; 39 | PIDController pidSpeed(P_TERM, I_TERM, D_TERM); 40 | PIDAngleController pidAttitude(2.5, 0, 400); 41 | // ------------------ 42 | 43 | // ------MPU6050----- 44 | #include 45 | #define MPU_ADDRESS 0x68 // I2C address of the MPU-6050 46 | double GYRO_ERROR = 61.96; // rollingAvg error compensation 47 | double yawAngle=0, yawAngularSpeed=0; 48 | // ------------------ 49 | 50 | // ------GENERAL----- 51 | long timeCur, timePrev, timeStart; 52 | const int numReadings= 5; 53 | double readings[numReadings]; 54 | int readIndex = 0; 55 | double total = 0, rollingAvg = 0; 56 | double targetAttitude = 0; 57 | // ------------------ 58 | 59 | void setup() { 60 | #if SERIAL_DEBUG_ENABLE == 1 61 | Serial.begin(115200); 62 | delay(500); 63 | Serial.println("Attitude Speed"); 64 | #endif 65 | 66 | // Gyro setup 67 | Wire.begin(); 68 | Wire.beginTransmission(MPU_ADDRESS); 69 | Wire.write(0x6B); // PWR_MGMT_1 register 70 | Wire.write(0); // wakes up the MPU-6050 71 | Wire.endTransmission(true); 72 | Wire.beginTransmission(MPU_ADDRESS); 73 | Wire.write(0x1B); // GYRO_CONFIG register 74 | Wire.write(0x10); // 1000dps full scale 75 | Wire.endTransmission(true); 76 | 77 | // Gyro cal (only if selected, otherwise use saved value) 78 | #if MPU6050_CALIBRATION == 1 79 | calibrateMPU(); 80 | #endif 81 | 82 | // LED 83 | pinMode(PIN_LED,OUTPUT); 84 | digitalWrite(PIN_LED,1); 85 | 86 | // Initial stepper parameters 87 | myStepper.setEnablePin (PIN_SLEEP); 88 | myStepper.setAcceleration(ACCELERATION); 89 | setSpeedStepper(0); 90 | 91 | timeCur = millis(); 92 | timeStart = timeCur; 93 | } 94 | 95 | // FSM variables 96 | byte controllerState = 0; 97 | int counts = 0; 98 | 99 | // Main loop 100 | void loop() { 101 | // Stop control after 40s 102 | // if(millis() - timeStart > 40000){ 103 | // digitalWrite(PIN_LED,0); 104 | // myStepper.disableOutputs (); 105 | // } 106 | 107 | // Pulse stepper 108 | myStepper.run(); 109 | 110 | // Every 10ms, read MPU and call controllers 111 | if(millis() - timeCur > 10) { 112 | timePrev = timeCur; 113 | timeCur = millis(); 114 | 115 | // Measure Gyro value 116 | yawAngularSpeed = ((double)readMPU()-GYRO_ERROR) / 32.8; 117 | yawAngle += (yawAngularSpeed * (timeCur - timePrev) / 1000.0); 118 | // Put angle between -180 and 180 119 | while (yawAngle <= -180) yawAngle += 360; 120 | while (yawAngle > 180) yawAngle -= 360; 121 | 122 | // Low Pass Filter the angular speed (https://www.arduino.cc/en/Tutorial/BuiltInExamples/Smoothing) 123 | total = total - readings[readIndex]; 124 | readings[readIndex] = yawAngularSpeed; 125 | total = total + readings[readIndex]; 126 | readIndex = readIndex + 1; 127 | if (readIndex >= numReadings) 128 | readIndex = 0; 129 | rollingAvg = total / numReadings; 130 | 131 | // Compute controller output 132 | #if CONTROLLER_MODE == 0 133 | // Detumbling only 134 | motorSpeed += pidSpeed.compute(0,rollingAvg,timeCur - timePrev); 135 | #else if CONTROLLER_MODE == 1 || CONTROLLER_MODE == 2 136 | // Change set point 137 | #if CONTROLLER_MODE == 2 138 | counts ++; 139 | if(counts == 250) { 140 | counts = 0; 141 | if(targetAttitude == 0) 142 | targetAttitude = 180; 143 | else 144 | targetAttitude = 0; 145 | } 146 | #endif 147 | 148 | // FSM transition 149 | if(controllerState == 1 && fabs(rollingAvg) > 360 /* °/s */){ 150 | controllerState = 0;digitalWrite(PIN_LED,0); 151 | } 152 | else if(controllerState == 0 && fabs(rollingAvg) < 45 /* °/s */) 153 | controllerState = 1; 154 | 155 | //FSM action 156 | if(controllerState == 0){ 157 | motorSpeed += pidSpeed.compute(0,rollingAvg,timeCur - timePrev); 158 | pidAttitude.compute(targetAttitude,yawAngle,timeCur - timePrev); 159 | } 160 | else 161 | motorSpeed += pidSpeed.compute(pidAttitude.compute(targetAttitude,yawAngle,timeCur - timePrev),rollingAvg,timeCur - timePrev); 162 | #endif 163 | 164 | // Constrain speed to valid interval (saturation) 165 | if(motorSpeed > MAX_SPEED) motorSpeed = MAX_SPEED; 166 | else if (motorSpeed < -MAX_SPEED) motorSpeed = -MAX_SPEED; 167 | 168 | setSpeedStepper(motorSpeed); 169 | 170 | // Report attitude and speed 171 | #if SERIAL_DEBUG_ENABLE == 1 172 | Serial.print(yawAngle); 173 | Serial.print(" "); 174 | Serial.println(rollingAvg); 175 | #endif 176 | } 177 | } 178 | 179 | // Set the current speed and direction of the motor 180 | void setSpeedStepper(double targetSpeed){ 181 | if(targetSpeed > 0) 182 | myStepper.moveTo(1000000); 183 | else 184 | myStepper.moveTo(-1000000); 185 | 186 | myStepper.setMaxSpeed(fabs(targetSpeed)); 187 | } 188 | 189 | // Read a yaw angular speed value 190 | int16_t readMPU(){ 191 | Wire.beginTransmission(MPU_ADDRESS); 192 | Wire.write(0x47); 193 | Wire.endTransmission(false); 194 | Wire.requestFrom(MPU_ADDRESS,2,true); 195 | return Wire.read()<<8|Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L) 196 | } 197 | 198 | // Calibrate the gyro by doing CALIBRATION_MEASUREMENTS_COUNT measurements 199 | #define CALIBRATION_MEASUREMENTS_COUNT 200 200 | void calibrateMPU(){ 201 | GYRO_ERROR = 0; 202 | for(int i=0;i= _stepInterval) 49 | { 50 | if (_direction == DIRECTION_CW) 51 | { 52 | // Clockwise 53 | //_currentPos += 1; 54 | } 55 | else 56 | { 57 | // Anticlockwise 58 | //_currentPos -= 1; 59 | } 60 | step(_currentPos); 61 | 62 | _lastStepTime = time; // Caution: does not account for costs in step() 63 | 64 | return true; 65 | } 66 | else 67 | { 68 | return false; 69 | } 70 | } 71 | 72 | long AccelStepper::distanceToGo() 73 | { 74 | return _targetPos - _currentPos; 75 | //return _targetPos*1000; 76 | } 77 | 78 | long AccelStepper::targetPosition() 79 | { 80 | return _targetPos; 81 | } 82 | 83 | long AccelStepper::currentPosition() 84 | { 85 | return _currentPos; 86 | } 87 | 88 | // Useful during initialisations or after initial positioning 89 | // Sets speed to 0 90 | void AccelStepper::setCurrentPosition(long position) 91 | { 92 | _targetPos = _currentPos = position; 93 | _n = 0; 94 | _stepInterval = 0; 95 | _speed = 0.0; 96 | } 97 | 98 | void AccelStepper::computeNewSpeed() 99 | { 100 | long distanceTo = distanceToGo(); // +ve is clockwise from curent location 101 | 102 | long stepsToStop = (long)((_speed * _speed) / (2.0 * _acceleration)); // Equation 16 103 | 104 | if (distanceTo == 0 && stepsToStop <= 1) 105 | { 106 | // We are at the target and its time to stop 107 | _stepInterval = 0; 108 | _speed = 0.0; 109 | _n = 0; 110 | return; 111 | } 112 | 113 | if (distanceTo > 0) 114 | { 115 | // We are anticlockwise from the target 116 | // Need to go clockwise from here, maybe decelerate now 117 | if (_n > 0) 118 | { 119 | // Currently accelerating, need to decel now? Or maybe going the wrong way? 120 | if ((stepsToStop >= distanceTo) || _direction == DIRECTION_CCW) 121 | _n = -stepsToStop; // Start deceleration 122 | } 123 | else if (_n < 0) 124 | { 125 | // Currently decelerating, need to accel again? 126 | if ((stepsToStop < distanceTo) && _direction == DIRECTION_CW) 127 | _n = -_n; // Start accceleration 128 | } 129 | } 130 | else if (distanceTo < 0) 131 | { 132 | // We are clockwise from the target 133 | // Need to go anticlockwise from here, maybe decelerate 134 | if (_n > 0) 135 | { 136 | // Currently accelerating, need to decel now? Or maybe going the wrong way? 137 | if ((stepsToStop >= -distanceTo) || _direction == DIRECTION_CW) 138 | _n = -stepsToStop; // Start deceleration 139 | } 140 | else if (_n < 0) 141 | { 142 | // Currently decelerating, need to accel again? 143 | if ((stepsToStop < -distanceTo) && _direction == DIRECTION_CCW) 144 | _n = -_n; // Start accceleration 145 | } 146 | } 147 | 148 | // Need to accelerate or decelerate 149 | if (_n == 0) 150 | { 151 | // First step from stopped 152 | _cn = _c0; 153 | _direction = (distanceTo > 0) ? DIRECTION_CW : DIRECTION_CCW; 154 | } 155 | else 156 | { 157 | // Subsequent step. Works for accel (n is +_ve) and decel (n is -ve). 158 | _cn = _cn - ((2.0 * _cn) / ((4.0 * _n) + 1)); // Equation 13 159 | _cn = max(_cn, _cmin); 160 | } 161 | _n++; 162 | _stepInterval = _cn; 163 | _speed = 1000000.0 / _cn; 164 | if (_direction == DIRECTION_CCW) 165 | _speed = -_speed; 166 | 167 | #if 0 168 | Serial.println(_speed); 169 | Serial.println(_acceleration); 170 | Serial.println(_cn); 171 | Serial.println(_c0); 172 | Serial.println(_n); 173 | Serial.println(_stepInterval); 174 | Serial.println(distanceTo); 175 | Serial.println(stepsToStop); 176 | Serial.println("-----"); 177 | #endif 178 | } 179 | 180 | // Run the motor to implement speed and acceleration in order to proceed to the target position 181 | // You must call this at least once per step, preferably in your main loop 182 | // If the motor is in the desired position, the cost is very small 183 | // returns true if the motor is still running to the target position. 184 | boolean AccelStepper::run() 185 | { 186 | if (runSpeed()) 187 | computeNewSpeed(); 188 | return _speed != 0.0 || distanceToGo() != 0; 189 | } 190 | 191 | AccelStepper::AccelStepper(uint8_t interface, uint8_t pin1, uint8_t pin2, uint8_t pin3, uint8_t pin4, bool enable) 192 | { 193 | _interface = interface; 194 | _currentPos = 0; 195 | _targetPos = 0; 196 | _speed = 0.0; 197 | _maxSpeed = 1.0; 198 | _acceleration = 0.0; 199 | _sqrt_twoa = 1.0; 200 | _stepInterval = 0; 201 | _minPulseWidth = 1; 202 | _enablePin = 0xff; 203 | _lastStepTime = 0; 204 | _pin[0] = pin1; 205 | _pin[1] = pin2; 206 | _pin[2] = pin3; 207 | _pin[3] = pin4; 208 | _enableInverted = false; 209 | 210 | // NEW 211 | _n = 0; 212 | _c0 = 0.0; 213 | _cn = 0.0; 214 | _cmin = 1.0; 215 | _direction = DIRECTION_CCW; 216 | 217 | int i; 218 | for (i = 0; i < 4; i++) 219 | _pinInverted[i] = 0; 220 | if (enable) 221 | enableOutputs(); 222 | // Some reasonable default 223 | setAcceleration(1); 224 | } 225 | 226 | AccelStepper::AccelStepper(void (*forward)(), void (*backward)()) 227 | { 228 | _interface = 0; 229 | _currentPos = 0; 230 | _targetPos = 0; 231 | _speed = 0.0; 232 | _maxSpeed = 1.0; 233 | _acceleration = 0.0; 234 | _sqrt_twoa = 1.0; 235 | _stepInterval = 0; 236 | _minPulseWidth = 1; 237 | _enablePin = 0xff; 238 | _lastStepTime = 0; 239 | _pin[0] = 0; 240 | _pin[1] = 0; 241 | _pin[2] = 0; 242 | _pin[3] = 0; 243 | _forward = forward; 244 | _backward = backward; 245 | 246 | // NEW 247 | _n = 0; 248 | _c0 = 0.0; 249 | _cn = 0.0; 250 | _cmin = 1.0; 251 | _direction = DIRECTION_CCW; 252 | 253 | int i; 254 | for (i = 0; i < 4; i++) 255 | _pinInverted[i] = 0; 256 | // Some reasonable default 257 | setAcceleration(1); 258 | } 259 | 260 | void AccelStepper::setMaxSpeed(float speed) 261 | { 262 | if (speed < 0.0) 263 | speed = -speed; 264 | if (_maxSpeed != speed) 265 | { 266 | _maxSpeed = speed; 267 | _cmin = 1000000.0 / speed; 268 | // Recompute _n from current speed and adjust speed if accelerating or cruising 269 | if (_n > 0) 270 | { 271 | _n = (long)((_speed * _speed) / (2.0 * _acceleration)); // Equation 16 272 | computeNewSpeed(); 273 | } 274 | } 275 | } 276 | 277 | float AccelStepper::maxSpeed() 278 | { 279 | return _maxSpeed; 280 | } 281 | 282 | void AccelStepper::setAcceleration(float acceleration) 283 | { 284 | if (acceleration == 0.0) 285 | return; 286 | if (acceleration < 0.0) 287 | acceleration = -acceleration; 288 | if (_acceleration != acceleration) 289 | { 290 | // Recompute _n per Equation 17 291 | _n = _n * (_acceleration / acceleration); 292 | // New c0 per Equation 7, with correction per Equation 15 293 | _c0 = 0.676 * sqrt(2.0 / acceleration) * 1000000.0; // Equation 15 294 | _acceleration = acceleration; 295 | computeNewSpeed(); 296 | } 297 | } 298 | 299 | void AccelStepper::setSpeed(float speed) 300 | { 301 | if (speed == _speed) 302 | return; 303 | speed = constrain(speed, -_maxSpeed, _maxSpeed); 304 | if (speed == 0.0) 305 | _stepInterval = 0; 306 | else 307 | { 308 | _stepInterval = fabs(1000000.0 / speed); 309 | _direction = (speed > 0.0) ? DIRECTION_CW : DIRECTION_CCW; 310 | } 311 | _speed = speed; 312 | } 313 | 314 | float AccelStepper::speed() 315 | { 316 | return _speed; 317 | } 318 | 319 | // Subclasses can override 320 | void AccelStepper::step(long step) 321 | { 322 | switch (_interface) 323 | { 324 | case FUNCTION: 325 | step0(step); 326 | break; 327 | 328 | case DRIVER: 329 | step1(step); 330 | break; 331 | 332 | case FULL2WIRE: 333 | step2(step); 334 | break; 335 | 336 | case FULL3WIRE: 337 | step3(step); 338 | break; 339 | 340 | case FULL4WIRE: 341 | step4(step); 342 | break; 343 | 344 | case HALF3WIRE: 345 | step6(step); 346 | break; 347 | 348 | case HALF4WIRE: 349 | step8(step); 350 | break; 351 | } 352 | } 353 | 354 | // You might want to override this to implement eg serial output 355 | // bit 0 of the mask corresponds to _pin[0] 356 | // bit 1 of the mask corresponds to _pin[1] 357 | // .... 358 | void AccelStepper::setOutputPins(uint8_t mask) 359 | { 360 | uint8_t numpins = 2; 361 | if (_interface == FULL4WIRE || _interface == HALF4WIRE) 362 | numpins = 4; 363 | else if (_interface == FULL3WIRE || _interface == HALF3WIRE) 364 | numpins = 3; 365 | uint8_t i; 366 | for (i = 0; i < numpins; i++) 367 | digitalWrite(_pin[i], (mask & (1 << i)) ? (HIGH ^ _pinInverted[i]) : (LOW ^ _pinInverted[i])); 368 | } 369 | 370 | // 0 pin step function (ie for functional usage) 371 | void AccelStepper::step0(long step) 372 | { 373 | (void)(step); // Unused 374 | if (_speed > 0) 375 | _forward(); 376 | else 377 | _backward(); 378 | } 379 | 380 | // 1 pin step function (ie for stepper drivers) 381 | // This is passed the current step number (0 to 7) 382 | // Subclasses can override 383 | void AccelStepper::step1(long step) 384 | { 385 | (void)(step); // Unused 386 | 387 | // _pin[0] is step, _pin[1] is direction 388 | setOutputPins(_direction ? 0b10 : 0b00); // Set direction first else get rogue pulses 389 | setOutputPins(_direction ? 0b11 : 0b01); // step HIGH 390 | // Caution 200ns setup time 391 | // Delay the minimum allowed pulse width 392 | delayMicroseconds(_minPulseWidth); 393 | setOutputPins(_direction ? 0b10 : 0b00); // step LOW 394 | } 395 | 396 | 397 | // 2 pin step function 398 | // This is passed the current step number (0 to 7) 399 | // Subclasses can override 400 | void AccelStepper::step2(long step) 401 | { 402 | switch (step & 0x3) 403 | { 404 | case 0: /* 01 */ 405 | setOutputPins(0b10); 406 | break; 407 | 408 | case 1: /* 11 */ 409 | setOutputPins(0b11); 410 | break; 411 | 412 | case 2: /* 10 */ 413 | setOutputPins(0b01); 414 | break; 415 | 416 | case 3: /* 00 */ 417 | setOutputPins(0b00); 418 | break; 419 | } 420 | } 421 | // 3 pin step function 422 | // This is passed the current step number (0 to 7) 423 | // Subclasses can override 424 | void AccelStepper::step3(long step) 425 | { 426 | switch (step % 3) 427 | { 428 | case 0: // 100 429 | setOutputPins(0b100); 430 | break; 431 | 432 | case 1: // 001 433 | setOutputPins(0b001); 434 | break; 435 | 436 | case 2: //010 437 | setOutputPins(0b010); 438 | break; 439 | 440 | } 441 | } 442 | 443 | // 4 pin step function for half stepper 444 | // This is passed the current step number (0 to 7) 445 | // Subclasses can override 446 | void AccelStepper::step4(long step) 447 | { 448 | switch (step & 0x3) 449 | { 450 | case 0: // 1010 451 | setOutputPins(0b0101); 452 | break; 453 | 454 | case 1: // 0110 455 | setOutputPins(0b0110); 456 | break; 457 | 458 | case 2: //0101 459 | setOutputPins(0b1010); 460 | break; 461 | 462 | case 3: //1001 463 | setOutputPins(0b1001); 464 | break; 465 | } 466 | } 467 | 468 | // 3 pin half step function 469 | // This is passed the current step number (0 to 7) 470 | // Subclasses can override 471 | void AccelStepper::step6(long step) 472 | { 473 | switch (step % 6) 474 | { 475 | case 0: // 100 476 | setOutputPins(0b100); 477 | break; 478 | 479 | case 1: // 101 480 | setOutputPins(0b101); 481 | break; 482 | 483 | case 2: // 001 484 | setOutputPins(0b001); 485 | break; 486 | 487 | case 3: // 011 488 | setOutputPins(0b011); 489 | break; 490 | 491 | case 4: // 010 492 | setOutputPins(0b010); 493 | break; 494 | 495 | case 5: // 011 496 | setOutputPins(0b110); 497 | break; 498 | 499 | } 500 | } 501 | 502 | // 4 pin half step function 503 | // This is passed the current step number (0 to 7) 504 | // Subclasses can override 505 | void AccelStepper::step8(long step) 506 | { 507 | switch (step & 0x7) 508 | { 509 | case 0: // 1000 510 | setOutputPins(0b0001); 511 | break; 512 | 513 | case 1: // 1010 514 | setOutputPins(0b0101); 515 | break; 516 | 517 | case 2: // 0010 518 | setOutputPins(0b0100); 519 | break; 520 | 521 | case 3: // 0110 522 | setOutputPins(0b0110); 523 | break; 524 | 525 | case 4: // 0100 526 | setOutputPins(0b0010); 527 | break; 528 | 529 | case 5: //0101 530 | setOutputPins(0b1010); 531 | break; 532 | 533 | case 6: // 0001 534 | setOutputPins(0b1000); 535 | break; 536 | 537 | case 7: //1001 538 | setOutputPins(0b1001); 539 | break; 540 | } 541 | } 542 | 543 | // Prevents power consumption on the outputs 544 | void AccelStepper::disableOutputs() 545 | { 546 | if (! _interface) return; 547 | 548 | setOutputPins(0); // Handles inversion automatically 549 | if (_enablePin != 0xff) 550 | { 551 | pinMode(_enablePin, OUTPUT); 552 | digitalWrite(_enablePin, LOW ^ _enableInverted); 553 | } 554 | } 555 | 556 | void AccelStepper::enableOutputs() 557 | { 558 | if (! _interface) 559 | return; 560 | 561 | pinMode(_pin[0], OUTPUT); 562 | pinMode(_pin[1], OUTPUT); 563 | if (_interface == FULL4WIRE || _interface == HALF4WIRE) 564 | { 565 | pinMode(_pin[2], OUTPUT); 566 | pinMode(_pin[3], OUTPUT); 567 | } 568 | else if (_interface == FULL3WIRE || _interface == HALF3WIRE) 569 | { 570 | pinMode(_pin[2], OUTPUT); 571 | } 572 | 573 | if (_enablePin != 0xff) 574 | { 575 | pinMode(_enablePin, OUTPUT); 576 | digitalWrite(_enablePin, HIGH ^ _enableInverted); 577 | } 578 | } 579 | 580 | void AccelStepper::setMinPulseWidth(unsigned int minWidth) 581 | { 582 | _minPulseWidth = minWidth; 583 | } 584 | 585 | void AccelStepper::setEnablePin(uint8_t enablePin) 586 | { 587 | _enablePin = enablePin; 588 | 589 | // This happens after construction, so init pin now. 590 | if (_enablePin != 0xff) 591 | { 592 | pinMode(_enablePin, OUTPUT); 593 | digitalWrite(_enablePin, HIGH ^ _enableInverted); 594 | } 595 | } 596 | 597 | void AccelStepper::setPinsInverted(bool directionInvert, bool stepInvert, bool enableInvert) 598 | { 599 | _pinInverted[0] = stepInvert; 600 | _pinInverted[1] = directionInvert; 601 | _enableInverted = enableInvert; 602 | } 603 | 604 | void AccelStepper::setPinsInverted(bool pin1Invert, bool pin2Invert, bool pin3Invert, bool pin4Invert, bool enableInvert) 605 | { 606 | _pinInverted[0] = pin1Invert; 607 | _pinInverted[1] = pin2Invert; 608 | _pinInverted[2] = pin3Invert; 609 | _pinInverted[3] = pin4Invert; 610 | _enableInverted = enableInvert; 611 | } 612 | 613 | // Blocks until the target position is reached and stopped 614 | void AccelStepper::runToPosition() 615 | { 616 | while (run()) 617 | YIELD; // Let system housekeeping occur 618 | } 619 | 620 | boolean AccelStepper::runSpeedToPosition() 621 | { 622 | if (_targetPos == _currentPos) 623 | return false; 624 | if (_targetPos >_currentPos) 625 | _direction = DIRECTION_CW; 626 | else 627 | _direction = DIRECTION_CCW; 628 | return runSpeed(); 629 | } 630 | 631 | // Blocks until the new target position is reached 632 | void AccelStepper::runToNewPosition(long position) 633 | { 634 | moveTo(position); 635 | runToPosition(); 636 | } 637 | 638 | void AccelStepper::stop() 639 | { 640 | if (_speed != 0.0) 641 | { 642 | long stepsToStop = (long)((_speed * _speed) / (2.0 * _acceleration)) + 1; // Equation 16 (+integer rounding) 643 | if (_speed > 0) 644 | move(stepsToStop); 645 | else 646 | move(-stepsToStop); 647 | } 648 | } 649 | 650 | bool AccelStepper::isRunning() 651 | { 652 | return !(_speed == 0.0 && _targetPos == _currentPos); 653 | } 654 | -------------------------------------------------------------------------------- /Arduino_ReactionWheel/AccelStepper.h: -------------------------------------------------------------------------------- 1 | // AccelStepper.h 2 | // 3 | /// \mainpage AccelStepper library for Arduino 4 | /// 5 | /// This is the Arduino AccelStepper library. 6 | /// It provides an object-oriented interface for 2, 3 or 4 pin stepper motors and motor drivers. 7 | /// 8 | /// The standard Arduino IDE includes the Stepper library 9 | /// (http://arduino.cc/en/Reference/Stepper) for stepper motors. It is 10 | /// perfectly adequate for simple, single motor applications. 11 | /// 12 | /// AccelStepper significantly improves on the standard Arduino Stepper library in several ways: 13 | /// \li Supports acceleration and deceleration 14 | /// \li Supports multiple simultaneous steppers, with independent concurrent stepping on each stepper 15 | /// \li Most API functions never delay() or block (unless otherwise stated) 16 | /// \li Supports 2, 3 and 4 wire steppers, plus 3 and 4 wire half steppers. 17 | /// \li Supports alternate stepping functions to enable support of AFMotor (https://github.com/adafruit/Adafruit-Motor-Shield-library) 18 | /// \li Supports stepper drivers such as the Sparkfun EasyDriver (based on 3967 driver chip) 19 | /// \li Very slow speeds are supported 20 | /// \li Extensive API 21 | /// \li Subclass support 22 | /// 23 | /// The latest version of this documentation can be downloaded from 24 | /// http://www.airspayce.com/mikem/arduino/AccelStepper 25 | /// The version of the package that this documentation refers to can be downloaded 26 | /// from http://www.airspayce.com/mikem/arduino/AccelStepper/AccelStepper-1.61.zip 27 | /// 28 | /// Example Arduino programs are included to show the main modes of use. 29 | /// 30 | /// You can also find online help and discussion at http://groups.google.com/group/accelstepper 31 | /// Please use that group for all questions and discussions on this topic. 32 | /// Do not contact the author directly, unless it is to discuss commercial licensing. 33 | /// Before asking a question or reporting a bug, please read 34 | /// - http://en.wikipedia.org/wiki/Wikipedia:Reference_desk/How_to_ask_a_software_question 35 | /// - http://www.catb.org/esr/faqs/smart-questions.html 36 | /// - http://www.chiark.greenend.org.uk/~shgtatham/bugs.html 37 | /// 38 | /// Tested on Arduino Diecimila and Mega with arduino-0018 & arduino-0021 39 | /// on OpenSuSE 11.1 and avr-libc-1.6.1-1.15, 40 | /// cross-avr-binutils-2.19-9.1, cross-avr-gcc-4.1.3_20080612-26.5. 41 | /// Tested on Teensy http://www.pjrc.com/teensy including Teensy 3.1 built using Arduino IDE 1.0.5 with 42 | /// teensyduino addon 1.18 and later. 43 | /// 44 | /// \par Installation 45 | /// 46 | /// Install in the usual way: unzip the distribution zip file to the libraries 47 | /// sub-folder of your sketchbook. 48 | /// 49 | /// \par Theory 50 | /// 51 | /// This code uses speed calculations as described in 52 | /// "Generate stepper-motor speed profiles in real time" by David Austin 53 | /// http://fab.cba.mit.edu/classes/MIT/961.09/projects/i0/Stepper_Motor_Speed_Profile.pdf or 54 | /// http://www.embedded.com/design/mcus-processors-and-socs/4006438/Generate-stepper-motor-speed-profiles-in-real-time or 55 | /// http://web.archive.org/web/20140705143928/http://fab.cba.mit.edu/classes/MIT/961.09/projects/i0/Stepper_Motor_Speed_Profile.pdf 56 | /// with the exception that AccelStepper uses steps per second rather than radians per second 57 | /// (because we dont know the step angle of the motor) 58 | /// An initial step interval is calculated for the first step, based on the desired acceleration 59 | /// On subsequent steps, shorter step intervals are calculated based 60 | /// on the previous step until max speed is achieved. 61 | /// 62 | /// \par Adafruit Motor Shield V2 63 | /// 64 | /// The included examples AFMotor_* are for Adafruit Motor Shield V1 and do not work with Adafruit Motor Shield V2. 65 | /// See https://github.com/adafruit/Adafruit_Motor_Shield_V2_Library for examples that work with Adafruit Motor Shield V2. 66 | /// 67 | /// \par Donations 68 | /// 69 | /// This library is offered under a free GPL license for those who want to use it that way. 70 | /// We try hard to keep it up to date, fix bugs 71 | /// and to provide free support. If this library has helped you save time or money, please consider donating at 72 | /// http://www.airspayce.com or here: 73 | /// 74 | /// \htmlonly
\endhtmlonly 75 | /// 76 | /// \par Trademarks 77 | /// 78 | /// AccelStepper is a trademark of AirSpayce Pty Ltd. The AccelStepper mark was first used on April 26 2010 for 79 | /// international trade, and is used only in relation to motor control hardware and software. 80 | /// It is not to be confused with any other similar marks covering other goods and services. 81 | /// 82 | /// \par Copyright 83 | /// 84 | /// This software is Copyright (C) 2010-2018 Mike McCauley. Use is subject to license 85 | /// conditions. The main licensing options available are GPL V3 or Commercial: 86 | /// 87 | /// \par Open Source Licensing GPL V3 88 | /// This is the appropriate option if you want to share the source code of your 89 | /// application with everyone you distribute it to, and you also want to give them 90 | /// the right to share who uses it. If you wish to use this software under Open 91 | /// Source Licensing, you must contribute all your source code to the open source 92 | /// community in accordance with the GPL Version 23 when your application is 93 | /// distributed. See https://www.gnu.org/licenses/gpl-3.0.html 94 | /// 95 | /// \par Commercial Licensing 96 | /// This is the appropriate option if you are creating proprietary applications 97 | /// and you are not prepared to distribute and share the source code of your 98 | /// application. To purchase a commercial license, contact info@airspayce.com 99 | /// 100 | /// \par Revision History 101 | /// \version 1.0 Initial release 102 | /// 103 | /// \version 1.1 Added speed() function to get the current speed. 104 | /// \version 1.2 Added runSpeedToPosition() submitted by Gunnar Arndt. 105 | /// \version 1.3 Added support for stepper drivers (ie with Step and Direction inputs) with _pins == 1 106 | /// \version 1.4 Added functional contructor to support AFMotor, contributed by Limor, with example sketches. 107 | /// \version 1.5 Improvements contributed by Peter Mousley: Use of microsecond steps and other speed improvements 108 | /// to increase max stepping speed to about 4kHz. New option for user to set the min allowed pulse width. 109 | /// Added checks for already running at max speed and skip further calcs if so. 110 | /// \version 1.6 Fixed a problem with wrapping of microsecond stepping that could cause stepping to hang. 111 | /// Reported by Sandy Noble. 112 | /// Removed redundant _lastRunTime member. 113 | /// \version 1.7 Fixed a bug where setCurrentPosition() did not always work as expected. 114 | /// Reported by Peter Linhart. 115 | /// \version 1.8 Added support for 4 pin half-steppers, requested by Harvey Moon 116 | /// \version 1.9 setCurrentPosition() now also sets motor speed to 0. 117 | /// \version 1.10 Builds on Arduino 1.0 118 | /// \version 1.11 Improvments from Michael Ellison: 119 | /// Added optional enable line support for stepper drivers 120 | /// Added inversion for step/direction/enable lines for stepper drivers 121 | /// \version 1.12 Announce Google Group 122 | /// \version 1.13 Improvements to speed calculation. Cost of calculation is now less in the worst case, 123 | /// and more or less constant in all cases. This should result in slightly beter high speed performance, and 124 | /// reduce anomalous speed glitches when other steppers are accelerating. 125 | /// However, its hard to see how to replace the sqrt() required at the very first step from 0 speed. 126 | /// \version 1.14 Fixed a problem with compiling under arduino 0021 reported by EmbeddedMan 127 | /// \version 1.15 Fixed a problem with runSpeedToPosition which did not correctly handle 128 | /// running backwards to a smaller target position. Added examples 129 | /// \version 1.16 Fixed some cases in the code where abs() was used instead of fabs(). 130 | /// \version 1.17 Added example ProportionalControl 131 | /// \version 1.18 Fixed a problem: If one calls the funcion runSpeed() when Speed is zero, it makes steps 132 | /// without counting. reported by Friedrich, Klappenbach. 133 | /// \version 1.19 Added MotorInterfaceType and symbolic names for the number of pins to use 134 | /// for the motor interface. Updated examples to suit. 135 | /// Replaced individual pin assignment variables _pin1, _pin2 etc with array _pin[4]. 136 | /// _pins member changed to _interface. 137 | /// Added _pinInverted array to simplify pin inversion operations. 138 | /// Added new function setOutputPins() which sets the motor output pins. 139 | /// It can be overridden in order to provide, say, serial output instead of parallel output 140 | /// Some refactoring and code size reduction. 141 | /// \version 1.20 Improved documentation and examples to show need for correctly 142 | /// specifying AccelStepper::FULL4WIRE and friends. 143 | /// \version 1.21 Fixed a problem where desiredSpeed could compute the wrong step acceleration 144 | /// when _speed was small but non-zero. Reported by Brian Schmalz. 145 | /// Precompute sqrt_twoa to improve performance and max possible stepping speed 146 | /// \version 1.22 Added Bounce.pde example 147 | /// Fixed a problem where calling moveTo(), setMaxSpeed(), setAcceleration() more 148 | /// frequently than the step time, even 149 | /// with the same values, would interfere with speed calcs. Now a new speed is computed 150 | /// only if there was a change in the set value. Reported by Brian Schmalz. 151 | /// \version 1.23 Rewrite of the speed algorithms in line with 152 | /// http://fab.cba.mit.edu/classes/MIT/961.09/projects/i0/Stepper_Motor_Speed_Profile.pdf 153 | /// Now expect smoother and more linear accelerations and decelerations. The desiredSpeed() 154 | /// function was removed. 155 | /// \version 1.24 Fixed a problem introduced in 1.23: with runToPosition, which did never returned 156 | /// \version 1.25 Now ignore attempts to set acceleration to 0.0 157 | /// \version 1.26 Fixed a problem where certina combinations of speed and accelration could cause 158 | /// oscillation about the target position. 159 | /// \version 1.27 Added stop() function to stop as fast as possible with current acceleration parameters. 160 | /// Also added new Quickstop example showing its use. 161 | /// \version 1.28 Fixed another problem where certain combinations of speed and acceleration could cause 162 | /// oscillation about the target position. 163 | /// Added support for 3 wire full and half steppers such as Hard Disk Drive spindle. 164 | /// Contributed by Yuri Ivatchkovitch. 165 | /// \version 1.29 Fixed a problem that could cause a DRIVER stepper to continually step 166 | /// with some sketches. Reported by Vadim. 167 | /// \version 1.30 Fixed a problem that could cause stepper to back up a few steps at the end of 168 | /// accelerated travel with certain speeds. Reported and patched by jolo. 169 | /// \version 1.31 Updated author and distribution location details to airspayce.com 170 | /// \version 1.32 Fixed a problem with enableOutputs() and setEnablePin on Arduino Due that 171 | /// prevented the enable pin changing stae correctly. Reported by Duane Bishop. 172 | /// \version 1.33 Fixed an error in example AFMotor_ConstantSpeed.pde did not setMaxSpeed(); 173 | /// Fixed a problem that caused incorrect pin sequencing of FULL3WIRE and HALF3WIRE. 174 | /// Unfortunately this meant changing the signature for all step*() functions. 175 | /// Added example MotorShield, showing how to use AdaFruit Motor Shield to control 176 | /// a 3 phase motor such as a HDD spindle motor (and without using the AFMotor library. 177 | /// \version 1.34 Added setPinsInverted(bool pin1Invert, bool pin2Invert, bool pin3Invert, bool pin4Invert, bool enableInvert) 178 | /// to allow inversion of 2, 3 and 4 wire stepper pins. Requested by Oleg. 179 | /// \version 1.35 Removed default args from setPinsInverted(bool, bool, bool, bool, bool) to prevent ambiguity with 180 | /// setPinsInverted(bool, bool, bool). Reported by Mac Mac. 181 | /// \version 1.36 Changed enableOutputs() and disableOutputs() to be virtual so can be overridden. 182 | /// Added new optional argument 'enable' to constructor, which allows you toi disable the 183 | /// automatic enabling of outputs at construction time. Suggested by Guido. 184 | /// \version 1.37 Fixed a problem with step1 that could cause a rogue step in the 185 | /// wrong direction (or not, 186 | /// depending on the setup-time requirements of the connected hardware). 187 | /// Reported by Mark Tillotson. 188 | /// \version 1.38 run() function incorrectly always returned true. Updated function and doc so it returns true 189 | /// if the motor is still running to the target position. 190 | /// \version 1.39 Updated typos in keywords.txt, courtesey Jon Magill. 191 | /// \version 1.40 Updated documentation, including testing on Teensy 3.1 192 | /// \version 1.41 Fixed an error in the acceleration calculations, resulting in acceleration of haldf the intended value 193 | /// \version 1.42 Improved support for FULL3WIRE and HALF3WIRE output pins. These changes were in Yuri's original 194 | /// contribution but did not make it into production.
195 | /// \version 1.43 Added DualMotorShield example. Shows how to use AccelStepper to control 2 x 2 phase steppers using the 196 | /// Itead Studio Arduino Dual Stepper Motor Driver Shield model IM120417015.
197 | /// \version 1.44 examples/DualMotorShield/DualMotorShield.ino examples/DualMotorShield/DualMotorShield.pde 198 | /// was missing from the distribution.
199 | /// \version 1.45 Fixed a problem where if setAcceleration was not called, there was no default 200 | /// acceleration. Reported by Michael Newman.
201 | /// \version 1.45 Fixed inaccuracy in acceleration rate by using Equation 15, suggested by Sebastian Gracki.
202 | /// Performance improvements in runSpeed suggested by Jaakko Fagerlund.
203 | /// \version 1.46 Fixed error in documentation for runToPosition(). 204 | /// Reinstated time calculations in runSpeed() since new version is reported 205 | /// not to work correctly under some circumstances. Reported by Oleg V Gavva.
206 | /// \version 1.48 2015-08-25 207 | /// Added new class MultiStepper that can manage multiple AccelSteppers, 208 | /// and cause them all to move 209 | /// to selected positions at such a (constant) speed that they all arrive at their 210 | /// target position at the same time. Suitable for X-Y flatbeds etc.
211 | /// Added new method maxSpeed() to AccelStepper to return the currently configured maxSpeed.
212 | /// \version 1.49 2016-01-02 213 | /// Testing with VID28 series instrument stepper motors and EasyDriver. 214 | /// OK, although with light pointers 215 | /// and slow speeds like 180 full steps per second the motor movement can be erratic, 216 | /// probably due to some mechanical resonance. Best to accelerate through this speed.
217 | /// Added isRunning().
218 | /// \version 1.50 2016-02-25 219 | /// AccelStepper::disableOutputs now sets the enable pion to OUTPUT mode if the enable pin is defined. 220 | /// Patch from Piet De Jong.
221 | /// Added notes about the fact that AFMotor_* examples do not work with Adafruit Motor Shield V2.
222 | /// \version 1.51 2016-03-24 223 | /// Fixed a problem reported by gregor: when resetting the stepper motor position using setCurrentPosition() the 224 | /// stepper speed is reset by setting _stepInterval to 0, but _speed is not 225 | /// reset. this results in the stepper motor not starting again when calling 226 | /// setSpeed() with the same speed the stepper was set to before. 227 | /// \version 1.52 2016-08-09 228 | /// Added MultiStepper to keywords.txt. 229 | /// Improvements to efficiency of AccelStepper::runSpeed() as suggested by David Grayson. 230 | /// Improvements to speed accuracy as suggested by David Grayson. 231 | /// \version 1.53 2016-08-14 232 | /// Backed out Improvements to speed accuracy from 1.52 as it did not work correctly. 233 | /// \version 1.54 2017-01-24 234 | /// Fixed some warnings about unused arguments. 235 | /// \version 1.55 2017-01-25 236 | /// Fixed another warning in MultiStepper.cpp 237 | /// \version 1.56 2017-02-03 238 | /// Fixed minor documentation error with DIRECTION_CCW and DIRECTION_CW. Reported by David Mutterer. 239 | /// Added link to Binpress commercial license purchasing. 240 | /// \version 1.57 2017-03-28 241 | /// _direction moved to protected at the request of Rudy Ercek. 242 | /// setMaxSpeed() and setAcceleration() now correct negative values to be positive. 243 | /// \version 1.58 2018-04-13 244 | /// Add initialisation for _enableInverted in constructor. 245 | /// \version 1.59 2018-08-28 246 | /// Update commercial licensing, remove binpress. 247 | /// \version 1.60 2020-03-07 248 | /// Release under GPL V3 249 | /// \version 1.61 2020-04-20 250 | /// Added yield() call in runToPosition(), so that platforms like esp8266 dont hang/crash 251 | /// during long runs. 252 | /// 253 | /// \author Mike McCauley (mikem@airspayce.com) DO NOT CONTACT THE AUTHOR DIRECTLY: USE THE LISTS 254 | // Copyright (C) 2009-2013 Mike McCauley 255 | // $Id: AccelStepper.h,v 1.28 2020/04/20 00:15:03 mikem Exp mikem $ 256 | 257 | #ifndef AccelStepper_h 258 | #define AccelStepper_h 259 | 260 | #include 261 | #if ARDUINO >= 100 262 | #include 263 | #else 264 | #include 265 | #include 266 | #endif 267 | 268 | // These defs cause trouble on some versions of Arduino 269 | #undef round 270 | 271 | // Use the system yield() whenever possoible, since some platforms require it for housekeeping, especially 272 | // ESP8266 273 | #if (defined(ARDUINO) && ARDUINO >= 155) || defined(ESP8266) 274 | #define YIELD yield(); 275 | #else 276 | #define YIELD 277 | #endif 278 | 279 | ///////////////////////////////////////////////////////////////////// 280 | /// \class AccelStepper AccelStepper.h 281 | /// \brief Support for stepper motors with acceleration etc. 282 | /// 283 | /// This defines a single 2 or 4 pin stepper motor, or stepper moter with fdriver chip, with optional 284 | /// acceleration, deceleration, absolute positioning commands etc. Multiple 285 | /// simultaneous steppers are supported, all moving 286 | /// at different speeds and accelerations. 287 | /// 288 | /// \par Operation 289 | /// This module operates by computing a step time in microseconds. The step 290 | /// time is recomputed after each step and after speed and acceleration 291 | /// parameters are changed by the caller. The time of each step is recorded in 292 | /// microseconds. The run() function steps the motor once if a new step is due. 293 | /// The run() function must be called frequently until the motor is in the 294 | /// desired position, after which time run() will do nothing. 295 | /// 296 | /// \par Positioning 297 | /// Positions are specified by a signed long integer. At 298 | /// construction time, the current position of the motor is consider to be 0. Positive 299 | /// positions are clockwise from the initial position; negative positions are 300 | /// anticlockwise. The current position can be altered for instance after 301 | /// initialization positioning. 302 | /// 303 | /// \par Caveats 304 | /// This is an open loop controller: If the motor stalls or is oversped, 305 | /// AccelStepper will not have a correct 306 | /// idea of where the motor really is (since there is no feedback of the motor's 307 | /// real position. We only know where we _think_ it is, relative to the 308 | /// initial starting point). 309 | /// 310 | /// \par Performance 311 | /// The fastest motor speed that can be reliably supported is about 4000 steps per 312 | /// second at a clock frequency of 16 MHz on Arduino such as Uno etc. 313 | /// Faster processors can support faster stepping speeds. 314 | /// However, any speed less than that 315 | /// down to very slow speeds (much less than one per second) are also supported, 316 | /// provided the run() function is called frequently enough to step the motor 317 | /// whenever required for the speed set. 318 | /// Calling setAcceleration() is expensive, 319 | /// since it requires a square root to be calculated. 320 | /// 321 | /// Gregor Christandl reports that with an Arduino Due and a simple test program, 322 | /// he measured 43163 steps per second using runSpeed(), 323 | /// and 16214 steps per second using run(); 324 | class AccelStepper 325 | { 326 | public: 327 | /// \brief Symbolic names for number of pins. 328 | /// Use this in the pins argument the AccelStepper constructor to 329 | /// provide a symbolic name for the number of pins 330 | /// to use. 331 | typedef enum 332 | { 333 | FUNCTION = 0, ///< Use the functional interface, implementing your own driver functions (internal use only) 334 | DRIVER = 1, ///< Stepper Driver, 2 driver pins required 335 | FULL2WIRE = 2, ///< 2 wire stepper, 2 motor pins required 336 | FULL3WIRE = 3, ///< 3 wire stepper, such as HDD spindle, 3 motor pins required 337 | FULL4WIRE = 4, ///< 4 wire full stepper, 4 motor pins required 338 | HALF3WIRE = 6, ///< 3 wire half stepper, such as HDD spindle, 3 motor pins required 339 | HALF4WIRE = 8 ///< 4 wire half stepper, 4 motor pins required 340 | } MotorInterfaceType; 341 | 342 | /// Constructor. You can have multiple simultaneous steppers, all moving 343 | /// at different speeds and accelerations, provided you call their run() 344 | /// functions at frequent enough intervals. Current Position is set to 0, target 345 | /// position is set to 0. MaxSpeed and Acceleration default to 1.0. 346 | /// The motor pins will be initialised to OUTPUT mode during the 347 | /// constructor by a call to enableOutputs(). 348 | /// \param[in] interface Number of pins to interface to. Integer values are 349 | /// supported, but it is preferred to use the \ref MotorInterfaceType symbolic names. 350 | /// AccelStepper::DRIVER (1) means a stepper driver (with Step and Direction pins). 351 | /// If an enable line is also needed, call setEnablePin() after construction. 352 | /// You may also invert the pins using setPinsInverted(). 353 | /// Caution: DRIVER implements a blocking delay of minPulseWidth microseconds (default 1us) for each step. 354 | /// You can change this with setMinPulseWidth(). 355 | /// AccelStepper::FULL2WIRE (2) means a 2 wire stepper (2 pins required). 356 | /// AccelStepper::FULL3WIRE (3) means a 3 wire stepper, such as HDD spindle (3 pins required). 357 | /// AccelStepper::FULL4WIRE (4) means a 4 wire stepper (4 pins required). 358 | /// AccelStepper::HALF3WIRE (6) means a 3 wire half stepper, such as HDD spindle (3 pins required) 359 | /// AccelStepper::HALF4WIRE (8) means a 4 wire half stepper (4 pins required) 360 | /// Defaults to AccelStepper::FULL4WIRE (4) pins. 361 | /// \param[in] pin1 Arduino digital pin number for motor pin 1. Defaults 362 | /// to pin 2. For a AccelStepper::DRIVER (interface==1), 363 | /// this is the Step input to the driver. Low to high transition means to step) 364 | /// \param[in] pin2 Arduino digital pin number for motor pin 2. Defaults 365 | /// to pin 3. For a AccelStepper::DRIVER (interface==1), 366 | /// this is the Direction input the driver. High means forward. 367 | /// \param[in] pin3 Arduino digital pin number for motor pin 3. Defaults 368 | /// to pin 4. 369 | /// \param[in] pin4 Arduino digital pin number for motor pin 4. Defaults 370 | /// to pin 5. 371 | /// \param[in] enable If this is true (the default), enableOutputs() will be called to enable 372 | /// the output pins at construction time. 373 | AccelStepper(uint8_t interface = AccelStepper::FULL4WIRE, uint8_t pin1 = 2, uint8_t pin2 = 3, uint8_t pin3 = 4, uint8_t pin4 = 5, bool enable = true); 374 | 375 | /// Alternate Constructor which will call your own functions for forward and backward steps. 376 | /// You can have multiple simultaneous steppers, all moving 377 | /// at different speeds and accelerations, provided you call their run() 378 | /// functions at frequent enough intervals. Current Position is set to 0, target 379 | /// position is set to 0. MaxSpeed and Acceleration default to 1.0. 380 | /// Any motor initialization should happen before hand, no pins are used or initialized. 381 | /// \param[in] forward void-returning procedure that will make a forward step 382 | /// \param[in] backward void-returning procedure that will make a backward step 383 | AccelStepper(void (*forward)(), void (*backward)()); 384 | 385 | /// Set the target position. The run() function will try to move the motor (at most one step per call) 386 | /// from the current position to the target position set by the most 387 | /// recent call to this function. Caution: moveTo() also recalculates the speed for the next step. 388 | /// If you are trying to use constant speed movements, you should call setSpeed() after calling moveTo(). 389 | /// \param[in] absolute The desired absolute position. Negative is 390 | /// anticlockwise from the 0 position. 391 | void moveTo(long absolute); 392 | 393 | /// Set the target position relative to the current position. 394 | /// \param[in] relative The desired position relative to the current position. Negative is 395 | /// anticlockwise from the current position. 396 | void move(long relative); 397 | 398 | /// Poll the motor and step it if a step is due, implementing 399 | /// accelerations and decelerations to achieve the target position. You must call this as 400 | /// frequently as possible, but at least once per minimum step time interval, 401 | /// preferably in your main loop. Note that each call to run() will make at most one step, and then only when a step is due, 402 | /// based on the current speed and the time since the last step. 403 | /// \return true if the motor is still running to the target position. 404 | boolean run(); 405 | 406 | /// Poll the motor and step it if a step is due, implementing a constant 407 | /// speed as set by the most recent call to setSpeed(). You must call this as 408 | /// frequently as possible, but at least once per step interval, 409 | /// \return true if the motor was stepped. 410 | boolean runSpeed(); 411 | 412 | /// Sets the maximum permitted speed. The run() function will accelerate 413 | /// up to the speed set by this function. 414 | /// Caution: the maximum speed achievable depends on your processor and clock speed. 415 | /// The default maxSpeed is 1.0 steps per second. 416 | /// \param[in] speed The desired maximum speed in steps per second. Must 417 | /// be > 0. Caution: Speeds that exceed the maximum speed supported by the processor may 418 | /// Result in non-linear accelerations and decelerations. 419 | void setMaxSpeed(float speed); 420 | 421 | /// Returns the maximum speed configured for this stepper 422 | /// that was previously set by setMaxSpeed(); 423 | /// \return The currently configured maximum speed 424 | float maxSpeed(); 425 | 426 | /// Sets the acceleration/deceleration rate. 427 | /// \param[in] acceleration The desired acceleration in steps per second 428 | /// per second. Must be > 0.0. This is an expensive call since it requires a square 429 | /// root to be calculated. Dont call more ofthen than needed 430 | void setAcceleration(float acceleration); 431 | 432 | /// Sets the desired constant speed for use with runSpeed(). 433 | /// \param[in] speed The desired constant speed in steps per 434 | /// second. Positive is clockwise. Speeds of more than 1000 steps per 435 | /// second are unreliable. Very slow speeds may be set (eg 0.00027777 for 436 | /// once per hour, approximately. Speed accuracy depends on the Arduino 437 | /// crystal. Jitter depends on how frequently you call the runSpeed() function. 438 | /// The speed will be limited by the current value of setMaxSpeed() 439 | void setSpeed(float speed); 440 | 441 | /// The most recently set speed. 442 | /// \return the most recent speed in steps per second 443 | float speed(); 444 | 445 | /// The distance from the current position to the target position. 446 | /// \return the distance from the current position to the target position 447 | /// in steps. Positive is clockwise from the current position. 448 | long distanceToGo(); 449 | 450 | /// The most recently set target position. 451 | /// \return the target position 452 | /// in steps. Positive is clockwise from the 0 position. 453 | long targetPosition(); 454 | 455 | /// The current motor position. 456 | /// \return the current motor position 457 | /// in steps. Positive is clockwise from the 0 position. 458 | long currentPosition(); 459 | 460 | /// Resets the current position of the motor, so that wherever the motor 461 | /// happens to be right now is considered to be the new 0 position. Useful 462 | /// for setting a zero position on a stepper after an initial hardware 463 | /// positioning move. 464 | /// Has the side effect of setting the current motor speed to 0. 465 | /// \param[in] position The position in steps of wherever the motor 466 | /// happens to be right now. 467 | void setCurrentPosition(long position); 468 | 469 | /// Moves the motor (with acceleration/deceleration) 470 | /// to the target position and blocks until it is at 471 | /// position. Dont use this in event loops, since it blocks. 472 | void runToPosition(); 473 | 474 | /// Runs at the currently selected speed until the target position is reached. 475 | /// Does not implement accelerations. 476 | /// \return true if it stepped 477 | boolean runSpeedToPosition(); 478 | 479 | /// Moves the motor (with acceleration/deceleration) 480 | /// to the new target position and blocks until it is at 481 | /// position. Dont use this in event loops, since it blocks. 482 | /// \param[in] position The new target position. 483 | void runToNewPosition(long position); 484 | 485 | /// Sets a new target position that causes the stepper 486 | /// to stop as quickly as possible, using the current speed and acceleration parameters. 487 | void stop(); 488 | 489 | /// Disable motor pin outputs by setting them all LOW 490 | /// Depending on the design of your electronics this may turn off 491 | /// the power to the motor coils, saving power. 492 | /// This is useful to support Arduino low power modes: disable the outputs 493 | /// during sleep and then reenable with enableOutputs() before stepping 494 | /// again. 495 | /// If the enable Pin is defined, sets it to OUTPUT mode and clears the pin to disabled. 496 | virtual void disableOutputs(); 497 | 498 | /// Enable motor pin outputs by setting the motor pins to OUTPUT 499 | /// mode. Called automatically by the constructor. 500 | /// If the enable Pin is defined, sets it to OUTPUT mode and sets the pin to enabled. 501 | virtual void enableOutputs(); 502 | 503 | /// Sets the minimum pulse width allowed by the stepper driver. The minimum practical pulse width is 504 | /// approximately 20 microseconds. Times less than 20 microseconds 505 | /// will usually result in 20 microseconds or so. 506 | /// \param[in] minWidth The minimum pulse width in microseconds. 507 | void setMinPulseWidth(unsigned int minWidth); 508 | 509 | /// Sets the enable pin number for stepper drivers. 510 | /// 0xFF indicates unused (default). 511 | /// Otherwise, if a pin is set, the pin will be turned on when 512 | /// enableOutputs() is called and switched off when disableOutputs() 513 | /// is called. 514 | /// \param[in] enablePin Arduino digital pin number for motor enable 515 | /// \sa setPinsInverted 516 | void setEnablePin(uint8_t enablePin = 0xff); 517 | 518 | /// Sets the inversion for stepper driver pins 519 | /// \param[in] directionInvert True for inverted direction pin, false for non-inverted 520 | /// \param[in] stepInvert True for inverted step pin, false for non-inverted 521 | /// \param[in] enableInvert True for inverted enable pin, false (default) for non-inverted 522 | void setPinsInverted(bool directionInvert = false, bool stepInvert = false, bool enableInvert = false); 523 | 524 | /// Sets the inversion for 2, 3 and 4 wire stepper pins 525 | /// \param[in] pin1Invert True for inverted pin1, false for non-inverted 526 | /// \param[in] pin2Invert True for inverted pin2, false for non-inverted 527 | /// \param[in] pin3Invert True for inverted pin3, false for non-inverted 528 | /// \param[in] pin4Invert True for inverted pin4, false for non-inverted 529 | /// \param[in] enableInvert True for inverted enable pin, false (default) for non-inverted 530 | void setPinsInverted(bool pin1Invert, bool pin2Invert, bool pin3Invert, bool pin4Invert, bool enableInvert); 531 | 532 | /// Checks to see if the motor is currently running to a target 533 | /// \return true if the speed is not zero or not at the target position 534 | bool isRunning(); 535 | 536 | protected: 537 | 538 | /// \brief Direction indicator 539 | /// Symbolic names for the direction the motor is turning 540 | typedef enum 541 | { 542 | DIRECTION_CCW = 0, ///< Counter-Clockwise 543 | DIRECTION_CW = 1 ///< Clockwise 544 | } Direction; 545 | 546 | /// Forces the library to compute a new instantaneous speed and set that as 547 | /// the current speed. It is called by 548 | /// the library: 549 | /// \li after each step 550 | /// \li after change to maxSpeed through setMaxSpeed() 551 | /// \li after change to acceleration through setAcceleration() 552 | /// \li after change to target position (relative or absolute) through 553 | /// move() or moveTo() 554 | void computeNewSpeed(); 555 | 556 | /// Low level function to set the motor output pins 557 | /// bit 0 of the mask corresponds to _pin[0] 558 | /// bit 1 of the mask corresponds to _pin[1] 559 | /// You can override this to impment, for example serial chip output insted of using the 560 | /// output pins directly 561 | virtual void setOutputPins(uint8_t mask); 562 | 563 | /// Called to execute a step. Only called when a new step is 564 | /// required. Subclasses may override to implement new stepping 565 | /// interfaces. The default calls step1(), step2(), step4() or step8() depending on the 566 | /// number of pins defined for the stepper. 567 | /// \param[in] step The current step phase number (0 to 7) 568 | virtual void step(long step); 569 | 570 | /// Called to execute a step using stepper functions (pins = 0) Only called when a new step is 571 | /// required. Calls _forward() or _backward() to perform the step 572 | /// \param[in] step The current step phase number (0 to 7) 573 | virtual void step0(long step); 574 | 575 | /// Called to execute a step on a stepper driver (ie where pins == 1). Only called when a new step is 576 | /// required. Subclasses may override to implement new stepping 577 | /// interfaces. The default sets or clears the outputs of Step pin1 to step, 578 | /// and sets the output of _pin2 to the desired direction. The Step pin (_pin1) is pulsed for 1 microsecond 579 | /// which is the minimum STEP pulse width for the 3967 driver. 580 | /// \param[in] step The current step phase number (0 to 7) 581 | virtual void step1(long step); 582 | 583 | /// Called to execute a step on a 2 pin motor. Only called when a new step is 584 | /// required. Subclasses may override to implement new stepping 585 | /// interfaces. The default sets or clears the outputs of pin1 and pin2 586 | /// \param[in] step The current step phase number (0 to 7) 587 | virtual void step2(long step); 588 | 589 | /// Called to execute a step on a 3 pin motor, such as HDD spindle. Only called when a new step is 590 | /// required. Subclasses may override to implement new stepping 591 | /// interfaces. The default sets or clears the outputs of pin1, pin2, 592 | /// pin3 593 | /// \param[in] step The current step phase number (0 to 7) 594 | virtual void step3(long step); 595 | 596 | /// Called to execute a step on a 4 pin motor. Only called when a new step is 597 | /// required. Subclasses may override to implement new stepping 598 | /// interfaces. The default sets or clears the outputs of pin1, pin2, 599 | /// pin3, pin4. 600 | /// \param[in] step The current step phase number (0 to 7) 601 | virtual void step4(long step); 602 | 603 | /// Called to execute a step on a 3 pin motor, such as HDD spindle. Only called when a new step is 604 | /// required. Subclasses may override to implement new stepping 605 | /// interfaces. The default sets or clears the outputs of pin1, pin2, 606 | /// pin3 607 | /// \param[in] step The current step phase number (0 to 7) 608 | virtual void step6(long step); 609 | 610 | /// Called to execute a step on a 4 pin half-steper motor. Only called when a new step is 611 | /// required. Subclasses may override to implement new stepping 612 | /// interfaces. The default sets or clears the outputs of pin1, pin2, 613 | /// pin3, pin4. 614 | /// \param[in] step The current step phase number (0 to 7) 615 | virtual void step8(long step); 616 | 617 | /// Current direction motor is spinning in 618 | /// Protected because some peoples subclasses need it to be so 619 | boolean _direction; // 1 == CW 620 | 621 | private: 622 | /// Number of pins on the stepper motor. Permits 2 or 4. 2 pins is a 623 | /// bipolar, and 4 pins is a unipolar. 624 | uint8_t _interface; // 0, 1, 2, 4, 8, See MotorInterfaceType 625 | 626 | /// Arduino pin number assignments for the 2 or 4 pins required to interface to the 627 | /// stepper motor or driver 628 | uint8_t _pin[4]; 629 | 630 | /// Whether the _pins is inverted or not 631 | uint8_t _pinInverted[4]; 632 | 633 | /// The current absolution position in steps. 634 | long _currentPos; // Steps 635 | 636 | /// The target position in steps. The AccelStepper library will move the 637 | /// motor from the _currentPos to the _targetPos, taking into account the 638 | /// max speed, acceleration and deceleration 639 | long _targetPos; // Steps 640 | 641 | /// The current motos speed in steps per second 642 | /// Positive is clockwise 643 | float _speed; // Steps per second 644 | 645 | /// The maximum permitted speed in steps per second. Must be > 0. 646 | float _maxSpeed; 647 | 648 | /// The acceleration to use to accelerate or decelerate the motor in steps 649 | /// per second per second. Must be > 0 650 | float _acceleration; 651 | float _sqrt_twoa; // Precomputed sqrt(2*_acceleration) 652 | 653 | /// The current interval between steps in microseconds. 654 | /// 0 means the motor is currently stopped with _speed == 0 655 | unsigned long _stepInterval; 656 | 657 | /// The last step time in microseconds 658 | unsigned long _lastStepTime; 659 | 660 | /// The minimum allowed pulse width in microseconds 661 | unsigned int _minPulseWidth; 662 | 663 | /// Is the direction pin inverted? 664 | ///bool _dirInverted; /// Moved to _pinInverted[1] 665 | 666 | /// Is the step pin inverted? 667 | ///bool _stepInverted; /// Moved to _pinInverted[0] 668 | 669 | /// Is the enable pin inverted? 670 | bool _enableInverted; 671 | 672 | /// Enable pin for stepper driver, or 0xFF if unused. 673 | uint8_t _enablePin; 674 | 675 | /// The pointer to a forward-step procedure 676 | void (*_forward)(); 677 | 678 | /// The pointer to a backward-step procedure 679 | void (*_backward)(); 680 | 681 | /// The step counter for speed calculations 682 | long _n; 683 | 684 | /// Initial step size in microseconds 685 | float _c0; 686 | 687 | /// Last step size in microseconds 688 | float _cn; 689 | 690 | /// Min step size in microseconds based on maxSpeed 691 | float _cmin; // at max speed 692 | 693 | }; 694 | 695 | /// @example Random.pde 696 | /// Make a single stepper perform random changes in speed, position and acceleration 697 | 698 | /// @example Overshoot.pde 699 | /// Check overshoot handling 700 | /// which sets a new target position and then waits until the stepper has 701 | /// achieved it. This is used for testing the handling of overshoots 702 | 703 | /// @example MultipleSteppers.pde 704 | /// Shows how to multiple simultaneous steppers 705 | /// Runs one stepper forwards and backwards, accelerating and decelerating 706 | /// at the limits. Runs other steppers at the same time 707 | 708 | /// @example ConstantSpeed.pde 709 | /// Shows how to run AccelStepper in the simplest, 710 | /// fixed speed mode with no accelerations 711 | 712 | /// @example Blocking.pde 713 | /// Shows how to use the blocking call runToNewPosition 714 | /// Which sets a new target position and then waits until the stepper has 715 | /// achieved it. 716 | 717 | /// @example AFMotor_MultiStepper.pde 718 | /// Control both Stepper motors at the same time with different speeds 719 | /// and accelerations. 720 | 721 | /// @example AFMotor_ConstantSpeed.pde 722 | /// Shows how to run AccelStepper in the simplest, 723 | /// fixed speed mode with no accelerations 724 | 725 | /// @example ProportionalControl.pde 726 | /// Make a single stepper follow the analog value read from a pot or whatever 727 | /// The stepper will move at a constant speed to each newly set posiiton, 728 | /// depending on the value of the pot. 729 | 730 | /// @example Bounce.pde 731 | /// Make a single stepper bounce from one limit to another, observing 732 | /// accelrations at each end of travel 733 | 734 | /// @example Quickstop.pde 735 | /// Check stop handling. 736 | /// Calls stop() while the stepper is travelling at full speed, causing 737 | /// the stepper to stop as quickly as possible, within the constraints of the 738 | /// current acceleration. 739 | 740 | /// @example MotorShield.pde 741 | /// Shows how to use AccelStepper to control a 3-phase motor, such as a HDD spindle motor 742 | /// using the Adafruit Motor Shield http://www.ladyada.net/make/mshield/index.html. 743 | 744 | /// @example DualMotorShield.pde 745 | /// Shows how to use AccelStepper to control 2 x 2 phase steppers using the 746 | /// Itead Studio Arduino Dual Stepper Motor Driver Shield 747 | /// model IM120417015 748 | 749 | #endif 750 | --------------------------------------------------------------------------------