robo 8080 / Adafruit-MotorShield

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?

UserRevisionLine numberNew 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