Adafruit-MotorShield
Dependents: Adafruit-MortorShield_sample Low_Power_Long_Distance_IR_Vision_Robot Low_Power_Long_Distance_IR_Vision_Robot
Adafruit_MotorShield.cpp@0:3d17b246f7fe, 2014-10-15 (annotated)
- Committer:
- robo8080
- Date:
- Wed Oct 15 02:16:15 2014 +0000
- Revision:
- 0:3d17b246f7fe
test1
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
robo8080 | 0:3d17b246f7fe | 1 | /****************************************************************** |
robo8080 | 0:3d17b246f7fe | 2 | This is the library for the Adafruit Motor Shield V2 for Arduino. |
robo8080 | 0:3d17b246f7fe | 3 | It supports DC motors & Stepper motors with microstepping as well |
robo8080 | 0:3d17b246f7fe | 4 | as stacking-support. It is *not* compatible with the V1 library! |
robo8080 | 0:3d17b246f7fe | 5 | |
robo8080 | 0:3d17b246f7fe | 6 | It will only work with https://www.adafruit.com/products/1483 |
robo8080 | 0:3d17b246f7fe | 7 | |
robo8080 | 0:3d17b246f7fe | 8 | Adafruit invests time and resources providing this open |
robo8080 | 0:3d17b246f7fe | 9 | source code, please support Adafruit and open-source hardware |
robo8080 | 0:3d17b246f7fe | 10 | by purchasing products from Adafruit! |
robo8080 | 0:3d17b246f7fe | 11 | |
robo8080 | 0:3d17b246f7fe | 12 | Written by Limor Fried/Ladyada for Adafruit Industries. |
robo8080 | 0:3d17b246f7fe | 13 | BSD license, check license.txt for more information. |
robo8080 | 0:3d17b246f7fe | 14 | All text above must be included in any redistribution. |
robo8080 | 0:3d17b246f7fe | 15 | ******************************************************************/ |
robo8080 | 0:3d17b246f7fe | 16 | |
robo8080 | 0:3d17b246f7fe | 17 | |
robo8080 | 0:3d17b246f7fe | 18 | //#if (ARDUINO >= 100) |
robo8080 | 0:3d17b246f7fe | 19 | // #include "Arduino.h" |
robo8080 | 0:3d17b246f7fe | 20 | //#else |
robo8080 | 0:3d17b246f7fe | 21 | // #include "WProgram.h" |
robo8080 | 0:3d17b246f7fe | 22 | //#endif |
robo8080 | 0:3d17b246f7fe | 23 | //#include <Wire.h> |
robo8080 | 0:3d17b246f7fe | 24 | #include "Adafruit_MotorShield.h" |
robo8080 | 0:3d17b246f7fe | 25 | #include <Adafruit_PWMServoDriver.h> |
robo8080 | 0:3d17b246f7fe | 26 | //#ifdef __AVR__ |
robo8080 | 0:3d17b246f7fe | 27 | // #define WIRE Wire |
robo8080 | 0:3d17b246f7fe | 28 | //#else // Arduino Due |
robo8080 | 0:3d17b246f7fe | 29 | // #define WIRE Wire1 |
robo8080 | 0:3d17b246f7fe | 30 | //#endif |
robo8080 | 0:3d17b246f7fe | 31 | |
robo8080 | 0:3d17b246f7fe | 32 | #define LOW 0 |
robo8080 | 0:3d17b246f7fe | 33 | #define HIGH 1 |
robo8080 | 0:3d17b246f7fe | 34 | |
robo8080 | 0:3d17b246f7fe | 35 | #if (MICROSTEPS == 8) |
robo8080 | 0:3d17b246f7fe | 36 | uint8_t microstepcurve[] = {0, 50, 98, 142, 180, 212, 236, 250, 255}; |
robo8080 | 0:3d17b246f7fe | 37 | #elif (MICROSTEPS == 16) |
robo8080 | 0:3d17b246f7fe | 38 | uint8_t microstepcurve[] = {0, 25, 50, 74, 98, 120, 141, 162, 180, 197, 212, 225, 236, 244, 250, 253, 255}; |
robo8080 | 0:3d17b246f7fe | 39 | #endif |
robo8080 | 0:3d17b246f7fe | 40 | |
robo8080 | 0:3d17b246f7fe | 41 | //Adafruit_MotorShield::Adafruit_MotorShield(PinName sda, PinName scl, uint8_t addr) { |
robo8080 | 0:3d17b246f7fe | 42 | Adafruit_MotorShield::Adafruit_MotorShield(PinName sda, PinName scl, uint8_t addr):_pwm(sda, scl, addr) { |
robo8080 | 0:3d17b246f7fe | 43 | _addr = addr; |
robo8080 | 0:3d17b246f7fe | 44 | // _pwm = Adafruit_PWMServoDriver(sda, scl, _addr); |
robo8080 | 0:3d17b246f7fe | 45 | } |
robo8080 | 0:3d17b246f7fe | 46 | |
robo8080 | 0:3d17b246f7fe | 47 | void Adafruit_MotorShield::begin(uint16_t freq) { |
robo8080 | 0:3d17b246f7fe | 48 | // init PWM w/_freq |
robo8080 | 0:3d17b246f7fe | 49 | // WIRE.begin(); |
robo8080 | 0:3d17b246f7fe | 50 | _pwm.begin(); |
robo8080 | 0:3d17b246f7fe | 51 | _freq = freq; |
robo8080 | 0:3d17b246f7fe | 52 | _pwm.setPWMFreq(_freq); // This is the maximum PWM frequency |
robo8080 | 0:3d17b246f7fe | 53 | for (uint8_t i=0; i<16; i++) |
robo8080 | 0:3d17b246f7fe | 54 | _pwm.setPWM(i, 0, 0); |
robo8080 | 0:3d17b246f7fe | 55 | } |
robo8080 | 0:3d17b246f7fe | 56 | |
robo8080 | 0:3d17b246f7fe | 57 | void Adafruit_MotorShield::setPWM(uint8_t pin, uint16_t value) { |
robo8080 | 0:3d17b246f7fe | 58 | if (value > 4095) { |
robo8080 | 0:3d17b246f7fe | 59 | _pwm.setPWM(pin, 4096, 0); |
robo8080 | 0:3d17b246f7fe | 60 | } else |
robo8080 | 0:3d17b246f7fe | 61 | _pwm.setPWM(pin, 0, value); |
robo8080 | 0:3d17b246f7fe | 62 | } |
robo8080 | 0:3d17b246f7fe | 63 | //void Adafruit_MotorShield::setPin(uint8_t pin, bool value) { |
robo8080 | 0:3d17b246f7fe | 64 | void Adafruit_MotorShield::setPin(uint8_t pin, uint16_t value) { |
robo8080 | 0:3d17b246f7fe | 65 | if (value == LOW) |
robo8080 | 0:3d17b246f7fe | 66 | _pwm.setPWM(pin, 0, 0); |
robo8080 | 0:3d17b246f7fe | 67 | else |
robo8080 | 0:3d17b246f7fe | 68 | _pwm.setPWM(pin, 4096, 0); |
robo8080 | 0:3d17b246f7fe | 69 | } |
robo8080 | 0:3d17b246f7fe | 70 | |
robo8080 | 0:3d17b246f7fe | 71 | Adafruit_DCMotor *Adafruit_MotorShield::getMotor(uint8_t num) { |
robo8080 | 0:3d17b246f7fe | 72 | if (num > 4) return NULL; |
robo8080 | 0:3d17b246f7fe | 73 | |
robo8080 | 0:3d17b246f7fe | 74 | num--; |
robo8080 | 0:3d17b246f7fe | 75 | |
robo8080 | 0:3d17b246f7fe | 76 | if (dcmotors[num].motornum == 0) { |
robo8080 | 0:3d17b246f7fe | 77 | // not init'd yet! |
robo8080 | 0:3d17b246f7fe | 78 | dcmotors[num].motornum = num; |
robo8080 | 0:3d17b246f7fe | 79 | dcmotors[num].MC = this; |
robo8080 | 0:3d17b246f7fe | 80 | uint8_t pwm, in1, in2; |
robo8080 | 0:3d17b246f7fe | 81 | if (num == 0) { |
robo8080 | 0:3d17b246f7fe | 82 | pwm = 8; in2 = 9; in1 = 10; |
robo8080 | 0:3d17b246f7fe | 83 | } else if (num == 1) { |
robo8080 | 0:3d17b246f7fe | 84 | pwm = 13; in2 = 12; in1 = 11; |
robo8080 | 0:3d17b246f7fe | 85 | } else if (num == 2) { |
robo8080 | 0:3d17b246f7fe | 86 | pwm = 2; in2 = 3; in1 = 4; |
robo8080 | 0:3d17b246f7fe | 87 | } else if (num == 3) { |
robo8080 | 0:3d17b246f7fe | 88 | pwm = 7; in2 = 6; in1 = 5; |
robo8080 | 0:3d17b246f7fe | 89 | } |
robo8080 | 0:3d17b246f7fe | 90 | dcmotors[num].PWMpin = pwm; |
robo8080 | 0:3d17b246f7fe | 91 | dcmotors[num].IN1pin = in1; |
robo8080 | 0:3d17b246f7fe | 92 | dcmotors[num].IN2pin = in2; |
robo8080 | 0:3d17b246f7fe | 93 | } |
robo8080 | 0:3d17b246f7fe | 94 | return &dcmotors[num]; |
robo8080 | 0:3d17b246f7fe | 95 | } |
robo8080 | 0:3d17b246f7fe | 96 | |
robo8080 | 0:3d17b246f7fe | 97 | |
robo8080 | 0:3d17b246f7fe | 98 | Adafruit_StepperMotor *Adafruit_MotorShield::getStepper(uint16_t steps, uint8_t num) { |
robo8080 | 0:3d17b246f7fe | 99 | if (num > 2) return NULL; |
robo8080 | 0:3d17b246f7fe | 100 | |
robo8080 | 0:3d17b246f7fe | 101 | num--; |
robo8080 | 0:3d17b246f7fe | 102 | |
robo8080 | 0:3d17b246f7fe | 103 | if (steppers[num].steppernum == 0) { |
robo8080 | 0:3d17b246f7fe | 104 | // not init'd yet! |
robo8080 | 0:3d17b246f7fe | 105 | steppers[num].steppernum = num; |
robo8080 | 0:3d17b246f7fe | 106 | steppers[num].revsteps = steps; |
robo8080 | 0:3d17b246f7fe | 107 | steppers[num].MC = this; |
robo8080 | 0:3d17b246f7fe | 108 | uint8_t pwma, pwmb, ain1, ain2, bin1, bin2; |
robo8080 | 0:3d17b246f7fe | 109 | if (num == 0) { |
robo8080 | 0:3d17b246f7fe | 110 | pwma = 8; ain2 = 9; ain1 = 10; |
robo8080 | 0:3d17b246f7fe | 111 | pwmb = 13; bin2 = 12; bin1 = 11; |
robo8080 | 0:3d17b246f7fe | 112 | } else if (num == 1) { |
robo8080 | 0:3d17b246f7fe | 113 | pwma = 2; ain2 = 3; ain1 = 4; |
robo8080 | 0:3d17b246f7fe | 114 | pwmb = 7; bin2 = 6; bin1 = 5; |
robo8080 | 0:3d17b246f7fe | 115 | } |
robo8080 | 0:3d17b246f7fe | 116 | steppers[num].PWMApin = pwma; |
robo8080 | 0:3d17b246f7fe | 117 | steppers[num].PWMBpin = pwmb; |
robo8080 | 0:3d17b246f7fe | 118 | steppers[num].AIN1pin = ain1; |
robo8080 | 0:3d17b246f7fe | 119 | steppers[num].AIN2pin = ain2; |
robo8080 | 0:3d17b246f7fe | 120 | steppers[num].BIN1pin = bin1; |
robo8080 | 0:3d17b246f7fe | 121 | steppers[num].BIN2pin = bin2; |
robo8080 | 0:3d17b246f7fe | 122 | } |
robo8080 | 0:3d17b246f7fe | 123 | return &steppers[num]; |
robo8080 | 0:3d17b246f7fe | 124 | } |
robo8080 | 0:3d17b246f7fe | 125 | |
robo8080 | 0:3d17b246f7fe | 126 | |
robo8080 | 0:3d17b246f7fe | 127 | /****************************************** |
robo8080 | 0:3d17b246f7fe | 128 | MOTORS |
robo8080 | 0:3d17b246f7fe | 129 | ******************************************/ |
robo8080 | 0:3d17b246f7fe | 130 | |
robo8080 | 0:3d17b246f7fe | 131 | Adafruit_DCMotor::Adafruit_DCMotor(void) { |
robo8080 | 0:3d17b246f7fe | 132 | MC = NULL; |
robo8080 | 0:3d17b246f7fe | 133 | motornum = 0; |
robo8080 | 0:3d17b246f7fe | 134 | PWMpin = IN1pin = IN2pin = 0; |
robo8080 | 0:3d17b246f7fe | 135 | } |
robo8080 | 0:3d17b246f7fe | 136 | |
robo8080 | 0:3d17b246f7fe | 137 | void Adafruit_DCMotor::run(uint8_t cmd) { |
robo8080 | 0:3d17b246f7fe | 138 | switch (cmd) { |
robo8080 | 0:3d17b246f7fe | 139 | case FORWARD: |
robo8080 | 0:3d17b246f7fe | 140 | MC->setPin(IN2pin, LOW); // take low first to avoid 'break' |
robo8080 | 0:3d17b246f7fe | 141 | MC->setPin(IN1pin, HIGH); |
robo8080 | 0:3d17b246f7fe | 142 | break; |
robo8080 | 0:3d17b246f7fe | 143 | case BACKWARD: |
robo8080 | 0:3d17b246f7fe | 144 | MC->setPin(IN1pin, LOW); // take low first to avoid 'break' |
robo8080 | 0:3d17b246f7fe | 145 | MC->setPin(IN2pin, HIGH); |
robo8080 | 0:3d17b246f7fe | 146 | break; |
robo8080 | 0:3d17b246f7fe | 147 | case RELEASE: |
robo8080 | 0:3d17b246f7fe | 148 | MC->setPin(IN1pin, LOW); |
robo8080 | 0:3d17b246f7fe | 149 | MC->setPin(IN2pin, LOW); |
robo8080 | 0:3d17b246f7fe | 150 | break; |
robo8080 | 0:3d17b246f7fe | 151 | } |
robo8080 | 0:3d17b246f7fe | 152 | } |
robo8080 | 0:3d17b246f7fe | 153 | |
robo8080 | 0:3d17b246f7fe | 154 | void Adafruit_DCMotor::setSpeed(uint8_t speed) { |
robo8080 | 0:3d17b246f7fe | 155 | MC->setPWM(PWMpin, speed*16); |
robo8080 | 0:3d17b246f7fe | 156 | } |
robo8080 | 0:3d17b246f7fe | 157 | |
robo8080 | 0:3d17b246f7fe | 158 | /****************************************** |
robo8080 | 0:3d17b246f7fe | 159 | STEPPERS |
robo8080 | 0:3d17b246f7fe | 160 | ******************************************/ |
robo8080 | 0:3d17b246f7fe | 161 | |
robo8080 | 0:3d17b246f7fe | 162 | Adafruit_StepperMotor::Adafruit_StepperMotor(void) { |
robo8080 | 0:3d17b246f7fe | 163 | revsteps = steppernum = currentstep = 0; |
robo8080 | 0:3d17b246f7fe | 164 | } |
robo8080 | 0:3d17b246f7fe | 165 | /* |
robo8080 | 0:3d17b246f7fe | 166 | |
robo8080 | 0:3d17b246f7fe | 167 | uint16_t steps, Adafruit_MotorShield controller) { |
robo8080 | 0:3d17b246f7fe | 168 | |
robo8080 | 0:3d17b246f7fe | 169 | revsteps = steps; |
robo8080 | 0:3d17b246f7fe | 170 | steppernum = 1; |
robo8080 | 0:3d17b246f7fe | 171 | currentstep = 0; |
robo8080 | 0:3d17b246f7fe | 172 | |
robo8080 | 0:3d17b246f7fe | 173 | if (steppernum == 1) { |
robo8080 | 0:3d17b246f7fe | 174 | latch_state &= ~_BV(MOTOR1_A) & ~_BV(MOTOR1_B) & |
robo8080 | 0:3d17b246f7fe | 175 | ~_BV(MOTOR2_A) & ~_BV(MOTOR2_B); // all motor pins to 0 |
robo8080 | 0:3d17b246f7fe | 176 | |
robo8080 | 0:3d17b246f7fe | 177 | // enable both H bridges |
robo8080 | 0:3d17b246f7fe | 178 | pinMode(11, OUTPUT); |
robo8080 | 0:3d17b246f7fe | 179 | pinMode(3, OUTPUT); |
robo8080 | 0:3d17b246f7fe | 180 | digitalWrite(11, HIGH); |
robo8080 | 0:3d17b246f7fe | 181 | digitalWrite(3, HIGH); |
robo8080 | 0:3d17b246f7fe | 182 | |
robo8080 | 0:3d17b246f7fe | 183 | // use PWM for microstepping support |
robo8080 | 0:3d17b246f7fe | 184 | MC->setPWM(1, 255); |
robo8080 | 0:3d17b246f7fe | 185 | MC->setPWM(2, 255); |
robo8080 | 0:3d17b246f7fe | 186 | |
robo8080 | 0:3d17b246f7fe | 187 | } else if (steppernum == 2) { |
robo8080 | 0:3d17b246f7fe | 188 | latch_state &= ~_BV(MOTOR3_A) & ~_BV(MOTOR3_B) & |
robo8080 | 0:3d17b246f7fe | 189 | ~_BV(MOTOR4_A) & ~_BV(MOTOR4_B); // all motor pins to 0 |
robo8080 | 0:3d17b246f7fe | 190 | |
robo8080 | 0:3d17b246f7fe | 191 | // enable both H bridges |
robo8080 | 0:3d17b246f7fe | 192 | pinMode(5, OUTPUT); |
robo8080 | 0:3d17b246f7fe | 193 | pinMode(6, OUTPUT); |
robo8080 | 0:3d17b246f7fe | 194 | digitalWrite(5, HIGH); |
robo8080 | 0:3d17b246f7fe | 195 | digitalWrite(6, HIGH); |
robo8080 | 0:3d17b246f7fe | 196 | |
robo8080 | 0:3d17b246f7fe | 197 | // use PWM for microstepping support |
robo8080 | 0:3d17b246f7fe | 198 | // use PWM for microstepping support |
robo8080 | 0:3d17b246f7fe | 199 | MC->setPWM(3, 255); |
robo8080 | 0:3d17b246f7fe | 200 | MC->setPWM(4, 255); |
robo8080 | 0:3d17b246f7fe | 201 | } |
robo8080 | 0:3d17b246f7fe | 202 | } |
robo8080 | 0:3d17b246f7fe | 203 | */ |
robo8080 | 0:3d17b246f7fe | 204 | |
robo8080 | 0:3d17b246f7fe | 205 | void Adafruit_StepperMotor::setSpeed(uint16_t rpm) { |
robo8080 | 0:3d17b246f7fe | 206 | //Serial.println("steps per rev: "); Serial.println(revsteps); |
robo8080 | 0:3d17b246f7fe | 207 | //Serial.println("RPM: "); Serial.println(rpm); |
robo8080 | 0:3d17b246f7fe | 208 | |
robo8080 | 0:3d17b246f7fe | 209 | usperstep = 60000000 / ((uint32_t)revsteps * (uint32_t)rpm); |
robo8080 | 0:3d17b246f7fe | 210 | steppingcounter = 0; |
robo8080 | 0:3d17b246f7fe | 211 | } |
robo8080 | 0:3d17b246f7fe | 212 | |
robo8080 | 0:3d17b246f7fe | 213 | void Adafruit_StepperMotor::release(void) { |
robo8080 | 0:3d17b246f7fe | 214 | MC->setPin(AIN1pin, LOW); |
robo8080 | 0:3d17b246f7fe | 215 | MC->setPin(AIN2pin, LOW); |
robo8080 | 0:3d17b246f7fe | 216 | MC->setPin(BIN1pin, LOW); |
robo8080 | 0:3d17b246f7fe | 217 | MC->setPin(BIN2pin, LOW); |
robo8080 | 0:3d17b246f7fe | 218 | MC->setPWM(PWMApin, 0); |
robo8080 | 0:3d17b246f7fe | 219 | MC->setPWM(PWMBpin, 0); |
robo8080 | 0:3d17b246f7fe | 220 | } |
robo8080 | 0:3d17b246f7fe | 221 | |
robo8080 | 0:3d17b246f7fe | 222 | void Adafruit_StepperMotor::step(uint16_t steps, uint8_t dir, uint8_t style) { |
robo8080 | 0:3d17b246f7fe | 223 | uint32_t uspers = usperstep; |
robo8080 | 0:3d17b246f7fe | 224 | uint8_t ret = 0; |
robo8080 | 0:3d17b246f7fe | 225 | |
robo8080 | 0:3d17b246f7fe | 226 | if (style == INTERLEAVE) { |
robo8080 | 0:3d17b246f7fe | 227 | uspers /= 2; |
robo8080 | 0:3d17b246f7fe | 228 | } |
robo8080 | 0:3d17b246f7fe | 229 | else if (style == MICROSTEP) { |
robo8080 | 0:3d17b246f7fe | 230 | uspers /= MICROSTEPS; |
robo8080 | 0:3d17b246f7fe | 231 | steps *= MICROSTEPS; |
robo8080 | 0:3d17b246f7fe | 232 | #ifdef MOTORDEBUG |
robo8080 | 0:3d17b246f7fe | 233 | Serial.print("steps = "); Serial.println(steps, DEC); |
robo8080 | 0:3d17b246f7fe | 234 | #endif |
robo8080 | 0:3d17b246f7fe | 235 | } |
robo8080 | 0:3d17b246f7fe | 236 | |
robo8080 | 0:3d17b246f7fe | 237 | while (steps--) { |
robo8080 | 0:3d17b246f7fe | 238 | //Serial.println("step!"); Serial.println(uspers); |
robo8080 | 0:3d17b246f7fe | 239 | ret = onestep(dir, style); |
robo8080 | 0:3d17b246f7fe | 240 | // delay(uspers/1000); // in ms |
robo8080 | 0:3d17b246f7fe | 241 | wait_ms(uspers/1000); // in ms |
robo8080 | 0:3d17b246f7fe | 242 | steppingcounter += (uspers % 1000); |
robo8080 | 0:3d17b246f7fe | 243 | if (steppingcounter >= 1000) { |
robo8080 | 0:3d17b246f7fe | 244 | // delay(1); |
robo8080 | 0:3d17b246f7fe | 245 | wait_ms(1); |
robo8080 | 0:3d17b246f7fe | 246 | steppingcounter -= 1000; |
robo8080 | 0:3d17b246f7fe | 247 | } |
robo8080 | 0:3d17b246f7fe | 248 | } |
robo8080 | 0:3d17b246f7fe | 249 | if (style == MICROSTEP) { |
robo8080 | 0:3d17b246f7fe | 250 | while ((ret != 0) && (ret != MICROSTEPS)) { |
robo8080 | 0:3d17b246f7fe | 251 | ret = onestep(dir, style); |
robo8080 | 0:3d17b246f7fe | 252 | // delay(uspers/1000); // in ms |
robo8080 | 0:3d17b246f7fe | 253 | wait_ms(uspers/1000); // in ms |
robo8080 | 0:3d17b246f7fe | 254 | steppingcounter += (uspers % 1000); |
robo8080 | 0:3d17b246f7fe | 255 | if (steppingcounter >= 1000) { |
robo8080 | 0:3d17b246f7fe | 256 | // delay(1); |
robo8080 | 0:3d17b246f7fe | 257 | wait_ms(1); |
robo8080 | 0:3d17b246f7fe | 258 | steppingcounter -= 1000; |
robo8080 | 0:3d17b246f7fe | 259 | } |
robo8080 | 0:3d17b246f7fe | 260 | } |
robo8080 | 0:3d17b246f7fe | 261 | } |
robo8080 | 0:3d17b246f7fe | 262 | } |
robo8080 | 0:3d17b246f7fe | 263 | |
robo8080 | 0:3d17b246f7fe | 264 | uint8_t Adafruit_StepperMotor::onestep(uint8_t dir, uint8_t style) { |
robo8080 | 0:3d17b246f7fe | 265 | uint8_t a, b, c, d; |
robo8080 | 0:3d17b246f7fe | 266 | uint8_t ocrb, ocra; |
robo8080 | 0:3d17b246f7fe | 267 | |
robo8080 | 0:3d17b246f7fe | 268 | ocra = ocrb = 255; |
robo8080 | 0:3d17b246f7fe | 269 | |
robo8080 | 0:3d17b246f7fe | 270 | |
robo8080 | 0:3d17b246f7fe | 271 | // next determine what sort of stepping procedure we're up to |
robo8080 | 0:3d17b246f7fe | 272 | if (style == SINGLE) { |
robo8080 | 0:3d17b246f7fe | 273 | if ((currentstep/(MICROSTEPS/2)) % 2) { // we're at an odd step, weird |
robo8080 | 0:3d17b246f7fe | 274 | if (dir == FORWARD) { |
robo8080 | 0:3d17b246f7fe | 275 | currentstep += MICROSTEPS/2; |
robo8080 | 0:3d17b246f7fe | 276 | } |
robo8080 | 0:3d17b246f7fe | 277 | else { |
robo8080 | 0:3d17b246f7fe | 278 | currentstep -= MICROSTEPS/2; |
robo8080 | 0:3d17b246f7fe | 279 | } |
robo8080 | 0:3d17b246f7fe | 280 | } else { // go to the next even step |
robo8080 | 0:3d17b246f7fe | 281 | if (dir == FORWARD) { |
robo8080 | 0:3d17b246f7fe | 282 | currentstep += MICROSTEPS; |
robo8080 | 0:3d17b246f7fe | 283 | } |
robo8080 | 0:3d17b246f7fe | 284 | else { |
robo8080 | 0:3d17b246f7fe | 285 | currentstep -= MICROSTEPS; |
robo8080 | 0:3d17b246f7fe | 286 | } |
robo8080 | 0:3d17b246f7fe | 287 | } |
robo8080 | 0:3d17b246f7fe | 288 | } else if (style == DOUBLE) { |
robo8080 | 0:3d17b246f7fe | 289 | if (! (currentstep/(MICROSTEPS/2) % 2)) { // we're at an even step, weird |
robo8080 | 0:3d17b246f7fe | 290 | if (dir == FORWARD) { |
robo8080 | 0:3d17b246f7fe | 291 | currentstep += MICROSTEPS/2; |
robo8080 | 0:3d17b246f7fe | 292 | } else { |
robo8080 | 0:3d17b246f7fe | 293 | currentstep -= MICROSTEPS/2; |
robo8080 | 0:3d17b246f7fe | 294 | } |
robo8080 | 0:3d17b246f7fe | 295 | } else { // go to the next odd step |
robo8080 | 0:3d17b246f7fe | 296 | if (dir == FORWARD) { |
robo8080 | 0:3d17b246f7fe | 297 | currentstep += MICROSTEPS; |
robo8080 | 0:3d17b246f7fe | 298 | } else { |
robo8080 | 0:3d17b246f7fe | 299 | currentstep -= MICROSTEPS; |
robo8080 | 0:3d17b246f7fe | 300 | } |
robo8080 | 0:3d17b246f7fe | 301 | } |
robo8080 | 0:3d17b246f7fe | 302 | } else if (style == INTERLEAVE) { |
robo8080 | 0:3d17b246f7fe | 303 | if (dir == FORWARD) { |
robo8080 | 0:3d17b246f7fe | 304 | currentstep += MICROSTEPS/2; |
robo8080 | 0:3d17b246f7fe | 305 | } else { |
robo8080 | 0:3d17b246f7fe | 306 | currentstep -= MICROSTEPS/2; |
robo8080 | 0:3d17b246f7fe | 307 | } |
robo8080 | 0:3d17b246f7fe | 308 | } |
robo8080 | 0:3d17b246f7fe | 309 | |
robo8080 | 0:3d17b246f7fe | 310 | if (style == MICROSTEP) { |
robo8080 | 0:3d17b246f7fe | 311 | if (dir == FORWARD) { |
robo8080 | 0:3d17b246f7fe | 312 | currentstep++; |
robo8080 | 0:3d17b246f7fe | 313 | } else { |
robo8080 | 0:3d17b246f7fe | 314 | // BACKWARDS |
robo8080 | 0:3d17b246f7fe | 315 | currentstep--; |
robo8080 | 0:3d17b246f7fe | 316 | } |
robo8080 | 0:3d17b246f7fe | 317 | |
robo8080 | 0:3d17b246f7fe | 318 | currentstep += MICROSTEPS*4; |
robo8080 | 0:3d17b246f7fe | 319 | currentstep %= MICROSTEPS*4; |
robo8080 | 0:3d17b246f7fe | 320 | |
robo8080 | 0:3d17b246f7fe | 321 | ocra = ocrb = 0; |
robo8080 | 0:3d17b246f7fe | 322 | if ( (currentstep >= 0) && (currentstep < MICROSTEPS)) { |
robo8080 | 0:3d17b246f7fe | 323 | ocra = microstepcurve[MICROSTEPS - currentstep]; |
robo8080 | 0:3d17b246f7fe | 324 | ocrb = microstepcurve[currentstep]; |
robo8080 | 0:3d17b246f7fe | 325 | } else if ( (currentstep >= MICROSTEPS) && (currentstep < MICROSTEPS*2)) { |
robo8080 | 0:3d17b246f7fe | 326 | ocra = microstepcurve[currentstep - MICROSTEPS]; |
robo8080 | 0:3d17b246f7fe | 327 | ocrb = microstepcurve[MICROSTEPS*2 - currentstep]; |
robo8080 | 0:3d17b246f7fe | 328 | } else if ( (currentstep >= MICROSTEPS*2) && (currentstep < MICROSTEPS*3)) { |
robo8080 | 0:3d17b246f7fe | 329 | ocra = microstepcurve[MICROSTEPS*3 - currentstep]; |
robo8080 | 0:3d17b246f7fe | 330 | ocrb = microstepcurve[currentstep - MICROSTEPS*2]; |
robo8080 | 0:3d17b246f7fe | 331 | } else if ( (currentstep >= MICROSTEPS*3) && (currentstep < MICROSTEPS*4)) { |
robo8080 | 0:3d17b246f7fe | 332 | ocra = microstepcurve[currentstep - MICROSTEPS*3]; |
robo8080 | 0:3d17b246f7fe | 333 | ocrb = microstepcurve[MICROSTEPS*4 - currentstep]; |
robo8080 | 0:3d17b246f7fe | 334 | } |
robo8080 | 0:3d17b246f7fe | 335 | } |
robo8080 | 0:3d17b246f7fe | 336 | |
robo8080 | 0:3d17b246f7fe | 337 | currentstep += MICROSTEPS*4; |
robo8080 | 0:3d17b246f7fe | 338 | currentstep %= MICROSTEPS*4; |
robo8080 | 0:3d17b246f7fe | 339 | |
robo8080 | 0:3d17b246f7fe | 340 | #ifdef MOTORDEBUG |
robo8080 | 0:3d17b246f7fe | 341 | Serial.print("current step: "); Serial.println(currentstep, DEC); |
robo8080 | 0:3d17b246f7fe | 342 | Serial.print(" pwmA = "); Serial.print(ocra, DEC); |
robo8080 | 0:3d17b246f7fe | 343 | Serial.print(" pwmB = "); Serial.println(ocrb, DEC); |
robo8080 | 0:3d17b246f7fe | 344 | #endif |
robo8080 | 0:3d17b246f7fe | 345 | MC->setPWM(PWMApin, ocra*16); |
robo8080 | 0:3d17b246f7fe | 346 | MC->setPWM(PWMBpin, ocrb*16); |
robo8080 | 0:3d17b246f7fe | 347 | |
robo8080 | 0:3d17b246f7fe | 348 | |
robo8080 | 0:3d17b246f7fe | 349 | // release all |
robo8080 | 0:3d17b246f7fe | 350 | uint8_t latch_state = 0; // all motor pins to 0 |
robo8080 | 0:3d17b246f7fe | 351 | |
robo8080 | 0:3d17b246f7fe | 352 | //Serial.println(step, DEC); |
robo8080 | 0:3d17b246f7fe | 353 | if (style == MICROSTEP) { |
robo8080 | 0:3d17b246f7fe | 354 | if ((currentstep >= 0) && (currentstep < MICROSTEPS)) |
robo8080 | 0:3d17b246f7fe | 355 | latch_state |= 0x03; |
robo8080 | 0:3d17b246f7fe | 356 | if ((currentstep >= MICROSTEPS) && (currentstep < MICROSTEPS*2)) |
robo8080 | 0:3d17b246f7fe | 357 | latch_state |= 0x06; |
robo8080 | 0:3d17b246f7fe | 358 | if ((currentstep >= MICROSTEPS*2) && (currentstep < MICROSTEPS*3)) |
robo8080 | 0:3d17b246f7fe | 359 | latch_state |= 0x0C; |
robo8080 | 0:3d17b246f7fe | 360 | if ((currentstep >= MICROSTEPS*3) && (currentstep < MICROSTEPS*4)) |
robo8080 | 0:3d17b246f7fe | 361 | latch_state |= 0x09; |
robo8080 | 0:3d17b246f7fe | 362 | } else { |
robo8080 | 0:3d17b246f7fe | 363 | switch (currentstep/(MICROSTEPS/2)) { |
robo8080 | 0:3d17b246f7fe | 364 | case 0: |
robo8080 | 0:3d17b246f7fe | 365 | latch_state |= 0x1; // energize coil 1 only |
robo8080 | 0:3d17b246f7fe | 366 | break; |
robo8080 | 0:3d17b246f7fe | 367 | case 1: |
robo8080 | 0:3d17b246f7fe | 368 | latch_state |= 0x3; // energize coil 1+2 |
robo8080 | 0:3d17b246f7fe | 369 | break; |
robo8080 | 0:3d17b246f7fe | 370 | case 2: |
robo8080 | 0:3d17b246f7fe | 371 | latch_state |= 0x2; // energize coil 2 only |
robo8080 | 0:3d17b246f7fe | 372 | break; |
robo8080 | 0:3d17b246f7fe | 373 | case 3: |
robo8080 | 0:3d17b246f7fe | 374 | latch_state |= 0x6; // energize coil 2+3 |
robo8080 | 0:3d17b246f7fe | 375 | break; |
robo8080 | 0:3d17b246f7fe | 376 | case 4: |
robo8080 | 0:3d17b246f7fe | 377 | latch_state |= 0x4; // energize coil 3 only |
robo8080 | 0:3d17b246f7fe | 378 | break; |
robo8080 | 0:3d17b246f7fe | 379 | case 5: |
robo8080 | 0:3d17b246f7fe | 380 | latch_state |= 0xC; // energize coil 3+4 |
robo8080 | 0:3d17b246f7fe | 381 | break; |
robo8080 | 0:3d17b246f7fe | 382 | case 6: |
robo8080 | 0:3d17b246f7fe | 383 | latch_state |= 0x8; // energize coil 4 only |
robo8080 | 0:3d17b246f7fe | 384 | break; |
robo8080 | 0:3d17b246f7fe | 385 | case 7: |
robo8080 | 0:3d17b246f7fe | 386 | latch_state |= 0x9; // energize coil 1+4 |
robo8080 | 0:3d17b246f7fe | 387 | break; |
robo8080 | 0:3d17b246f7fe | 388 | } |
robo8080 | 0:3d17b246f7fe | 389 | } |
robo8080 | 0:3d17b246f7fe | 390 | #ifdef MOTORDEBUG |
robo8080 | 0:3d17b246f7fe | 391 | Serial.print("Latch: 0x"); Serial.println(latch_state, HEX); |
robo8080 | 0:3d17b246f7fe | 392 | #endif |
robo8080 | 0:3d17b246f7fe | 393 | |
robo8080 | 0:3d17b246f7fe | 394 | if (latch_state & 0x1) { |
robo8080 | 0:3d17b246f7fe | 395 | // Serial.println(AIN2pin); |
robo8080 | 0:3d17b246f7fe | 396 | MC->setPin(AIN2pin, HIGH); |
robo8080 | 0:3d17b246f7fe | 397 | } else { |
robo8080 | 0:3d17b246f7fe | 398 | MC->setPin(AIN2pin, LOW); |
robo8080 | 0:3d17b246f7fe | 399 | } |
robo8080 | 0:3d17b246f7fe | 400 | if (latch_state & 0x2) { |
robo8080 | 0:3d17b246f7fe | 401 | MC->setPin(BIN1pin, HIGH); |
robo8080 | 0:3d17b246f7fe | 402 | // Serial.println(BIN1pin); |
robo8080 | 0:3d17b246f7fe | 403 | } else { |
robo8080 | 0:3d17b246f7fe | 404 | MC->setPin(BIN1pin, LOW); |
robo8080 | 0:3d17b246f7fe | 405 | } |
robo8080 | 0:3d17b246f7fe | 406 | if (latch_state & 0x4) { |
robo8080 | 0:3d17b246f7fe | 407 | MC->setPin(AIN1pin, HIGH); |
robo8080 | 0:3d17b246f7fe | 408 | // Serial.println(AIN1pin); |
robo8080 | 0:3d17b246f7fe | 409 | } else { |
robo8080 | 0:3d17b246f7fe | 410 | MC->setPin(AIN1pin, LOW); |
robo8080 | 0:3d17b246f7fe | 411 | } |
robo8080 | 0:3d17b246f7fe | 412 | if (latch_state & 0x8) { |
robo8080 | 0:3d17b246f7fe | 413 | MC->setPin(BIN2pin, HIGH); |
robo8080 | 0:3d17b246f7fe | 414 | // Serial.println(BIN2pin); |
robo8080 | 0:3d17b246f7fe | 415 | } else { |
robo8080 | 0:3d17b246f7fe | 416 | MC->setPin(BIN2pin, LOW); |
robo8080 | 0:3d17b246f7fe | 417 | } |
robo8080 | 0:3d17b246f7fe | 418 | |
robo8080 | 0:3d17b246f7fe | 419 | return currentstep; |
robo8080 | 0:3d17b246f7fe | 420 | } |
robo8080 | 0:3d17b246f7fe | 421 |