Revision 0:3d17b246f7fe, committed 2014-10-15

Comitter:

robo8080

Date:

Wed Oct 15 02:16:15 2014 +0000

Commit message:

test1

Changed in this revision

diff -r 000000000000 -r 3d17b246f7fe Adafruit_MotorShield.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Adafruit_MotorShield.cpp	Wed Oct 15 02:16:15 2014 +0000
@@ -0,0 +1,421 @@
+/******************************************************************
+ This is the library for the Adafruit Motor Shield V2 for Arduino. 
+ It supports DC motors & Stepper motors with microstepping as well
+ as stacking-support. It is *not* compatible with the V1 library!
+
+ It will only work with https://www.adafruit.com/products/1483
+ 
+ Adafruit invests time and resources providing this open
+ source code, please support Adafruit and open-source hardware
+ by purchasing products from Adafruit!
+ 
+ Written by Limor Fried/Ladyada for Adafruit Industries.
+ BSD license, check license.txt for more information.
+ All text above must be included in any redistribution.
+ ******************************************************************/
+
+
+//#if (ARDUINO >= 100)
+// #include "Arduino.h"
+//#else
+// #include "WProgram.h"
+//#endif
+//#include <Wire.h>
+#include "Adafruit_MotorShield.h"
+#include <Adafruit_PWMServoDriver.h>
+//#ifdef __AVR__
+// #define WIRE Wire
+//#else // Arduino Due
+// #define WIRE Wire1
+//#endif
+
+#define LOW 0
+#define HIGH 1
+
+#if (MICROSTEPS == 8)
+uint8_t microstepcurve[] = {0, 50, 98, 142, 180, 212, 236, 250, 255};
+#elif (MICROSTEPS == 16)
+uint8_t microstepcurve[] = {0, 25, 50, 74, 98, 120, 141, 162, 180, 197, 212, 225, 236, 244, 250, 253, 255};
+#endif
+
+//Adafruit_MotorShield::Adafruit_MotorShield(PinName sda, PinName scl, uint8_t addr) {
+Adafruit_MotorShield::Adafruit_MotorShield(PinName sda, PinName scl, uint8_t addr):_pwm(sda, scl, addr) {
+  _addr = addr;
+//  _pwm = Adafruit_PWMServoDriver(sda, scl, _addr);
+}
+
+void Adafruit_MotorShield::begin(uint16_t freq) {
+  // init PWM w/_freq
+//  WIRE.begin();
+  _pwm.begin();
+  _freq = freq;
+  _pwm.setPWMFreq(_freq);  // This is the maximum PWM frequency
+  for (uint8_t i=0; i<16; i++) 
+    _pwm.setPWM(i, 0, 0);
+}
+
+void Adafruit_MotorShield::setPWM(uint8_t pin, uint16_t value) {
+  if (value > 4095) {
+    _pwm.setPWM(pin, 4096, 0);
+  } else 
+    _pwm.setPWM(pin, 0, value);
+}
+//void Adafruit_MotorShield::setPin(uint8_t pin, bool value) {
+void Adafruit_MotorShield::setPin(uint8_t pin, uint16_t value) {
+  if (value == LOW)
+    _pwm.setPWM(pin, 0, 0);
+  else
+    _pwm.setPWM(pin, 4096, 0);
+}
+
+Adafruit_DCMotor *Adafruit_MotorShield::getMotor(uint8_t num) {
+  if (num > 4) return NULL;
+
+  num--;
+
+  if (dcmotors[num].motornum == 0) {
+    // not init'd yet!
+    dcmotors[num].motornum = num;
+    dcmotors[num].MC = this;
+    uint8_t pwm, in1, in2;
+    if (num == 0) {
+      pwm = 8; in2 = 9; in1 = 10;
+    } else if (num == 1) {
+      pwm = 13; in2 = 12; in1 = 11;
+    } else if (num == 2) {
+      pwm = 2; in2 = 3; in1 = 4;
+    } else if (num == 3) {
+      pwm = 7; in2 = 6; in1 = 5;
+    }
+    dcmotors[num].PWMpin = pwm;
+    dcmotors[num].IN1pin = in1;
+    dcmotors[num].IN2pin = in2;
+  }
+  return &dcmotors[num];
+}
+
+
+Adafruit_StepperMotor *Adafruit_MotorShield::getStepper(uint16_t steps, uint8_t num) {
+  if (num > 2) return NULL;
+
+  num--;
+
+  if (steppers[num].steppernum == 0) {
+    // not init'd yet!
+    steppers[num].steppernum = num;
+    steppers[num].revsteps = steps;
+    steppers[num].MC = this;
+    uint8_t pwma, pwmb, ain1, ain2, bin1, bin2;
+    if (num == 0) {
+      pwma = 8; ain2 = 9; ain1 = 10;
+      pwmb = 13; bin2 = 12; bin1 = 11;
+    } else if (num == 1) {
+      pwma = 2; ain2 = 3; ain1 = 4;
+      pwmb = 7; bin2 = 6; bin1 = 5;
+    }
+    steppers[num].PWMApin = pwma;
+    steppers[num].PWMBpin = pwmb;
+    steppers[num].AIN1pin = ain1;
+    steppers[num].AIN2pin = ain2;
+    steppers[num].BIN1pin = bin1;
+    steppers[num].BIN2pin = bin2;
+  }
+  return &steppers[num];
+}
+
+
+/******************************************
+               MOTORS
+******************************************/
+
+Adafruit_DCMotor::Adafruit_DCMotor(void) {
+  MC = NULL;
+  motornum = 0;
+  PWMpin = IN1pin = IN2pin = 0;
+}
+
+void Adafruit_DCMotor::run(uint8_t cmd) {
+  switch (cmd) {
+  case FORWARD:
+    MC->setPin(IN2pin, LOW);  // take low first to avoid 'break'
+    MC->setPin(IN1pin, HIGH);
+    break;
+  case BACKWARD:
+    MC->setPin(IN1pin, LOW);  // take low first to avoid 'break'
+    MC->setPin(IN2pin, HIGH);
+    break;
+  case RELEASE:
+    MC->setPin(IN1pin, LOW);
+    MC->setPin(IN2pin, LOW);
+    break;
+  }
+}
+
+void Adafruit_DCMotor::setSpeed(uint8_t speed) {
+  MC->setPWM(PWMpin, speed*16);
+}
+
+/******************************************
+               STEPPERS
+******************************************/
+
+Adafruit_StepperMotor::Adafruit_StepperMotor(void) {
+  revsteps = steppernum = currentstep = 0;
+}
+/*
+
+uint16_t steps, Adafruit_MotorShield controller)  {
+
+  revsteps = steps;
+  steppernum = 1;
+  currentstep = 0;
+
+  if (steppernum == 1) {
+    latch_state &= ~_BV(MOTOR1_A) & ~_BV(MOTOR1_B) &
+      ~_BV(MOTOR2_A) & ~_BV(MOTOR2_B); // all motor pins to 0
+    
+    // enable both H bridges
+    pinMode(11, OUTPUT);
+    pinMode(3, OUTPUT);
+    digitalWrite(11, HIGH);
+    digitalWrite(3, HIGH);
+
+    // use PWM for microstepping support
+    MC->setPWM(1, 255);
+    MC->setPWM(2, 255);
+
+  } else if (steppernum == 2) {
+    latch_state &= ~_BV(MOTOR3_A) & ~_BV(MOTOR3_B) &
+      ~_BV(MOTOR4_A) & ~_BV(MOTOR4_B); // all motor pins to 0
+
+    // enable both H bridges
+    pinMode(5, OUTPUT);
+    pinMode(6, OUTPUT);
+    digitalWrite(5, HIGH);
+    digitalWrite(6, HIGH);
+
+    // use PWM for microstepping support
+    // use PWM for microstepping support
+    MC->setPWM(3, 255);
+    MC->setPWM(4, 255);
+  }
+}
+*/
+
+void Adafruit_StepperMotor::setSpeed(uint16_t rpm) {
+  //Serial.println("steps per rev: "); Serial.println(revsteps);
+  //Serial.println("RPM: "); Serial.println(rpm);
+
+  usperstep = 60000000 / ((uint32_t)revsteps * (uint32_t)rpm);
+  steppingcounter = 0;
+}
+
+void Adafruit_StepperMotor::release(void) {
+  MC->setPin(AIN1pin, LOW);
+  MC->setPin(AIN2pin, LOW);
+  MC->setPin(BIN1pin, LOW);
+  MC->setPin(BIN2pin, LOW);
+  MC->setPWM(PWMApin, 0);
+  MC->setPWM(PWMBpin, 0);
+}
+
+void Adafruit_StepperMotor::step(uint16_t steps, uint8_t dir,  uint8_t style) {
+  uint32_t uspers = usperstep;
+  uint8_t ret = 0;
+
+  if (style == INTERLEAVE) {
+    uspers /= 2;
+  }
+ else if (style == MICROSTEP) {
+    uspers /= MICROSTEPS;
+    steps *= MICROSTEPS;
+#ifdef MOTORDEBUG
+    Serial.print("steps = "); Serial.println(steps, DEC);
+#endif
+  }
+
+  while (steps--) {
+    //Serial.println("step!"); Serial.println(uspers);
+    ret = onestep(dir, style);
+//    delay(uspers/1000); // in ms
+    wait_ms(uspers/1000); // in ms
+    steppingcounter += (uspers % 1000);
+    if (steppingcounter >= 1000) {
+//      delay(1);
+      wait_ms(1);
+      steppingcounter -= 1000;
+    }
+  }
+  if (style == MICROSTEP) {
+    while ((ret != 0) && (ret != MICROSTEPS)) {
+      ret = onestep(dir, style);
+//      delay(uspers/1000); // in ms
+      wait_ms(uspers/1000); // in ms
+      steppingcounter += (uspers % 1000);
+      if (steppingcounter >= 1000) {
+//	delay(1);
+	wait_ms(1);
+	steppingcounter -= 1000;
+      } 
+    }
+  }
+}
+
+uint8_t Adafruit_StepperMotor::onestep(uint8_t dir, uint8_t style) {
+  uint8_t a, b, c, d;
+  uint8_t ocrb, ocra;
+
+  ocra = ocrb = 255;
+
+
+  // next determine what sort of stepping procedure we're up to
+  if (style == SINGLE) {
+    if ((currentstep/(MICROSTEPS/2)) % 2) { // we're at an odd step, weird
+      if (dir == FORWARD) {
+	currentstep += MICROSTEPS/2;
+      }
+      else {
+	currentstep -= MICROSTEPS/2;
+      }
+    } else {           // go to the next even step
+      if (dir == FORWARD) {
+	currentstep += MICROSTEPS;
+      }
+      else {
+	currentstep -= MICROSTEPS;
+      }
+    }
+  } else if (style == DOUBLE) {
+    if (! (currentstep/(MICROSTEPS/2) % 2)) { // we're at an even step, weird
+      if (dir == FORWARD) {
+	currentstep += MICROSTEPS/2;
+      } else {
+	currentstep -= MICROSTEPS/2;
+      }
+    } else {           // go to the next odd step
+      if (dir == FORWARD) {
+	currentstep += MICROSTEPS;
+      } else {
+	currentstep -= MICROSTEPS;
+      }
+    }
+  } else if (style == INTERLEAVE) {
+    if (dir == FORWARD) {
+       currentstep += MICROSTEPS/2;
+    } else {
+       currentstep -= MICROSTEPS/2;
+    }
+  } 
+
+  if (style == MICROSTEP) {
+    if (dir == FORWARD) {
+      currentstep++;
+    } else {
+      // BACKWARDS
+      currentstep--;
+    }
+
+    currentstep += MICROSTEPS*4;
+    currentstep %= MICROSTEPS*4;
+
+    ocra = ocrb = 0;
+    if ( (currentstep >= 0) && (currentstep < MICROSTEPS)) {
+      ocra = microstepcurve[MICROSTEPS - currentstep];
+      ocrb = microstepcurve[currentstep];
+    } else if  ( (currentstep >= MICROSTEPS) && (currentstep < MICROSTEPS*2)) {
+      ocra = microstepcurve[currentstep - MICROSTEPS];
+      ocrb = microstepcurve[MICROSTEPS*2 - currentstep];
+    } else if  ( (currentstep >= MICROSTEPS*2) && (currentstep < MICROSTEPS*3)) {
+      ocra = microstepcurve[MICROSTEPS*3 - currentstep];
+      ocrb = microstepcurve[currentstep - MICROSTEPS*2];
+    } else if  ( (currentstep >= MICROSTEPS*3) && (currentstep < MICROSTEPS*4)) {
+      ocra = microstepcurve[currentstep - MICROSTEPS*3];
+      ocrb = microstepcurve[MICROSTEPS*4 - currentstep];
+    }
+  }
+
+  currentstep += MICROSTEPS*4;
+  currentstep %= MICROSTEPS*4;
+
+#ifdef MOTORDEBUG
+  Serial.print("current step: "); Serial.println(currentstep, DEC);
+  Serial.print(" pwmA = "); Serial.print(ocra, DEC); 
+  Serial.print(" pwmB = "); Serial.println(ocrb, DEC); 
+#endif
+  MC->setPWM(PWMApin, ocra*16);
+  MC->setPWM(PWMBpin, ocrb*16);
+  
+
+  // release all
+  uint8_t latch_state = 0; // all motor pins to 0
+
+  //Serial.println(step, DEC);
+  if (style == MICROSTEP) {
+    if ((currentstep >= 0) && (currentstep < MICROSTEPS))
+      latch_state |= 0x03;
+    if ((currentstep >= MICROSTEPS) && (currentstep < MICROSTEPS*2))
+      latch_state |= 0x06;
+    if ((currentstep >= MICROSTEPS*2) && (currentstep < MICROSTEPS*3))
+      latch_state |= 0x0C;
+    if ((currentstep >= MICROSTEPS*3) && (currentstep < MICROSTEPS*4))
+      latch_state |= 0x09;
+  } else {
+    switch (currentstep/(MICROSTEPS/2)) {
+    case 0:
+      latch_state |= 0x1; // energize coil 1 only
+      break;
+    case 1:
+      latch_state |= 0x3; // energize coil 1+2
+      break;
+    case 2:
+      latch_state |= 0x2; // energize coil 2 only
+      break;
+    case 3:
+      latch_state |= 0x6; // energize coil 2+3
+      break;
+    case 4:
+      latch_state |= 0x4; // energize coil 3 only
+      break; 
+    case 5:
+      latch_state |= 0xC; // energize coil 3+4
+      break;
+    case 6:
+      latch_state |= 0x8; // energize coil 4 only
+      break;
+    case 7:
+      latch_state |= 0x9; // energize coil 1+4
+      break;
+    }
+  }
+#ifdef MOTORDEBUG
+  Serial.print("Latch: 0x"); Serial.println(latch_state, HEX);
+#endif
+
+  if (latch_state & 0x1) {
+   // Serial.println(AIN2pin);
+    MC->setPin(AIN2pin, HIGH);
+  } else {
+    MC->setPin(AIN2pin, LOW);
+  }
+  if (latch_state & 0x2) {
+    MC->setPin(BIN1pin, HIGH);
+   // Serial.println(BIN1pin);
+  } else {
+    MC->setPin(BIN1pin, LOW);
+  }
+  if (latch_state & 0x4) {
+    MC->setPin(AIN1pin, HIGH);
+   // Serial.println(AIN1pin);
+  } else {
+    MC->setPin(AIN1pin, LOW);
+  }
+  if (latch_state & 0x8) {
+    MC->setPin(BIN2pin, HIGH);
+   // Serial.println(BIN2pin);
+  } else {
+    MC->setPin(BIN2pin, LOW);
+  }
+
+  return currentstep;
+}
+

diff -r 000000000000 -r 3d17b246f7fe Adafruit_MotorShield.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Adafruit_MotorShield.h	Wed Oct 15 02:16:15 2014 +0000
@@ -0,0 +1,105 @@
+/******************************************************************
+ This is the library for the Adafruit Motor Shield V2 for Arduino. 
+ It supports DC motors & Stepper motors with microstepping as well
+ as stacking-support. It is *not* compatible with the V1 library!
+
+ It will only work with https://www.adafruit.com/products/1483
+ 
+ Adafruit invests time and resources providing this open
+ source code, please support Adafruit and open-source hardware
+ by purchasing products from Adafruit!
+ 
+ Written by Limor Fried/Ladyada for Adafruit Industries.
+ BSD license, check license.txt for more information.
+ All text above must be included in any redistribution.
+ ******************************************************************/
+
+#ifndef _Adafruit_MotorShield_h_
+#define _Adafruit_MotorShield_h_
+
+//#include <inttypes.h>
+//#include <Wire.h>
+//#include "utility/Adafruit_PWMServoDriver.h"
+#include "Adafruit_PWMServoDriver.h"
+
+//#define MOTORDEBUG
+
+#define MICROSTEPS 16         // 8 or 16
+
+#define MOTOR1_A 2
+#define MOTOR1_B 3
+#define MOTOR2_A 1
+#define MOTOR2_B 4
+#define MOTOR4_A 0
+#define MOTOR4_B 6
+#define MOTOR3_A 5
+#define MOTOR3_B 7
+
+#define FORWARD 1
+#define BACKWARD 2
+#define BRAKE 3
+#define RELEASE 4
+
+#define SINGLE 1
+#define DOUBLE 2
+#define INTERLEAVE 3
+#define MICROSTEP 4
+
+class Adafruit_MotorShield;
+
+class Adafruit_DCMotor
+{
+ public:
+  Adafruit_DCMotor(void);
+  friend class Adafruit_MotorShield;
+  void run(uint8_t);
+  void setSpeed(uint8_t);
+  
+ private:
+  uint8_t PWMpin, IN1pin, IN2pin;
+  Adafruit_MotorShield *MC;
+  uint8_t motornum;
+};
+
+class Adafruit_StepperMotor {
+ public:
+  Adafruit_StepperMotor(void);
+  friend class Adafruit_MotorShield;
+
+  void step(uint16_t steps, uint8_t dir,  uint8_t style = SINGLE);
+  void setSpeed(uint16_t);
+  uint8_t onestep(uint8_t dir, uint8_t style);
+  void release(void);
+  uint32_t usperstep, steppingcounter;
+
+ private:
+  uint8_t PWMApin, AIN1pin, AIN2pin;
+  uint8_t PWMBpin, BIN1pin, BIN2pin;
+  uint16_t revsteps; // # steps per revolution
+  uint8_t currentstep;
+  Adafruit_MotorShield *MC;
+  uint8_t steppernum;
+};
+
+class Adafruit_MotorShield
+{
+  public:
+//    Adafruit_MotorShield(uint8_t addr = 0x60);
+    Adafruit_MotorShield(PinName sda, PinName scl, uint8_t addr = 0x60 << 1);
+    friend class Adafruit_DCMotor;
+    void begin(uint16_t freq = 1600);
+
+    void setPWM(uint8_t pin, uint16_t val);
+//    void setPin(uint8_t pin, boolean val);
+    void setPin(uint8_t pin, uint16_t val);
+    Adafruit_DCMotor *getMotor(uint8_t n);
+    Adafruit_StepperMotor *getStepper(uint16_t steps, uint8_t n);
+ private:
+    uint8_t _addr;
+    uint16_t _freq;
+    Adafruit_DCMotor dcmotors[4];
+    Adafruit_StepperMotor steppers[2];
+    Adafruit_PWMServoDriver _pwm;
+};
+
+#endif