Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with
or
.
Download ZIP
Tree: 5c39b93fad
Fetching contributors…

Cannot retrieve contributors at this time

285 lines (252 sloc) 7.199 kB
// Snootlab Max 7313 Motor shield library
// Based on Adafruit Motor shield library
// https://github.com/adafruit/Adafruit-Motor-Shield-library
// copyleft Snootlab, 2011
// this code is public domain, enjoy!
#include "snootor_dc.h"
#if defined(ARDUINO) && ARDUINO >= 100
#include "Arduino.h"
#else
#include <avr/io.h>
#include "WProgram.h"
#endif
/**
* binary mask for motor control
*
* each array contains values for {M1,M2,M3,M4}
*
**/
static uint8_t mask_forw[]={0xFE,0xFB,0xEF,0xBF}; // bit-by-bit "and" operation,
static uint8_t mask_back[]={0xFD,0xF7,0xDF,0x7F}; // bit-by-bit "and" operation,
static uint8_t mask_rel[] ={0x03,0x0C,0x30,0xC0}; // bit-by-bit "or" operation,
/******************************************
*
* PWM routines
*
******************************************/
inline void initPWM1(uint8_t freq) {
#if defined(__AVR_ATmega8__) || \
defined(__AVR_ATmega48__) || \
defined(__AVR_ATmega88__) || \
defined(__AVR_ATmega168__) || \
defined(__AVR_ATmega328P__)
// use PWM from timer2A on PB3 (Arduino pin #11)
TCCR2A |= _BV(COM2A1) | _BV(WGM20) | _BV(WGM21); // fast PWM, turn on oc2a
// TCCR2B = freq & 0x7;
OCR2A = 0;
#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
// on arduino mega, pin 11 is now PB5 (OC1A)
TCCR1A |= _BV(COM1A1) | _BV(WGM10); // fast PWM, turn on oc1a
TCCR1B = (freq & 0x7) | _BV(WGM12);
OCR1A = 0;
#else
#error "This chip is not supported!"
#endif
pinMode(11, OUTPUT);
}
inline void setPWM1(uint8_t s) {
#if defined(__AVR_ATmega8__) || \
defined(__AVR_ATmega48__) || \
defined(__AVR_ATmega88__) || \
defined(__AVR_ATmega168__) || \
defined(__AVR_ATmega328P__)
// use PWM from timer2A on PB3 (Arduino pin #11)
OCR2B = s;
#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
// on arduino mega, pin 11 is now PB5 (OC1A)
OCR3C = s;
#else
#error "This chip is not supported!"
#endif
}
inline void initPWM2(uint8_t freq) {
#if defined(__AVR_ATmega8__) || \
defined(__AVR_ATmega48__) || \
defined(__AVR_ATmega88__) || \
defined(__AVR_ATmega168__) || \
defined(__AVR_ATmega328P__)
// use PWM from timer2B (pin 3)
TCCR2A |= _BV(COM2B1) | _BV(WGM20) | _BV(WGM21); // fast PWM, turn on oc2b
// TCCR2B = freq & 0x7;
OCR2B = 0;
#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
// on arduino mega, pin 3 is now PE5 (OC3C)
TCCR3A |= _BV(COM1C1) | _BV(WGM10); // fast PWM, turn on oc3c
TCCR3B = (freq & 0x7) | _BV(WGM12);
OCR3C = 0;
#else
#error "This chip is not supported!"
#endif
pinMode(3, OUTPUT);
}
inline void setPWM2(uint8_t s) {
#if defined(__AVR_ATmega8__) || \
defined(__AVR_ATmega48__) || \
defined(__AVR_ATmega88__) || \
defined(__AVR_ATmega168__) || \
defined(__AVR_ATmega328P__)
// use PWM from timer2A on PB3 (Arduino pin #11)
OCR2A = s;
#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
// on arduino mega, pin 11 is now PB5 (OC1A)
OCR1A = s;
#else
#error "This chip is not supported!"
#endif
}
inline void initPWM3(uint8_t freq) {
#if defined(__AVR_ATmega8__) || \
defined(__AVR_ATmega48__) || \
defined(__AVR_ATmega88__) || \
defined(__AVR_ATmega168__) || \
defined(__AVR_ATmega328P__)
// use PWM from timer0B / PD5 (pin 5)
TCCR0A |= _BV(COM0B1) | _BV(WGM00) | _BV(WGM01); // fast PWM, turn on oc0a
//TCCR0B = freq & 0x7;
OCR0B = 0;
#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
// on arduino mega, pin 5 is now PE3 (OC3A)
TCCR3A |= _BV(COM1A1) | _BV(WGM10); // fast PWM, turn on oc3a
TCCR3B = (freq & 0x7) | _BV(WGM12);
//TCCR4B = 1 | _BV(WGM12);
OCR3A = 0;
#else
#error "This chip is not supported!"
#endif
pinMode(5, OUTPUT);
}
inline void setPWM3(uint8_t s) {
#if defined(__AVR_ATmega8__) || \
defined(__AVR_ATmega48__) || \
defined(__AVR_ATmega88__) || \
defined(__AVR_ATmega168__) || \
defined(__AVR_ATmega328P__)
// use PWM from timer0A on PB3 (Arduino pin #6)
OCR0B = s;
#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
// on arduino mega, pin 6 is now PH3 (OC4A)
OCR3A = s;
#else
#error "This chip is not supported!"
#endif
}
inline void initPWM4(uint8_t freq) {
#if defined(__AVR_ATmega8__) || \
defined(__AVR_ATmega48__) || \
defined(__AVR_ATmega88__) || \
defined(__AVR_ATmega168__) || \
defined(__AVR_ATmega328P__)
// use PWM from timer0A / PD6 (pin 6)
TCCR0A |= _BV(COM0A1) | _BV(WGM00) | _BV(WGM01); // fast PWM, turn on OC0A
//TCCR0B = freq & 0x7;
OCR0A = 0;
#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
// on arduino mega, pin 6 is now PH3 (OC4A)
TCCR4A |= _BV(COM1A1) | _BV(WGM10); // fast PWM, turn on oc4a
TCCR4B = (freq & 0x7) | _BV(WGM12);
//TCCR4B = 1 | _BV(WGM12);
OCR4A = 0;
#else
#error "This chip is not supported!"
#endif
pinMode(6, OUTPUT);
}
inline void setPWM4(uint8_t s) {
#if defined(__AVR_ATmega8__) || \
defined(__AVR_ATmega48__) || \
defined(__AVR_ATmega88__) || \
defined(__AVR_ATmega168__) || \
defined(__AVR_ATmega328P__)
// use PWM from timer0A on PB3 (Arduino pin #6)
OCR0A = s;
#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
// on arduino mega, pin 6 is now PH3 (OC4A)
OCR4A = s;
#else
#error "This chip is not supported!"
#endif
}
void SnootorDC::init(uint8_t num,uint8_t freq) {
SC.add(this);
motornum = num;
motor_speed=0;
inverted=(1+num)%2;
switch (motornum) {
case 1:
initPWM1(freq);
break;
case 2:
initPWM2(freq);
break;
case 3:
initPWM3(freq);
break;
case 4:
initPWM4(freq);
break;
}
}
SnootorDC* SnootorDC::run(uint8_t cmd) {
#ifdef MOTOR_DEBUG
Serial.print("before run : ");
Serial.println(SC._regvalue,HEX);
#endif
SC._regvalue = SC._regvalue | mask_rel[motornum-1];
switch (cmd) {
case FORWARD:
if(!inverted){
SC._regvalue = SC._regvalue & mask_forw[motornum-1];
}
else{
SC._regvalue = SC._regvalue & mask_back[motornum-1];
}
break;
case BACKWARD:
if(!inverted){
SC._regvalue = SC._regvalue & mask_back[motornum-1];
}
else{
SC._regvalue = SC._regvalue & mask_forw[motornum-1];
}
break;
case RELEASE:
break;
}
#ifdef MOTOR_DEBUG
Serial.print("after run : ");
Serial.println(SC._regvalue,HEX);
#endif
SC.i2c(0x02,SC._regvalue);
return this;
}
void SnootorDC::stop(){
run(RELEASE);
}
#ifdef MOTOR_DEBUG
void SnootorDC::dump(){
Serial.println("SnootorDC Motor state : ");
Serial.print("motornum: ");
Serial.print( motornum,DEC);
Serial.print(" inverted: ");
Serial.print( inverted,DEC);
Serial.print(" speed : ");
Serial.println(motor_speed,DEC);
}
#endif
SnootorDC* SnootorDC::setSpeed(uint8_t speed) {
motor_speed=speed;
switch (motornum) {
case 1:
setPWM1(speed); break;
case 2:
setPWM2(speed); break;
case 3:
setPWM3(speed); break;
case 4:
setPWM4(speed); break;
}
return this;
}
SnootorDC::SnootorDC(void){
}
SnootorDC::~SnootorDC(void){}
Jump to Line
Something went wrong with that request. Please try again.