arduino stuffs
36 files changed, 3998 insertions, 9 deletions
| Binary files differ diff --git a/ardu_robot/.DS_Store b/ardu_robot/.DS_Store Binary files differnew file mode 100644 index 0000000..5008ddf --- /dev/null +++ b/ardu_robot/.DS_Store diff --git a/ardu_robot/IrSensors.ino b/ardu_robot/IrSensors.ino new file mode 100644 index 0000000..5b3ee76 --- /dev/null +++ b/ardu_robot/IrSensors.ino @@ -0,0 +1,63 @@ + +/**************************** + ir reflectance sensor code +****************************/ + +const byte NBR_SENSORS = 3; // this version only has left and right sensors +const byte IR_SENSOR[NBR_SENSORS] = {0, 1, 2}; // analog pins for sensors + +int irSensorAmbient[NBR_SENSORS]; // sensor value with no reflection +int irSensorReflect[NBR_SENSORS]; // value considered detecting an object +int irSensorEdge[NBR_SENSORS]; // value considered detecting an edge +boolean isDetected[NBR_SENSORS] = {false,false}; // set true if object detected + +const int irReflectThreshold = 10; // % level below ambient to trigger reflection +const int irEdgeThreshold = 90; // % level above ambient to trigger edge + +void irSensorBegin() +{ + for(int sensor = 0; sensor < NBR_SENSORS; sensor++) + irSensorCalibrate(sensor); +} + +// calibrate for ambient light +void irSensorCalibrate(byte sensor) +{ + int ambient = analogRead(IR_SENSOR[sensor]); // get ambient level + irSensorAmbient[sensor] = ambient; + // precalculate the levels for object and edge detection + irSensorReflect[sensor] = (ambient * (long)(100-irReflectThreshold)) / 100; + irSensorEdge[sensor] = (ambient * (long)(100+irEdgeThreshold)) / 100; +} + +// returns true if an object reflection detected on the given sensor +// the sensor parameter is the index into the sensor array +boolean irSensorDetect(int sensor) +{ + boolean result = false; // default value + int value = analogRead(IR_SENSOR[sensor]); // get IR light level + if( value <= irSensorReflect[sensor]) { + result = true; // object detected (lower value means more reflection) + if( isDetected[sensor] == false) { // only print on initial detection + Serial.print(locationString[sensor]); + Serial.println(" object detected"); + } + } + isDetected[sensor] = result; + return result; +} + +boolean irEdgeDetect(int sensor) +{ + boolean result = false; // default value + int value = analogRead(IR_SENSOR[sensor]); // get IR light level + if( value >= irSensorEdge[sensor]) { + result = true; // edge detected (higher value means less reflection) + if( isDetected[sensor] == false) { // only print on initial detection + Serial.print(locationString[sensor]); + Serial.println(" edge detected"); + } + } + isDetected[sensor] = result; + return result; +} diff --git a/ardu_robot/ardu_robot.ino b/ardu_robot/ardu_robot.ino new file mode 100644 index 0000000..1409fa1 --- /dev/null +++ b/ardu_robot/ardu_robot.ino @@ -0,0 +1,134 @@ +/********************************************************** +MyRobot.ino + +Initial sketch structured using tabs + +Michael Margolis 4 July 2012 +***********************************************************/ + +#include <AFMotor.h> // adafruit motor shield library +#include "RobotMotor.h" // 2wd or 4wd motor library + +#include "globalDefines.h" // global defines + +// Setup runs at startup and is used configure pins and init system variables +void setup() +{ + Serial.begin(9600); + blinkNumber(8); // open port while flashing. Needed for Leonardo only + + motorBegin(MOTOR_LEFT); + motorBegin(MOTOR_RIGHT); + + irSensorBegin(); // initialize sensors + pinMode(LED_PIN, OUTPUT); // enable the LED pin for output + + Serial.println("Waiting for a sensor to detect blocked reflection"); +} + +void loop() +{ + // call a function when reflection blocked on left side + if(lookForObstacle(OBST_LEFT_EDGE) == true) { + calibrateRotationRate(DIR_LEFT,360); // calibrate ccw rotation + } + // as above for right sensor + if(lookForObstacle(OBST_RIGHT_EDGE) == true) { + calibrateRotationRate(DIR_RIGHT, 360); // calibrate CW rotation + } +} + +// function to indicate numbers by flashing the built-in LED +void blinkNumber( byte number) { + pinMode(LED_PIN, OUTPUT); // enable the LED pin for output + while(number--) { + digitalWrite(LED_PIN, HIGH); delay(100); + digitalWrite(LED_PIN, LOW); delay(400); + } +} + +/********************** + code to look for obstacles +**********************/ + +// returns true if the given obstacle is detected +boolean lookForObstacle(int obstacle) +{ + switch(obstacle) { + case OBST_FRONT_EDGE: return irEdgeDetect(DIR_LEFT) || + irEdgeDetect(DIR_RIGHT); + case OBST_LEFT_EDGE: return irEdgeDetect(DIR_LEFT); + case OBST_RIGHT_EDGE: return irEdgeDetect(DIR_RIGHT); + } + return false; +} + +/************************************* + functions to rotate the robot +*************************************/ + +// return the time in milliseconds to turn the given angle at the given speed +long rotationAngleToTime( int angle, int speed) +{ +int fullRotationTime; // time to rotate 360 degrees at given speed + + if(speed < MIN_SPEED) + return 0; // ignore speeds slower then the first table entry + + angle = abs(angle); + + if(speed >= 100) + fullRotationTime = rotationTime[NBR_SPEEDS-1]; // the last entry is 100% + else + { + int index = (speed - MIN_SPEED) / SPEED_TABLE_INTERVAL ; // index into speed and time tables + int t0 = rotationTime[index]; + int t1 = rotationTime[index+1]; // time of the next higher speed + fullRotationTime = map(speed, speedTable[index], speedTable[index+1], t0, t1); + // Serial.print("index= "); Serial.print(index); Serial.print(", t0 = "); Serial.print(t0); Serial.print(", t1 = "); Serial.print(t1); + } + // Serial.print(" full rotation time = "); Serial.println(fullRotationTime); + long result = map(angle, 0,360, 0, fullRotationTime); + return result; +} + +// rotate the robot from MIN_SPEED to 100% increasing by SPEED_TABLE_INTERVAL +void calibrateRotationRate(int sensor, int angle) +{ + Serial.print(locationString[sensor]); + Serial.println(" calibration" ); + for(int speed = MIN_SPEED; speed <= 100; speed += SPEED_TABLE_INTERVAL) + { + delay(1000); + blinkNumber(speed/10); + + if( sensor == DIR_LEFT) + { // rotate left + motorReverse(MOTOR_LEFT, speed); + motorForward(MOTOR_RIGHT, speed); + } + else if( sensor == DIR_RIGHT) + { // rotate right + motorForward(MOTOR_LEFT, speed); + motorReverse(MOTOR_RIGHT, speed); + } + else + Serial.println("Invalid sensor"); + + int time = rotationAngleToTime(angle, speed); + + Serial.print(locationString[sensor]); + Serial.print(": rotate "); + Serial.print(angle); + Serial.print(" degrees at speed "); + Serial.print(speed); + Serial.print(" for "); + Serial.print(time); + Serial.println("ms"); + delay(time); + motorStop(MOTOR_LEFT); + motorStop(MOTOR_RIGHT); + delay(2000); // two second delay between speeds + } +} + diff --git a/ardu_robot/globalDefines.h b/ardu_robot/globalDefines.h new file mode 100644 index 0000000..94b991a --- /dev/null +++ b/ardu_robot/globalDefines.h @@ -0,0 +1,23 @@ + +// defines to identify sensors +const int SENSE_IR_LEFT = 0; +const int SENSE_IR_RIGHT = 1; + +// defines for directions +const int DIR_LEFT = 0; +const int DIR_RIGHT = 1; +const int DIR_CENTER = 2; + +const char* locationString[] = {"Left", "Right", "Center"}; // Debug labels +// http://arduino.cc/en/Reference/String for more on character string arrays + +// obstacles constants +const int OBST_NONE = 0; // no obstacle detected +const int OBST_LEFT_EDGE = 1; // left edge detected +const int OBST_RIGHT_EDGE = 2; // right edge detected +const int OBST_FRONT_EDGE = 3; // edge detect at both left and right sensors + +const int LED_PIN = 13; + +/**** End of Global Defines ****************/ + diff --git a/libraries/.DS_Store b/libraries/.DS_Store Binary files differnew file mode 100644 index 0000000..aa223dc --- /dev/null +++ b/libraries/.DS_Store diff --git a/libraries/AFMotor/AFMotor.cpp b/libraries/AFMotor/AFMotor.cpp new file mode 100755 index 0000000..d3c90c6 --- /dev/null +++ b/libraries/AFMotor/AFMotor.cpp @@ -0,0 +1,598 @@ +// Adafruit Motor shield library +// copyright Adafruit Industries LLC, 2009 +// this code is public domain, enjoy! + +// added Leonardo support - Michael Margolis, 24 July 2012 + +#include <avr/io.h> +#if ARDUINO >= 100 +#include "Arduino.h" +#else +#include "WProgram.h" +#endif + +#include "AFMotor.h" + +static uint8_t latch_state; + +#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 + +AFMotorController::AFMotorController(void) { +} + +void AFMotorController::enable(void) { + // setup the latch + /* + LATCH_DDR |= _BV(LATCH); + ENABLE_DDR |= _BV(ENABLE); + CLK_DDR |= _BV(CLK); + SER_DDR |= _BV(SER); + */ + pinMode(MOTORLATCH, OUTPUT); + pinMode(MOTORENABLE, OUTPUT); + pinMode(MOTORDATA, OUTPUT); + pinMode(MOTORCLK, OUTPUT); + + latch_state = 0; + + latch_tx(); // "reset" + + //ENABLE_PORT &= ~_BV(ENABLE); // enable the chip outputs! + digitalWrite(MOTORENABLE, LOW); +} + + +void AFMotorController::latch_tx(void) { + uint8_t i; + + //LATCH_PORT &= ~_BV(LATCH); + digitalWrite(MOTORLATCH, LOW); + + //SER_PORT &= ~_BV(SER); + digitalWrite(MOTORDATA, LOW); + + for (i=0; i<8; i++) { + //CLK_PORT &= ~_BV(CLK); + digitalWrite(MOTORCLK, LOW); + + if (latch_state & _BV(7-i)) { + //SER_PORT |= _BV(SER); + digitalWrite(MOTORDATA, HIGH); + } else { + //SER_PORT &= ~_BV(SER); + digitalWrite(MOTORDATA, LOW); + } + //CLK_PORT |= _BV(CLK); + digitalWrite(MOTORCLK, HIGH); + } + //LATCH_PORT |= _BV(LATCH); + digitalWrite(MOTORLATCH, HIGH); +} + +static AFMotorController MC; + + +/****************************************** + MOTORS +******************************************/ +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; +#elif defined(__AVR_ATmega32U4__) // Leonardo (mem 24 July 2012) + // use PWM on timer0A (Arduino pin #11 on leo) + TCCR0A |= _BV(COM0A1) | _BV(WGM00) | _BV(WGM01); // fast PWM, turn on OC0A + OCR0A = 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) + OCR2A = s; +#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) + // on arduino mega, pin 11 is now PB5 (OC1A) + OCR1A = s; +#elif defined(__AVR_ATmega32U4__) // Leonardo (mem 24 July 2012) + // use PWM on timer0A (Arduino pin #11 pn leo) + OCR0A = 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; +#elif defined(__AVR_ATmega32U4__) // Leonardo (mem 24 July 2012) + // use PWM from timer0B (pin 3 on leo) + TCCR0A |= _BV(COM0B1) | _BV(WGM00) | _BV(WGM01); // fast PWM, turn on OC0B + OCR0B = 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 timer2B (pin 3) + OCR2B = s; +#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) + OCR3C = s; +#elif defined(__AVR_ATmega32U4__) // Leonardo (mem 24 July 2012) + // use PWM from timer0B (pin 3 pin Leo) + OCR0B = 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 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; +#elif defined(__AVR_ATmega32U4__) // Leonardo (mem 24 July 2012) + // use PWM from timer4D (leo pin 6) + TCCR4B = _BV(CS42) | _BV(CS41) ; //| _BV(CS40); + TCCR4C = _BV(PWM4D) |_BV(COM4D1); + TCCR4D = _BV(WGM40); + +#else + #error "This chip is not supported!" +#endif + pinMode(6, 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) + OCR0A = s; +#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) + // on arduino mega, pin 6 is now PH3 (OC4A) + OCR4A = s; +#elif defined(__AVR_ATmega32U4__) // Leonardo (mem 24 July 2012) + // use PWM from timer4D (leo pin 6) + OCR4D = 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 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) ; // turn on oc3a + TCCR3B = (freq & 0x7) | _BV(WGM12); + //TCCR4B = 1 | _BV(WGM12); + OCR3A = 0; +#elif defined(__AVR_ATmega32U4__) // Leonardo (mem 8 aug 2012) + // use PWM from timer3A (leo pin 5) + TCCR3B = 0; + TCCR3B |= _BV(CS31) | _BV(CS30); + TCCR3A = 0; + TCCR3A |= _BV(COM3A1) | _BV(WGM30); //turn on oc3a + OCR3A = 0; +#else + #error "This chip is not supported!" +#endif + pinMode(5, 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) + OCR0B = s; +#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) + // on arduino mega, pin 6 is now PH3 (OC4A) + OCR3A = s; +#elif defined(__AVR_ATmega32U4__) // Leonardo (mem 24 July 2012) + // use PWM from timer3A (leo pin 5) + OCR3A = s; +#else + #error "This chip is not supported!" +#endif +} + +AF_DCMotor::AF_DCMotor(uint8_t num, uint8_t freq) { + motornum = num; + pwmfreq = freq; + + MC.enable(); + + switch (num) { + case 1: + latch_state &= ~_BV(MOTOR1_A) & ~_BV(MOTOR1_B); // set both motor pins to 0 + MC.latch_tx(); + initPWM1(freq); + break; + case 2: + latch_state &= ~_BV(MOTOR2_A) & ~_BV(MOTOR2_B); // set both motor pins to 0 + MC.latch_tx(); + initPWM2(freq); + break; + case 3: + latch_state &= ~_BV(MOTOR3_A) & ~_BV(MOTOR3_B); // set both motor pins to 0 + MC.latch_tx(); + initPWM3(freq); + break; + case 4: + latch_state &= ~_BV(MOTOR4_A) & ~_BV(MOTOR4_B); // set both motor pins to 0 + MC.latch_tx(); + initPWM4(freq); + break; + } +} + +void AF_DCMotor::run(uint8_t cmd) { + uint8_t a, b; + switch (motornum) { + case 1: + a = MOTOR1_A; b = MOTOR1_B; break; + case 2: + a = MOTOR2_A; b = MOTOR2_B; break; + case 3: + a = MOTOR3_A; b = MOTOR3_B; break; + case 4: + a = MOTOR4_A; b = MOTOR4_B; break; + default: + return; + } + + switch (cmd) { + case FORWARD: + latch_state |= _BV(a); + latch_state &= ~_BV(b); + MC.latch_tx(); + break; + case BACKWARD: + latch_state &= ~_BV(a); + latch_state |= _BV(b); + MC.latch_tx(); + break; + case RELEASE: + latch_state &= ~_BV(a); + latch_state &= ~_BV(b); + MC.latch_tx(); + break; + } +} + +void AF_DCMotor::setSpeed(uint8_t speed) { + switch (motornum) { + case 1: + setPWM1(speed); break; + case 2: + setPWM2(speed); break; + case 3: + setPWM3(speed); break; + case 4: + setPWM4(speed); break; + } +} + +/****************************************** + STEPPERS +******************************************/ + +AF_Stepper::AF_Stepper(uint16_t steps, uint8_t num) { + MC.enable(); + + revsteps = steps; + steppernum = num; + currentstep = 0; + + if (steppernum == 1) { + latch_state &= ~_BV(MOTOR1_A) & ~_BV(MOTOR1_B) & + ~_BV(MOTOR2_A) & ~_BV(MOTOR2_B); // all motor pins to 0 + MC.latch_tx(); + + // enable both H bridges + pinMode(11, OUTPUT); + pinMode(3, OUTPUT); + digitalWrite(11, HIGH); + digitalWrite(3, HIGH); + + // use PWM for microstepping support + initPWM1(MOTOR12_64KHZ); + initPWM2(MOTOR12_64KHZ); + setPWM1(255); + setPWM2(255); + + } else if (steppernum == 2) { + latch_state &= ~_BV(MOTOR3_A) & ~_BV(MOTOR3_B) & + ~_BV(MOTOR4_A) & ~_BV(MOTOR4_B); // all motor pins to 0 + MC.latch_tx(); + + // 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 + initPWM3(1); + initPWM4(1); + setPWM3(255); + setPWM4(255); + } +} + +void AF_Stepper::setSpeed(uint16_t rpm) { + usperstep = 60000000 / ((uint32_t)revsteps * (uint32_t)rpm); + steppingcounter = 0; +} + +void AF_Stepper::release(void) { + if (steppernum == 1) { + latch_state &= ~_BV(MOTOR1_A) & ~_BV(MOTOR1_B) & + ~_BV(MOTOR2_A) & ~_BV(MOTOR2_B); // all motor pins to 0 + MC.latch_tx(); + } else if (steppernum == 2) { + latch_state &= ~_BV(MOTOR3_A) & ~_BV(MOTOR3_B) & + ~_BV(MOTOR4_A) & ~_BV(MOTOR4_B); // all motor pins to 0 + MC.latch_tx(); + } +} + +void AF_Stepper::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--) { + ret = onestep(dir, style); + delay(uspers/1000); // in ms + steppingcounter += (uspers % 1000); + if (steppingcounter >= 1000) { + delay(1); + steppingcounter -= 1000; + } + } + if (style == MICROSTEP) { + while ((ret != 0) && (ret != MICROSTEPS)) { + ret = onestep(dir, style); + delay(uspers/1000); // in ms + steppingcounter += (uspers % 1000); + if (steppingcounter >= 1000) { + delay(1); + steppingcounter -= 1000; + } + } + } +} + +uint8_t AF_Stepper::onestep(uint8_t dir, uint8_t style) { + uint8_t a, b, c, d; + uint8_t ocrb, ocra; + + ocra = ocrb = 255; + + if (steppernum == 1) { + a = _BV(MOTOR1_A); + b = _BV(MOTOR2_A); + c = _BV(MOTOR1_B); + d = _BV(MOTOR2_B); + } else if (steppernum == 2) { + a = _BV(MOTOR3_A); + b = _BV(MOTOR4_A); + c = _BV(MOTOR3_B); + d = _BV(MOTOR4_B); + } else { + return 0; + } + + // 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 + + if (steppernum == 1) { + setPWM1(ocra); + setPWM2(ocrb); + } else if (steppernum == 2) { + setPWM3(ocra); + setPWM4(ocrb); + } + + + // release all + latch_state &= ~a & ~b & ~c & ~d; // all motor pins to 0 + + //Serial.println(step, DEC); + if (style == MICROSTEP) { + if ((currentstep >= 0) && (currentstep < MICROSTEPS)) + latch_state |= a | b; + if ((currentstep >= MICROSTEPS) && (currentstep < MICROSTEPS*2)) + latch_state |= b | c; + if ((currentstep >= MICROSTEPS*2) && (currentstep < MICROSTEPS*3)) + latch_state |= c | d; + if ((currentstep >= MICROSTEPS*3) && (currentstep < MICROSTEPS*4)) + latch_state |= d | a; + } else { + switch (currentstep/(MICROSTEPS/2)) { + case 0: + latch_state |= a; // energize coil 1 only + break; + case 1: + latch_state |= a | b; // energize coil 1+2 + break; + case 2: + latch_state |= b; // energize coil 2 only + break; + case 3: + latch_state |= b | c; // energize coil 2+3 + break; + case 4: + latch_state |= c; // energize coil 3 only + break; + case 5: + latch_state |= c | d; // energize coil 3+4 + break; + case 6: + latch_state |= d; // energize coil 4 only + break; + case 7: + latch_state |= d | a; // energize coil 1+4 + break; + } + } + + + MC.latch_tx(); + return currentstep; +} + diff --git a/libraries/AFMotor/AFMotor.h b/libraries/AFMotor/AFMotor.h new file mode 100755 index 0000000..290d282 --- /dev/null +++ b/libraries/AFMotor/AFMotor.h @@ -0,0 +1,134 @@ +// Adafruit Motor shield library +// copyright Adafruit Industries LLC, 2009 +// this code is public domain, enjoy! + +// updated for Arduino 1.0.2 by mem 22 Nov 2012 + +#ifndef _AFMotor_h_ +#define _AFMotor_h_ + +#include <inttypes.h> +#include <avr/io.h> + +//#define MOTORDEBUG 1 + +#define MICROSTEPS 16 // 8 or 16 + +#if defined(__AVR_ATmega8__) || \ + defined(__AVR_ATmega48__) || \ + defined(__AVR_ATmega88__) || \ + defined(__AVR_ATmega168__) || \ + defined(__AVR_ATmega328P__) +#define MOTOR12_64KHZ _BV(CS20) // no prescale +#define MOTOR12_8KHZ _BV(CS21) // divide by 8 +#define MOTOR12_2KHZ _BV(CS21) | _BV(CS20) // divide by 32 +#define MOTOR12_1KHZ _BV(CS22) // divide by 64 + +#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) +#define MOTOR12_64KHZ _BV(CS10) // no prescale +#define MOTOR12_8KHZ _BV(CS11) // divide by 8 +#define MOTOR12_2KHZ _BV(CS11) | _BV(CS20) // divide by 32 +#define MOTOR12_1KHZ _BV(CS12) // divide by 64 + +#elif defined(__AVR_ATmega32U4__) // Leonardo (mem 22 Nov 2012) +#define MOTOR12_64KHZ _BV(CS00) // no prescale +#define MOTOR12_8KHZ _BV(CS01) // divide by 8 +#define MOTOR12_2KHZ _BV(CS01) | _BV(CS20) // divide by 32 +#define MOTOR12_1KHZ _BV(CS02) // divide by 64 +#endif + +#if defined(__AVR_ATmega8__) || \ + defined(__AVR_ATmega48__) || \ + defined(__AVR_ATmega88__) || \ + defined(__AVR_ATmega168__) || \ + defined(__AVR_ATmega328P__) +#define MOTOR34_64KHZ _BV(CS00) // no prescale +#define MOTOR34_8KHZ _BV(CS01) // divide by 8 +#define MOTOR34_1KHZ _BV(CS01) | _BV(CS00) // divide by 64 +#else +#define MOTOR34_64KHZ _BV(CS40) // no prescale +#define MOTOR34_8KHZ _BV(CS41) // divide by 8 +#define MOTOR34_1KHZ _BV(CS41) | _BV(CS40) // divide by 64 +#endif + +#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 + +/* +#define LATCH 4 +#define LATCH_DDR DDRB +#define LATCH_PORT PORTB + +#define CLK_PORT PORTD +#define CLK_DDR DDRD +#define CLK 4 + +#define ENABLE_PORT PORTD +#define ENABLE_DDR DDRD +#define ENABLE 7 + +#define SER 0 +#define SER_DDR DDRB +#define SER_PORT PORTB +*/ + +// Arduino pin names +#define MOTORLATCH 12 +#define MOTORCLK 4 +#define MOTORENABLE 7 +#define MOTORDATA 8 + +class AFMotorController +{ + public: + AFMotorController(void); + void enable(void); + friend class AF_DCMotor; + void latch_tx(void); +}; + +class AF_DCMotor +{ + public: + AF_DCMotor(uint8_t motornum, uint8_t freq = MOTOR34_8KHZ); + void run(uint8_t); + void setSpeed(uint8_t); + + private: + uint8_t motornum, pwmfreq; +}; + +class AF_Stepper { + public: + AF_Stepper(uint16_t, uint8_t); + 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); + uint16_t revsteps; // # steps per revolution + uint8_t steppernum; + uint32_t usperstep, steppingcounter; + private: + uint8_t currentstep; + +}; + +uint8_t getlatchstate(void); + +#endif diff --git a/libraries/AFMotor/README.txt b/libraries/AFMotor/README.txt new file mode 100755 index 0000000..fdea247 --- /dev/null +++ b/libraries/AFMotor/README.txt @@ -0,0 +1,7 @@ +This is the August 12, 2009 Adafruit Motor shield firmware with basic Microstepping support. Works with all Arduinos including Leonardo Mega + +Modified for the Leonardo by Michael Margolis + +For more information on the shield, please visit http://www.ladyada.net/make/mshield/ + +To install, click DOWNLOAD SOURCE in the top right corner, and see our tutorial at http://www.ladyada.net/library/arduino/libraries.html on Arduino Library installation
\ No newline at end of file diff --git a/libraries/AFMotor/examples/AFMotor_ConstantSpeed/AFMotor_ConstantSpeed.pde b/libraries/AFMotor/examples/AFMotor_ConstantSpeed/AFMotor_ConstantSpeed.pde new file mode 100755 index 0000000..523150f --- /dev/null +++ b/libraries/AFMotor/examples/AFMotor_ConstantSpeed/AFMotor_ConstantSpeed.pde @@ -0,0 +1,37 @@ +// ConstantSpeed.pde +// -*- mode: C++ -*- +// +// Shows how to run AccelStepper in the simplest, +// fixed speed mode with no accelerations +// Requires the AFMotor library (https://github.com/adafruit/Adafruit-Motor-Shield-library) +// And AccelStepper with AFMotor support (https://github.com/adafruit/AccelStepper) +// Public domain! + +#include <AccelStepper.h> +#include <AFMotor.h> + +AF_Stepper motor1(200, 1); + + +// you can change these to DOUBLE or INTERLEAVE or MICROSTEP! +void forwardstep() { + motor1.onestep(FORWARD, SINGLE); +} +void backwardstep() { + motor1.onestep(BACKWARD, SINGLE); +} + +AccelStepper stepper(forwardstep, backwardstep); // use functions to step + +void setup() +{ + Serial.begin(9600); // set up Serial library at 9600 bps + Serial.println("Stepper test!"); + + stepper.setSpeed(50); +} + +void loop() +{ + stepper.runSpeed(); +} diff --git a/libraries/AFMotor/examples/AFMotor_MultiStepper/AFMotor_MultiStepper.pde b/libraries/AFMotor/examples/AFMotor_MultiStepper/AFMotor_MultiStepper.pde new file mode 100755 index 0000000..d6db8b1 --- /dev/null +++ b/libraries/AFMotor/examples/AFMotor_MultiStepper/AFMotor_MultiStepper.pde @@ -0,0 +1,56 @@ +// MultiStepper +// -*- mode: C++ -*- +// +// Control both Stepper motors at the same time with different speeds +// and accelerations. +// Requires the AFMotor library (https://github.com/adafruit/Adafruit-Motor-Shield-library) +// And AccelStepper with AFMotor support (https://github.com/adafruit/AccelStepper) +// Public domain! + +#include <AccelStepper.h> +#include <AFMotor.h> + +// two stepper motors one on each port +AF_Stepper motor1(200, 1); +AF_Stepper motor2(200, 2); + +// you can change these to DOUBLE or INTERLEAVE or MICROSTEP! +// wrappers for the first motor! +void forwardstep1() { + motor1.onestep(FORWARD, SINGLE); +} +void backwardstep1() { + motor1.onestep(BACKWARD, SINGLE); +} +// wrappers for the second motor! +void forwardstep2() { + motor2.onestep(FORWARD, SINGLE); +} +void backwardstep2() { + motor2.onestep(BACKWARD, SINGLE); +} + +// Motor shield has two motor ports, now we'll wrap them in an AccelStepper object +AccelStepper stepper1(forwardstep1, backwardstep1); +AccelStepper stepper2(forwardstep2, backwardstep2); + +void setup() +{ + stepper1.setMaxSpeed(200.0); + stepper1.setAcceleration(100.0); + stepper1.moveTo(24); + + stepper2.setMaxSpeed(300.0); + stepper2.setAcceleration(100.0); + stepper2.moveTo(1000000); + +} + +void loop() +{ + // Change direction at the limits + if (stepper1.distanceToGo() == 0) + stepper1.moveTo(-stepper1.currentPosition()); + stepper1.run(); + stepper2.run(); +} diff --git a/libraries/AFMotor/examples/MotorParty/MotorParty.pde b/libraries/AFMotor/examples/MotorParty/MotorParty.pde new file mode 100755 index 0000000..2c3c5a5 --- /dev/null +++ b/libraries/AFMotor/examples/MotorParty/MotorParty.pde @@ -0,0 +1,60 @@ +// Adafruit Motor shield library +// copyright Adafruit Industries LLC, 2009 +// this code is public domain, enjoy! + +#include <AFMotor.h> +#include <Servo.h> + +// DC motor on M2 +AF_DCMotor motor(2); +// DC hobby servo +Servo servo1; +// Stepper motor on M3+M4 48 steps per revolution +AF_Stepper stepper(48, 2); + +void setup() { + Serial.begin(9600); // set up Serial library at 9600 bps + Serial.println("Motor party!"); + + // turn on servo + servo1.attach(9); + + // turn on motor #2 + motor.setSpeed(200); + motor.run(RELEASE); +} + +int i; + +// Test the DC motor, stepper and servo ALL AT ONCE! +void loop() { + motor.run(FORWARD); + for (i=0; i<255; i++) { + servo1.write(i); + motor.setSpeed(i); + stepper.step(1, FORWARD, INTERLEAVE); + delay(3); + } + + for (i=255; i!=0; i--) { + servo1.write(i-255); + motor.setSpeed(i); + stepper.step(1, BACKWARD, INTERLEAVE); + delay(3); + } + + motor.run(BACKWARD); + for (i=0; i<255; i++) { + servo1.write(i); + motor.setSpeed(i); + delay(3); + stepper.step(1, FORWARD, DOUBLE); + } + + for (i=255; i!=0; i--) { + servo1.write(i-255); + motor.setSpeed(i); + stepper.step(1, BACKWARD, DOUBLE); + delay(3); + } +} diff --git a/libraries/AFMotor/examples/MotorTest/MotorTest.pde b/libraries/AFMotor/examples/MotorTest/MotorTest.pde new file mode 100755 index 0000000..46eccc1 --- /dev/null +++ b/libraries/AFMotor/examples/MotorTest/MotorTest.pde @@ -0,0 +1,52 @@ +// Adafruit Motor shield library +// copyright Adafruit Industries LLC, 2009 +// this code is public domain, enjoy! + +#include <AFMotor.h> + +AF_DCMotor motor(4); + +void setup() { + Serial.begin(9600); // set up Serial library at 9600 bps + Serial.println("Motor test!"); + + // turn on motor + motor.setSpeed(200); + + motor.run(RELEASE); +} + +void loop() { + uint8_t i; + + Serial.print("tick"); + + motor.run(FORWARD); + for (i=0; i<255; i++) { + motor.setSpeed(i); + delay(10); + } + + for (i=255; i!=0; i--) { + motor.setSpeed(i); + delay(10); + } + + Serial.print("tock"); + + motor.run(BACKWARD); + for (i=0; i<255; i++) { + motor.setSpeed(i); + delay(10); + } + + for (i=255; i!=0; i--) { + motor.setSpeed(i); + delay(10); + } + + + Serial.print("tech"); + motor.run(RELEASE); + delay(1000); +} diff --git a/libraries/AFMotor/examples/StepperTest/StepperTest.pde b/libraries/AFMotor/examples/StepperTest/StepperTest.pde new file mode 100755 index 0000000..57d7fd4 --- /dev/null +++ b/libraries/AFMotor/examples/StepperTest/StepperTest.pde @@ -0,0 +1,34 @@ +// Adafruit Motor shield library +// copyright Adafruit Industries LLC, 2009 +// this code is public domain, enjoy! + +#include <AFMotor.h> + +// Connect a stepper motor with 48 steps per revolution (7.5 degree) +// to motor port #2 (M3 and M4) +AF_Stepper motor(48, 2); + +void setup() { + Serial.begin(9600); // set up Serial library at 9600 bps + Serial.println("Stepper test!"); + + motor.setSpeed(10); // 10 rpm +} + +void loop() { + Serial.println("Single coil steps"); + motor.step(100, FORWARD, SINGLE); + motor.step(100, BACKWARD, SINGLE); + + Serial.println("Double coil steps"); + motor.step(100, FORWARD, DOUBLE); + motor.step(100, BACKWARD, DOUBLE); + + Serial.println("Interleave coil steps"); + motor.step(100, FORWARD, INTERLEAVE); + motor.step(100, BACKWARD, INTERLEAVE); + + Serial.println("Micrsostep steps"); + motor.step(100, FORWARD, MICROSTEP); + motor.step(100, BACKWARD, MICROSTEP); +} diff --git a/libraries/AFMotor/keywords.txt b/libraries/AFMotor/keywords.txt new file mode 100755 index 0000000..6fa42dc --- /dev/null +++ b/libraries/AFMotor/keywords.txt @@ -0,0 +1,35 @@ +####################################### +# Syntax Coloring Map for AFMotor +####################################### + +####################################### +# Datatypes (KEYWORD1) +####################################### + +AF_DCMotor KEYWORD1 +AF_Stepper KEYWORD1 + +####################################### +# Methods and Functions (KEYWORD2) +####################################### + +enable KEYWORD2 +run KEYWORD2 +setSpeed KEYWORD2 +step KEYWORD2 +onestep KEYWORD2 +release KEYWORD2 + +####################################### +# Constants (LITERAL1) +####################################### + +MICROSTEPPING LITERAL1 +FORWARD LITERAL1 +BACKWARD LITERAL1 +BRAKE LITERAL1 +RELEASE LITERAL1 +SINGLE LITERAL1 +DOUBLE LITERAL1 +INTERLEAVE LITERAL1 +MICROSTEP LITERAL1
\ No newline at end of file diff --git a/libraries/IRremote/IRremote.cpp b/libraries/IRremote/IRremote.cpp new file mode 100755 index 0000000..96a407d --- /dev/null +++ b/libraries/IRremote/IRremote.cpp @@ -0,0 +1,596 @@ +/* + * IRremote + * Version 0.11 August, 2009 + * Copyright 2009 Ken Shirriff + * For details, see http://arcfn.com/2009/08/multi-protocol-infrared-remote-library.html + * + * Modified by Paul Stoffregen <[email protected]> to support other boards and timers + * + * Interrupt code based on NECIRrcv by Joe Knapp + * http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1210243556 + * Also influenced by http://zovirl.com/2008/11/12/building-a-universal-remote-with-an-arduino/ + */ + +#include "IRremote.h" +#include "IRremoteInt.h" + +// Provides ISR +#include <avr/interrupt.h> + +volatile irparams_t irparams; + +// These versions of MATCH, MATCH_MARK, and MATCH_SPACE are only for debugging. +// To use them, set DEBUG in IRremoteInt.h +// Normally macros are used for efficiency +#ifdef DEBUG +int MATCH(int measured, int desired) { + Serial.print("Testing: "); + Serial.print(TICKS_LOW(desired), DEC); + Serial.print(" <= "); + Serial.print(measured, DEC); + Serial.print(" <= "); + Serial.println(TICKS_HIGH(desired), DEC); + return measured >= TICKS_LOW(desired) && measured <= TICKS_HIGH(desired); +} + +int MATCH_MARK(int measured_ticks, int desired_us) { + Serial.print("Testing mark "); + Serial.print(measured_ticks * USECPERTICK, DEC); + Serial.print(" vs "); + Serial.print(desired_us, DEC); + Serial.print(": "); + Serial.print(TICKS_LOW(desired_us + MARK_EXCESS), DEC); + Serial.print(" <= "); + Serial.print(measured_ticks, DEC); + Serial.print(" <= "); + Serial.println(TICKS_HIGH(desired_us + MARK_EXCESS), DEC); + return measured_ticks >= TICKS_LOW(desired_us + MARK_EXCESS) && measured_ticks <= TICKS_HIGH(desired_us + MARK_EXCESS); +} + +int MATCH_SPACE(int measured_ticks, int desired_us) { + Serial.print("Testing space "); + Serial.print(measured_ticks * USECPERTICK, DEC); + Serial.print(" vs "); + Serial.print(desired_us, DEC); + Serial.print(": "); + Serial.print(TICKS_LOW(desired_us - MARK_EXCESS), DEC); + Serial.print(" <= "); + Serial.print(measured_ticks, DEC); + Serial.print(" <= "); + Serial.println(TICKS_HIGH(desired_us - MARK_EXCESS), DEC); + return measured_ticks >= TICKS_LOW(desired_us - MARK_EXCESS) && measured_ticks <= TICKS_HIGH(desired_us - MARK_EXCESS); +} +#endif + +void IRsend::sendNEC(unsigned long data, int nbits) +{ + enableIROut(38); + mark(NEC_HDR_MARK); + space(NEC_HDR_SPACE); + for (int i = 0; i < nbits; i++) { + if (data & TOPBIT) { + mark(NEC_BIT_MARK); + space(NEC_ONE_SPACE); + } + else { + mark(NEC_BIT_MARK); + space(NEC_ZERO_SPACE); + } + data <<= 1; + } + mark(NEC_BIT_MARK); + space(0); +} + +void IRsend::sendSony(unsigned long data, int nbits) { + enableIROut(40); + mark(SONY_HDR_MARK); + space(SONY_HDR_SPACE); + data = data << (32 - nbits); + for (int i = 0; i < nbits; i++) { + if (data & TOPBIT) { + mark(SONY_ONE_MARK); + space(SONY_HDR_SPACE); + } + else { + mark(SONY_ZERO_MARK); + space(SONY_HDR_SPACE); + } + data <<= 1; + } +} + +void IRsend::sendRaw(unsigned int buf[], int len, int hz) +{ + enableIROut(hz); + for (int i = 0; i < len; i++) { + if (i & 1) { + space(buf[i]); + } + else { + mark(buf[i]); + } + } + space(0); // Just to be sure +} + +// Note: first bit must be a one (start bit) +void IRsend::sendRC5(unsigned long data, int nbits) +{ + enableIROut(36); + data = data << (32 - nbits); + mark(RC5_T1); // First start bit + space(RC5_T1); // Second start bit + mark(RC5_T1); // Second start bit + for (int i = 0; i < nbits; i++) { + if (data & TOPBIT) { + space(RC5_T1); // 1 is space, then mark + mark(RC5_T1); + } + else { + mark(RC5_T1); + space(RC5_T1); + } + data <<= 1; + } + space(0); // Turn off at end +} + +// Caller needs to take care of flipping the toggle bit +void IRsend::sendRC6(unsigned long data, int nbits) +{ + enableIROut(36); + data = data << (32 - nbits); + mark(RC6_HDR_MARK); + space(RC6_HDR_SPACE); + mark(RC6_T1); // start bit + space(RC6_T1); + int t; + for (int i = 0; i < nbits; i++) { + if (i == 3) { + // double-wide trailer bit + t = 2 * RC6_T1; + } + else { + t = RC6_T1; + } + if (data & TOPBIT) { + mark(t); + space(t); + } + else { + space(t); + mark(t); + } + + data <<= 1; + } + space(0); // Turn off at end +} + +void IRsend::mark(int time) { + // Sends an IR mark for the specified number of microseconds. + // The mark output is modulated at the PWM frequency. + TIMER_ENABLE_PWM; // Enable pin 3 PWM output + delayMicroseconds(time); +} + +/* Leave pin off for time (given in microseconds) */ +void IRsend::space(int time) { + // Sends an IR space for the specified number of microseconds. + // A space is no output, so the PWM output is disabled. + TIMER_DISABLE_PWM; // Disable pin 3 PWM output + delayMicroseconds(time); +} + +void IRsend::enableIROut(int khz) { + // Enables IR output. The khz value controls the modulation frequency in kilohertz. + // The IR output will be on pin 3 (OC2B). + // This routine is designed for 36-40KHz; if you use it for other values, it's up to you + // to make sure it gives reasonable results. (Watch out for overflow / underflow / rounding.) + // TIMER2 is used in phase-correct PWM mode, with OCR2A controlling the frequency and OCR2B + // controlling the duty cycle. + // There is no prescaling, so the output frequency is 16MHz / (2 * OCR2A) + // To turn the output on and off, we leave the PWM running, but connect and disconnect the output pin. + // A few hours staring at the ATmega documentation and this will all make sense. + // See my Secrets of Arduino PWM at http://arcfn.com/2009/07/secrets-of-arduino-pwm.html for details. + + + // Disable the Timer2 Interrupt (which is used for receiving IR) + TIMER_DISABLE_INTR; //Timer2 Overflow Interrupt + + pinMode(TIMER_PWM_PIN, OUTPUT); + digitalWrite(TIMER_PWM_PIN, LOW); // When not sending PWM, we want it low + + // COM2A = 00: disconnect OC2A + // COM2B = 00: disconnect OC2B; to send signal set to 10: OC2B non-inverted + // WGM2 = 101: phase-correct PWM with OCRA as top + // CS2 = 000: no prescaling + // The top value for the timer. The modulation frequency will be SYSCLOCK / 2 / OCR2A. + TIMER_CONFIG_KHZ(khz); +} + +IRrecv::IRrecv(int recvpin) +{ + irparams.recvpin = recvpin; + irparams.blinkflag = 0; +} + +// initialization +void IRrecv::enableIRIn() { + cli(); + // setup pulse clock timer interrupt + //Prescale /8 (16M/8 = 0.5 microseconds per tick) + // Therefore, the timer interval can range from 0.5 to 128 microseconds + // depending on the reset value (255 to 0) + TIMER_CONFIG_NORMAL(); + + //Timer2 Overflow Interrupt Enable + TIMER_ENABLE_INTR; + + TIMER_RESET; + + sei(); // enable interrupts + + // initialize state machine variables + irparams.rcvstate = STATE_IDLE; + irparams.rawlen = 0; + + // set pin modes + pinMode(irparams.recvpin, INPUT); +} + +// enable/disable blinking of pin 13 on IR processing +void IRrecv::blink13(int blinkflag) +{ + irparams.blinkflag = blinkflag; + if (blinkflag) + pinMode(BLINKLED, OUTPUT); +} + +// TIMER2 interrupt code to collect raw data. +// Widths of alternating SPACE, MARK are recorded in rawbuf. +// Recorded in ticks of 50 microseconds. +// rawlen counts the number of entries recorded so far. +// First entry is the SPACE between transmissions. +// As soon as a SPACE gets long, ready is set, state switches to IDLE, timing of SPACE continues. +// As soon as first MARK arrives, gap width is recorded, ready is cleared, and new logging starts +ISR(TIMER_INTR_NAME) +{ + TIMER_RESET; + + uint8_t irdata = (uint8_t)digitalRead(irparams.recvpin); + //uint8_t irdata = PIND & _BV(2); // ir on pin 2 + + irparams.timer++; // One more 50us tick + if (irparams.rawlen >= RAWBUF) { + // Buffer overflow + irparams.rcvstate = STATE_STOP; + } + switch(irparams.rcvstate) { + case STATE_IDLE: // In the middle of a gap + if (irdata == MARK) { + if (irparams.timer < GAP_TICKS) { + // Not big enough to be a gap. + irparams.timer = 0; + } + else { + // gap just ended, record duration and start recording transmission + irparams.rawlen = 0; + irparams.rawbuf[irparams.rawlen++] = irparams.timer; + irparams.timer = 0; + irparams.rcvstate = STATE_MARK; + } + } + break; + case STATE_MARK: // timing MARK + if (irdata == SPACE) { // MARK ended, record time + irparams.rawbuf[irparams.rawlen++] = irparams.timer; + irparams.timer = 0; + irparams.rcvstate = STATE_SPACE; + } + break; + case STATE_SPACE: // timing SPACE + if (irdata == MARK) { // SPACE just ended, record it + irparams.rawbuf[irparams.rawlen++] = irparams.timer; + irparams.timer = 0; + irparams.rcvstate = STATE_MARK; + } + else { // SPACE + if (irparams.timer > GAP_TICKS) { + // big SPACE, indicates gap between codes + // Mark current code as ready for processing + // Switch to STOP + // Don't reset timer; keep counting space width + irparams.rcvstate = STATE_STOP; + } + } + break; + case STATE_STOP: // waiting, measuring gap + if (irdata == MARK) { // reset gap timer + irparams.timer = 0; + } + break; + } + + if (irparams.blinkflag) { + if (irdata == MARK) { + BLINKLED_ON(); // turn pin 13 LED on + } + else { + BLINKLED_OFF(); // turn pin 13 LED off + } + } +} + +void IRrecv::resume() { + irparams.rcvstate = STATE_IDLE; + irparams.rawlen = 0; +} + + + +// Decodes the received IR message +// Returns 0 if no data ready, 1 if data ready. +// Results of decoding are stored in results +int IRrecv::decode(decode_results *results) { + results->rawbuf = irparams.rawbuf; + results->rawlen = irparams.rawlen; + if (irparams.rcvstate != STATE_STOP) { + return ERR; + } +#ifdef DEBUG + Serial.println("Attempting NEC decode"); +#endif + if (decodeNEC(results)) { + return DECODED; + } +#ifdef DEBUG + Serial.println("Attempting Sony decode"); +#endif + if (decodeSony(results)) { + return DECODED; + } +#ifdef DEBUG + Serial.println("Attempting RC5 decode"); +#endif + if (decodeRC5(results)) { + return DECODED; + } +#ifdef DEBUG + Serial.println("Attempting RC6 decode"); +#endif + if (decodeRC6(results)) { + return DECODED; + } + if (results->rawlen >= 6) { + // Only return raw buffer if at least 6 bits + results->decode_type = UNKNOWN; + results->bits = 0; + results->value = 0; + return DECODED; + } + // Throw away and start over + resume(); + return ERR; +} + +long IRrecv::decodeNEC(decode_results *results) { + long data = 0; + int offset = 1; // Skip first space + // Initial mark + if (!MATCH_MARK(results->rawbuf[offset], NEC_HDR_MARK)) { + return ERR; + } + offset++; + // Check for repeat + if (irparams.rawlen == 4 && + MATCH_SPACE(results->rawbuf[offset], NEC_RPT_SPACE) && + MATCH_MARK(results->rawbuf[offset+1], NEC_BIT_MARK)) { + results->bits = 0; + results->value = REPEAT; + results->decode_type = NEC; + return DECODED; + } + if (irparams.rawlen < 2 * NEC_BITS + 4) { + return ERR; + } + // Initial space + if (!MATCH_SPACE(results->rawbuf[offset], NEC_HDR_SPACE)) { + return ERR; + } + offset++; + for (int i = 0; i < NEC_BITS; i++) { + if (!MATCH_MARK(results->rawbuf[offset], NEC_BIT_MARK)) { + return ERR; + } + offset++; + if (MATCH_SPACE(results->rawbuf[offset], NEC_ONE_SPACE)) { + data = (data << 1) | 1; + } + else if (MATCH_SPACE(results->rawbuf[offset], NEC_ZERO_SPACE)) { + data <<= 1; + } + else { + return ERR; + } + offset++; + } + // Success + results->bits = NEC_BITS; + results->value = data; + results->decode_type = NEC; + return DECODED; +} + +long IRrecv::decodeSony(decode_results *results) { + long data = 0; + if (irparams.rawlen < 2 * SONY_BITS + 2) { + return ERR; + } + int offset = 1; // Skip first space + // Initial mark + if (!MATCH_MARK(results->rawbuf[offset], SONY_HDR_MARK)) { + return ERR; + } + offset++; + + while (offset + 1 < irparams.rawlen) { + if (!MATCH_SPACE(results->rawbuf[offset], SONY_HDR_SPACE)) { + break; + } + offset++; + if (MATCH_MARK(results->rawbuf[offset], SONY_ONE_MARK)) { + data = (data << 1) | 1; + } + else if (MATCH_MARK(results->rawbuf[offset], SONY_ZERO_MARK)) { + data <<= 1; + } + else { + return ERR; + } + offset++; + } + + // Success + results->bits = (offset - 1) / 2; + if (results->bits < 12) { + results->bits = 0; + return ERR; + } + results->value = data; + results->decode_type = SONY; + return DECODED; +} + +// Gets one undecoded level at a time from the raw buffer. +// The RC5/6 decoding is easier if the data is broken into time intervals. +// E.g. if the buffer has MARK for 2 time intervals and SPACE for 1, +// successive calls to getRClevel will return MARK, MARK, SPACE. +// offset and used are updated to keep track of the current position. +// t1 is the time interval for a single bit in microseconds. +// Returns -1 for error (measured time interval is not a multiple of t1). +int IRrecv::getRClevel(decode_results *results, int *offset, int *used, int t1) { + if (*offset >= results->rawlen) { + // After end of recorded buffer, assume SPACE. + return SPACE; + } + int width = results->rawbuf[*offset]; + int val = ((*offset) % 2) ? MARK : SPACE; + int correction = (val == MARK) ? MARK_EXCESS : - MARK_EXCESS; + + int avail; + if (MATCH(width, t1 + correction)) { + avail = 1; + } + else if (MATCH(width, 2*t1 + correction)) { + avail = 2; + } + else if (MATCH(width, 3*t1 + correction)) { + avail = 3; + } + else { + return -1; + } + + (*used)++; + if (*used >= avail) { + *used = 0; + (*offset)++; + } +#ifdef DEBUG + if (val == MARK) { + Serial.println("MARK"); + } + else { + Serial.println("SPACE"); + } +#endif + return val; +} + +long IRrecv::decodeRC5(decode_results *results) { + if (irparams.rawlen < MIN_RC5_SAMPLES + 2) { + return ERR; + } + int offset = 1; // Skip gap space + long data = 0; + int used = 0; + // Get start bits + if (getRClevel(results, &offset, &used, RC5_T1) != MARK) return ERR; + if (getRClevel(results, &offset, &used, RC5_T1) != SPACE) return ERR; + if (getRClevel(results, &offset, &used, RC5_T1) != MARK) return ERR; + int nbits; + for (nbits = 0; offset < irparams.rawlen; nbits++) { + int levelA = getRClevel(results, &offset, &used, RC5_T1); + int levelB = getRClevel(results, &offset, &used, RC5_T1); + if (levelA == SPACE && levelB == MARK) { + // 1 bit + data = (data << 1) | 1; + } + else if (levelA == MARK && levelB == SPACE) { + // zero bit + data <<= 1; + } + else { + return ERR; + } + } + + // Success + results->bits = nbits; + results->value = data; + results->decode_type = RC5; + return DECODED; +} + +long IRrecv::decodeRC6(decode_results *results) { + if (results->rawlen < MIN_RC6_SAMPLES) { + return ERR; + } + int offset = 1; // Skip first space + // Initial mark + if (!MATCH_MARK(results->rawbuf[offset], RC6_HDR_MARK)) { + return ERR; + } + offset++; + if (!MATCH_SPACE(results->rawbuf[offset], RC6_HDR_SPACE)) { + return ERR; + } + offset++; + long data = 0; + int used = 0; + // Get start bit (1) + if (getRClevel(results, &offset, &used, RC6_T1) != MARK) return ERR; + if (getRClevel(results, &offset, &used, RC6_T1) != SPACE) return ERR; + int nbits; + for (nbits = 0; offset < results->rawlen; nbits++) { + int levelA, levelB; // Next two levels + levelA = getRClevel(results, &offset, &used, RC6_T1); + if (nbits == 3) { + // T bit is double wide; make sure second half matches + if (levelA != getRClevel(results, &offset, &used, RC6_T1)) return ERR; + } + levelB = getRClevel(results, &offset, &used, RC6_T1); + if (nbits == 3) { + // T bit is double wide; make sure second half matches + if (levelB != getRClevel(results, &offset, &used, RC6_T1)) return ERR; + } + if (levelA == MARK && levelB == SPACE) { // reversed compared to RC5 + // 1 bit + data = (data << 1) | 1; + } + else if (levelA == SPACE && levelB == MARK) { + // zero bit + data <<= 1; + } + else { + return ERR; // Error + } + } + // Success + results->bits = nbits; + results->value = data; + results->decode_type = RC6; + return DECODED; +} diff --git a/libraries/IRremote/IRremote.h b/libraries/IRremote/IRremote.h new file mode 100755 index 0000000..1e485a9 --- /dev/null +++ b/libraries/IRremote/IRremote.h @@ -0,0 +1,94 @@ +/* + * IRremote + * Version 0.1 July, 2009 + * Copyright 2009 Ken Shirriff + * For details, see http://arcfn.com/2009/08/multi-protocol-infrared-remote-library.htm http://arcfn.com + * + * Interrupt code based on NECIRrcv by Joe Knapp + * http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1210243556 + * Also influenced by http://zovirl.com/2008/11/12/building-a-universal-remote-with-an-arduino/ + */ + +#ifndef IRremote_h +#define IRremote_h + +// The following are compile-time library options. +// If you change them, recompile the library. +// If DEBUG is defined, a lot of debugging output will be printed during decoding. +// TEST must be defined for the IRtest unittests to work. It will make some +// methods virtual, which will be slightly slower, which is why it is optional. +// #define DEBUG +// #define TEST + +// Results returned from the decoder +class decode_results { +public: + int decode_type; // NEC, SONY, RC5, UNKNOWN + unsigned long value; // Decoded value + int bits; // Number of bits in decoded value + volatile unsigned int *rawbuf; // Raw intervals in .5 us ticks + int rawlen; // Number of records in rawbuf. +}; + +// Values for decode_type +#define NEC 1 +#define SONY 2 +#define RC5 3 +#define RC6 4 +#define UNKNOWN -1 + +// Decoded value for NEC when a repeat code is received +#define REPEAT 0xffffffff + +// main class for receiving IR +class IRrecv +{ +public: + IRrecv(int recvpin); + void blink13(int blinkflag); + int decode(decode_results *results); + void enableIRIn(); + void resume(); +private: + // These are called by decode + int getRClevel(decode_results *results, int *offset, int *used, int t1); + long decodeNEC(decode_results *results); + long decodeSony(decode_results *results); + long decodeRC5(decode_results *results); + long decodeRC6(decode_results *results); +} +; + +// Only used for testing; can remove virtual for shorter code +#ifdef TEST +#define VIRTUAL virtual +#else +#define VIRTUAL +#endif + +class IRsend +{ +public: + IRsend() {} + void sendNEC(unsigned long data, int nbits); + void sendSony(unsigned long data, int nbits); + void sendRaw(unsigned int buf[], int len, int hz); + void sendRC5(unsigned long data, int nbits); + void sendRC6(unsigned long data, int nbits); + // private: + void enableIROut(int khz); + VIRTUAL void mark(int usec); + VIRTUAL void space(int usec); +} +; + +// Some useful constants + +#define USECPERTICK 50 // microseconds per clock interrupt tick +#define RAWBUF 76 // Length of raw duration buffer + +// Marks tend to be 100us too long, and spaces 100us too short +// when received due to sensor lag. +#define MARK_EXCESS 100 + +#endif diff --git a/libraries/IRremote/IRremoteInt.h b/libraries/IRremote/IRremoteInt.h new file mode 100755 index 0000000..1948b13 --- /dev/null +++ b/libraries/IRremote/IRremoteInt.h @@ -0,0 +1,389 @@ +/* + * IRremote + * Version 0.1 July, 2009 + * Copyright 2009 Ken Shirriff + * For details, see http://arcfn.com/2009/08/multi-protocol-infrared-remote-library.html + * + * Modified by Paul Stoffregen <[email protected]> to support other boards and timers + * Michael Margolis added Leonardo support + * + * Interrupt code based on NECIRrcv by Joe Knapp + * http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1210243556 + * Also influenced by http://zovirl.com/2008/11/12/building-a-universal-remote-with-an-arduino/ + */ + +#ifndef IRremoteint_h +#define IRremoteint_h + +#if ARDUINO >= 100 +#include "Arduino.h" +#else +#include <WProgram.h> +#endif + +// define which timer to use +// +// Uncomment the timer you wish to use on your board. If you +// are using another library which uses timer2, you have options +// to switch IRremote to use a different timer. + +// Arduino Mega +#if defined(__AVR_ATmega1280__) + //#define IR_USE_TIMER1 // tx = pin 11 + #define IR_USE_TIMER2 // tx = pin 9 + //#define IR_USE_TIMER3 // tx = pin 5 + //#define IR_USE_TIMER4 // tx = pin 6 + //#define IR_USE_TIMER5 // tx = pin 46 + +// Teensy 1.0 +#elif defined(__AVR_AT90USB162__) + #define IR_USE_TIMER1 // tx = pin 17 + +// Leonardo or Teensy 2.0 +#elif defined(__AVR_ATmega32U4__) + #define IR_USE_TIMER1 // tx = pin 14 + // #define IR_USE_TIMER3 // tx = pin 9 + // #define IR_USE_TIMER4_HS // tx = pin 10 + +// Teensy++ 1.0 & 2.0 +#elif defined(__AVR_AT90USB646__) || defined(__AVR_AT90USB1286__) + //#define IR_USE_TIMER1 // tx = pin 25 + #define IR_USE_TIMER2 // tx = pin 1 + //#define IR_USE_TIMER3 // tx = pin 16 + +// Sanguino +#elif defined(__AVR_ATmega644P__) || defined(__AVR_ATmega644__) + //#define IR_USE_TIMER1 // tx = pin 13 + #define IR_USE_TIMER2 // tx = pin 14 + +// Arduino Duemilanove, Diecimila, LilyPad, Mini, Fio, etc +#else + #define IR_USE_TIMER1 // tx = pin 9 + //#define IR_USE_TIMER2 // tx = pin 3 +#endif + + + +#ifdef F_CPU +#define SYSCLOCK F_CPU // main Arduino clock +#else +#define SYSCLOCK 16000000 // main Arduino clock +#endif + +#define ERR 0 +#define DECODED 1 + + +// defines for setting and clearing register bits +#ifndef cbi +#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit)) +#endif +#ifndef sbi +#define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit)) +#endif + + +// pulse parameters in usec +#define NEC_HDR_MARK 9000 +#define NEC_HDR_SPACE 4500 +#define NEC_BIT_MARK 560 +#define NEC_ONE_SPACE 1600 +#define NEC_ZERO_SPACE 560 +#define NEC_RPT_SPACE 2250 + +#define SONY_HDR_MARK 2400 +#define SONY_HDR_SPACE 600 +#define SONY_ONE_MARK 1200 +#define SONY_ZERO_MARK 600 +#define SONY_RPT_LENGTH 45000 + +#define RC5_T1 889 +#define RC5_RPT_LENGTH 46000 + +#define RC6_HDR_MARK 2666 +#define RC6_HDR_SPACE 889 +#define RC6_T1 444 +#define RC6_RPT_LENGTH 46000 + +#define TOLERANCE 25 // percent tolerance in measurements +#define LTOL (1.0 - TOLERANCE/100.) +#define UTOL (1.0 + TOLERANCE/100.) + +#define _GAP 5000 // Minimum map between transmissions +#define GAP_TICKS (_GAP/USECPERTICK) + +#define TICKS_LOW(us) (int) (((us)*LTOL/USECPERTICK)) +#define TICKS_HIGH(us) (int) (((us)*UTOL/USECPERTICK + 1)) + +#ifndef DEBUG +#define MATCH(measured_ticks, desired_us) ((measured_ticks) >= TICKS_LOW(desired_us) && (measured_ticks) <= TICKS_HIGH(desired_us)) +#define MATCH_MARK(measured_ticks, desired_us) MATCH(measured_ticks, (desired_us) + MARK_EXCESS) +#define MATCH_SPACE(measured_ticks, desired_us) MATCH((measured_ticks), (desired_us) - MARK_EXCESS) +// Debugging versions are in IRremote.cpp +#endif + +// receiver states +#define STATE_IDLE 2 +#define STATE_MARK 3 +#define STATE_SPACE 4 +#define STATE_STOP 5 + +// information for the interrupt handler +typedef struct { + uint8_t recvpin; // pin for IR data from detector + uint8_t rcvstate; // state machine + uint8_t blinkflag; // TRUE to enable blinking of pin 13 on IR processing + unsigned int timer; // state timer, counts 50uS ticks. + unsigned int rawbuf[RAWBUF]; // raw data + uint8_t rawlen; // counter of entries in rawbuf +} +irparams_t; + +// Defined in IRremote.cpp +extern volatile irparams_t irparams; + +// IR detector output is active low +#define MARK 0 +#define SPACE 1 + +#define TOPBIT 0x80000000 + +#define NEC_BITS 32 +#define SONY_BITS 12 +#define MIN_RC5_SAMPLES 11 +#define MIN_RC6_SAMPLES 1 + + + + +// defines for timer2 (8 bits) +#if defined(IR_USE_TIMER2) +#define TIMER_RESET +#define TIMER_ENABLE_PWM (TCCR2A |= _BV(COM2B1)) +#define TIMER_DISABLE_PWM (TCCR2A &= ~(_BV(COM2B1))) +#define TIMER_ENABLE_INTR (TIMSK2 = _BV(OCIE2A)) +#define TIMER_DISABLE_INTR (TIMSK2 - 0) +#define TIMER_INTR_NAME TIMER2_COMPA_vect +#define TIMER_CONFIG_KHZ(val) ({ \ + const uint8_t pwmval = SYSCLOCK / 2000 / (val); \ + TCCR2A = _BV(WGM20); \ + TCCR2B = _BV(WGM22) | _BV(CS20); \ + OCR2A = pwmval; \ + OCR2B = pwmval / 3; \ +}) +#define TIMER_COUNT_TOP (SYSCLOCK * USECPERTICK / 1000000) +#if (TIMER_COUNT_TOP < 256) +#define TIMER_CONFIG_NORMAL() ({ \ + TCCR2A = _BV(WGM21); \ + TCCR2B = _BV(CS20); \ + OCR2A = TIMER_COUNT_TOP; \ + TCNT2 = 0; \ +}) +#else +#define TIMER_CONFIG_NORMAL() ({ \ + TCCR2A = _BV(WGM21); \ + TCCR2B = _BV(CS21); \ + OCR2A = TIMER_COUNT_TOP / 8; \ + TCNT2 = 0; \ +}) +#endif +#if defined(CORE_OC2B_PIN) +#define TIMER_PWM_PIN CORE_OC2B_PIN /* Teensy */ +#elif defined(__AVR_ATmega1280__) +#define TIMER_PWM_PIN 9 /* Arduino Mega */ +#elif defined(__AVR_ATmega644P__) || defined(__AVR_ATmega644__) +#define TIMER_PWM_PIN 14 /* Sanguino */ +#else +#define TIMER_PWM_PIN 3 /* Arduino Duemilanove, Diecimila, LilyPad, etc */ +#endif + + +// defines for timer1 (16 bits) +#elif defined(IR_USE_TIMER1) +#define TIMER_RESET +#define TIMER_ENABLE_PWM (TCCR1A |= _BV(COM1A1)) +#define TIMER_DISABLE_PWM (TCCR1A &= ~(_BV(COM1A1))) +#define TIMER_ENABLE_INTR (TIMSK1 = _BV(OCIE1A)) +#define TIMER_DISABLE_INTR (TIMSK1 = 0) +#define TIMER_INTR_NAME TIMER1_COMPA_vect +#define TIMER_CONFIG_KHZ(val) ({ \ + const uint16_t pwmval = SYSCLOCK / 2000 / (val); \ + TCCR1A = _BV(WGM11); \ + TCCR1B = _BV(WGM13) | _BV(CS10); \ + ICR1 = pwmval; \ + OCR1A = pwmval / 3; \ +}) +#define TIMER_CONFIG_NORMAL() ({ \ + TCCR1A = 0; \ + TCCR1B = _BV(WGM12) | _BV(CS10); \ + OCR1A = SYSCLOCK * USECPERTICK / 1000000; \ + TCNT1 = 0; \ +}) +#if defined(CORE_OC1A_PIN) +#define TIMER_PWM_PIN CORE_OC1A_PIN /* Teensy */ +#elif defined(__AVR_ATmega1280__) +#define TIMER_PWM_PIN 11 /* Arduino Mega */ +#elif defined(__AVR_ATmega644P__) || defined(__AVR_ATmega644__) +#define TIMER_PWM_PIN 13 /* Sanguino */ +#else +#define TIMER_PWM_PIN 9 /* Arduino Duemilanove, Diecimila, LilyPad, etc */ +#endif + + +// defines for timer3 (16 bits) +#elif defined(IR_USE_TIMER3) +#define TIMER_RESET +#define TIMER_ENABLE_PWM (TCCR3A |= _BV(COM3A1)) +#define TIMER_DISABLE_PWM (TCCR3A &= ~(_BV(COM3A1))) +#define TIMER_ENABLE_INTR (TIMSK3 = _BV(OCIE3A)) +#define TIMER_DISABLE_INTR (TIMSK3 = 0) +#define TIMER_INTR_NAME TIMER3_COMPA_vect +#define TIMER_CONFIG_KHZ(val) ({ \ + const uint16_t pwmval = SYSCLOCK / 2000 / (val); \ + TCCR3A = _BV(WGM31); \ + TCCR3B = _BV(WGM33) | _BV(CS30); \ + ICR3 = pwmval; \ + OCR3A = pwmval / 3; \ +}) +#define TIMER_CONFIG_NORMAL() ({ \ + TCCR3A = 0; \ + TCCR3B = _BV(WGM32) | _BV(CS30); \ + OCR3A = SYSCLOCK * USECPERTICK / 1000000; \ + TCNT3 = 0; \ +}) +#if defined(CORE_OC3A_PIN) +#define TIMER_PWM_PIN CORE_OC3A_PIN /* Teensy */ +#elif defined(__AVR_ATmega1280__) +#define TIMER_PWM_PIN 5 /* Arduino Mega */ +#elif defined(__AVR_ATmega32U4__) +#define TIMER_PWM_PIN 5 /* Arduino Leonardo */ // added by mem +#else +#error "Please add OC3A pin number here\n" +#endif + + +// defines for timer4 (10 bits, high speed option) +#elif defined(IR_USE_TIMER4_HS) +#define TIMER_RESET +#define TIMER_ENABLE_PWM (TCCR4A |= _BV(COM4A1)) +#define TIMER_DISABLE_PWM (TCCR4A &= ~(_BV(COM4A1))) +#define TIMER_ENABLE_INTR (TIMSK4 = _BV(TOIE4)) +#define TIMER_DISABLE_INTR (TIMSK4 = 0) +#define TIMER_INTR_NAME TIMER4_OVF_vect +#define TIMER_CONFIG_KHZ(val) ({ \ + const uint16_t pwmval = SYSCLOCK / 2000 / (val); \ + TCCR4A = (1<<PWM4A); \ + TCCR4B = _BV(CS40); \ + TCCR4C = 0; \ + TCCR4D = (1<<WGM40); \ + TCCR4E = 0; \ + TC4H = pwmval >> 8; \ + OCR4C = pwmval; \ + TC4H = (pwmval / 3) >> 8; \ + OCR4A = (pwmval / 3) & 255; \ +}) +#define TIMER_CONFIG_NORMAL() ({ \ + TCCR4A = 0; \ + TCCR4B = _BV(CS40); \ + TCCR4C = 0; \ + TCCR4D = 0; \ + TCCR4E = 0; \ + TC4H = (SYSCLOCK * USECPERTICK / 1000000) >> 8; \ + OCR4C = (SYSCLOCK * USECPERTICK / 1000000) & 255; \ + TC4H = 0; \ + TCNT4 = 0; \ +}) +#if defined(CORE_OC4A_PIN) +#define TIMER_PWM_PIN CORE_OC4A_PIN /* Teensy */ +#else +#error "Please add OC4A pin number here\n" +#endif + + +// defines for timer4 (16 bits) +#elif defined(IR_USE_TIMER4) +#define TIMER_RESET +#define TIMER_ENABLE_PWM (TCCR4A |= _BV(COM4A1)) +#define TIMER_DISABLE_PWM (TCCR4A &= ~(_BV(COM4A1))) +#define TIMER_ENABLE_INTR (TIMSK4 = _BV(OCIE4A)) +#define TIMER_DISABLE_INTR (TIMSK4 = 0) +#define TIMER_INTR_NAME TIMER4_COMPA_vect +#define TIMER_CONFIG_KHZ(val) ({ \ + const uint16_t pwmval = SYSCLOCK / 2000 / (val); \ + TCCR4A = _BV(WGM41); \ + TCCR4B = _BV(WGM43) | _BV(CS40); \ + ICR4 = pwmval; \ + OCR4A = pwmval / 3; \ +}) +#define TIMER_CONFIG_NORMAL() ({ \ + TCCR4A = 0; \ + TCCR4B = _BV(WGM42) | _BV(CS40); \ + OCR4A = SYSCLOCK * USECPERTICK / 1000000; \ + TCNT4 = 0; \ +}) +#if defined(CORE_OC4A_PIN) +#define TIMER_PWM_PIN CORE_OC4A_PIN +#elif defined(__AVR_ATmega1280__) +#define TIMER_PWM_PIN 6 /* Arduino Mega */ +#elif defined(__AVR_ATmega32U4__) +#define TIMER_PWM_PIN 5 /* Arduino Leonardo */ // added by mem (check pin number) +#else +#error "Please add OC4A pin number here\n" +#endif + + +// defines for timer5 (16 bits) +#elif defined(IR_USE_TIMER5) +#define TIMER_RESET +#define TIMER_ENABLE_PWM (TCCR5A |= _BV(COM5A1)) +#define TIMER_DISABLE_PWM (TCCR5A &= ~(_BV(COM5A1))) +#define TIMER_ENABLE_INTR (TIMSK5 = _BV(OCIE5A)) +#define TIMER_DISABLE_INTR (TIMSK5 = 0) +#define TIMER_INTR_NAME TIMER5_COMPA_vect +#define TIMER_CONFIG_KHZ(val) ({ \ + const uint16_t pwmval = SYSCLOCK / 2000 / (val); \ + TCCR5A = _BV(WGM51); \ + TCCR5B = _BV(WGM53) | _BV(CS50); \ + ICR5 = pwmval; \ + OCR5A = pwmval / 3; \ +}) +#define TIMER_CONFIG_NORMAL() ({ \ + TCCR5A = 0; \ + TCCR5B = _BV(WGM52) | _BV(CS50); \ + OCR5A = SYSCLOCK * USECPERTICK / 1000000; \ + TCNT5 = 0; \ +}) +#if defined(CORE_OC5A_PIN) +#define TIMER_PWM_PIN CORE_OC5A_PIN +#elif defined(__AVR_ATmega1280__) +#define TIMER_PWM_PIN 46 /* Arduino Mega */ +#else +#error "Please add OC5A pin number here\n" +#endif + + +#else // unknown timer +#error "Internal code configuration error, no known IR_USE_TIMER# defined\n" +#endif + + +// defines for blinking the LED +#if defined(CORE_LED0_PIN) +#define BLINKLED CORE_LED0_PIN +#define BLINKLED_ON() (digitalWrite(CORE_LED0_PIN, HIGH)) +#define BLINKLED_OFF() (digitalWrite(CORE_LED0_PIN, LOW)) +#elif defined(__AVR_ATmega1280__) +#define BLINKLED 13 +#define BLINKLED_ON() (PORTB |= B10000000) +#define BLINKLED_OFF() (PORTB &= B01111111) +#elif defined(__AVR_ATmega644P__) || defined(__AVR_ATmega644__) +#define BLINKLED 0 +#define BLINKLED_ON() (PORTD |= B00000001) +#define BLINKLED_OFF() (PORTD &= B11111110) +#else +#define BLINKLED 13 +#define BLINKLED_ON() (PORTB |= B00100000) +#define BLINKLED_OFF() (PORTB &= B11011111) +#endif + +#endif diff --git a/libraries/IRremote/LICENSE.txt b/libraries/IRremote/LICENSE.txt new file mode 100755 index 0000000..77cec6d --- /dev/null +++ b/libraries/IRremote/LICENSE.txt @@ -0,0 +1,458 @@ + + GNU LESSER GENERAL PUBLIC LICENSE + Version 2.1, February 1999 + + Copyright (C) 1991, 1999 Free Software Foundation, Inc. + 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + Everyone is permitted to copy and distribute verbatim copies + of this license document, but changing it is not allowed. + +[This is the first released version of the Lesser GPL. It also counts + as the successor of the GNU Library Public License, version 2, hence + the version number 2.1.] + + Preamble + + The licenses for most software are designed to take away your +freedom to share and change it. By contrast, the GNU General Public +Licenses are intended to guarantee your freedom to share and change +free software--to make sure the software is free for all its users. + + This license, the Lesser General Public License, applies to some +specially designated software packages--typically libraries--of the +Free Software Foundation and other authors who decide to use it. You +can use it too, but we suggest you first think carefully about whether +this license or the ordinary General Public License is the better +strategy to use in any particular case, based on the explanations below. + + When we speak of free software, we are referring to freedom of use, +not price. Our General Public Licenses are designed to make sure that +you have the freedom to distribute copies of free software (and charge +for this service if you wish); that you receive source code or can get +it if you want it; that you can change the software and use pieces of +it in new free programs; and that you are informed that you can do +these things. + + To protect your rights, we need to make restrictions that forbid +distributors to deny you these rights or to ask you to surrender these +rights. These restrictions translate to certain responsibilities for +you if you distribute copies of the library or if you modify it. + + For example, if you distribute copies of the library, whether gratis +or for a fee, you must give the recipients all the rights that we gave +you. You must make sure that they, too, receive or can get the source +code. If you link other code with the library, you must provide +complete object files to the recipients, so that they can relink them +with the library after making changes to the library and recompiling +it. And you must show them these terms so they know their rights. + + We protect your rights with a two-step method: (1) we copyright the +library, and (2) we offer you this license, which gives you legal +permission to copy, distribute and/or modify the library. + + To protect each distributor, we want to make it very clear that +there is no warranty for the free library. Also, if the library is +modified by someone else and passed on, the recipients should know +that what they have is not the original version, so that the original +author's reputation will not be affected by problems that might be +introduced by others. + + Finally, software patents pose a constant threat to the existence of +any free program. We wish to make sure that a company cannot +effectively restrict the users of a free program by obtaining a +restrictive license from a patent holder. Therefore, we insist that +any patent license obtained for a version of the library must be +consistent with the full freedom of use specified in this license. + + Most GNU software, including some libraries, is covered by the +ordinary GNU General Public License. This license, the GNU Lesser +General Public License, applies to certain designated libraries, and +is quite different from the ordinary General Public License. We use +this license for certain libraries in order to permit linking those +libraries into non-free programs. + + When a program is linked with a library, whether statically or using +a shared library, the combination of the two is legally speaking a +combined work, a derivative of the original library. The ordinary +General Public License therefore permits such linking only if the +entire combination fits its criteria of freedom. The Lesser General +Public License permits more lax criteria for linking other code with +the library. + + We call this license the "Lesser" General Public License because it +does Less to protect the user's freedom than the ordinary General +Public License. It also provides other free software developers Less +of an advantage over competing non-free programs. These disadvantages +are the reason we use the ordinary General Public License for many +libraries. However, the Lesser license provides advantages in certain +special circumstances. + + For example, on rare occasions, there may be a special need to +encourage the widest possible use of a certain library, so that it becomes +a de-facto standard. To achieve this, non-free programs must be +allowed to use the library. A more frequent case is that a free +library does the same job as widely used non-free libraries. In this +case, there is little to gain by limiting the free library to free +software only, so we use the Lesser General Public License. + + In other cases, permission to use a particular library in non-free +programs enables a greater number of people to use a large body of +free software. For example, permission to use the GNU C Library in +non-free programs enables many more people to use the whole GNU +operating system, as well as its variant, the GNU/Linux operating +system. + + Although the Lesser General Public License is Less protective of the +users' freedom, it does ensure that the user of a program that is +linked with the Library has the freedom and the wherewithal to run +that program using a modified version of the Library. + + The precise terms and conditions for copying, distribution and +modification follow. Pay close attention to the difference between a +"work based on the library" and a "work that uses the library". The +former contains code derived from the library, whereas the latter must +be combined with the library in order to run. + + GNU LESSER GENERAL PUBLIC LICENSE + TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION + + 0. This License Agreement applies to any software library or other +program which contains a notice placed by the copyright holder or +other authorized party saying it may be distributed under the terms of +this Lesser General Public License (also called "this License"). +Each licensee is addressed as "you". + + A "library" means a collection of software functions and/or data +prepared so as to be conveniently linked with application programs +(which use some of those functions and data) to form executables. + + The "Library", below, refers to any such software library or work +which has been distributed under these terms. A "work based on the +Library" means either the Library or any derivative work under +copyright law: that is to say, a work containing the Library or a +portion of it, either verbatim or with modifications and/or translated +straightforwardly into another language. (Hereinafter, translation is +included without limitation in the term "modification".) + + "Source code" for a work means the preferred form of the work for +making modifications to it. For a library, complete source code means +all the source code for all modules it contains, plus any associated +interface definition files, plus the scripts used to control compilation +and installation of the library. + + Activities other than copying, distribution and modification are not +covered by this License; they are outside its scope. The act of +running a program using the Library is not restricted, and output from +such a program is covered only if its contents constitute a work based +on the Library (independent of the use of the Library in a tool for +writing it). Whether that is true depends on what the Library does +and what the program that uses the Library does. + + 1. You may copy and distribute verbatim copies of the Library's +complete source code as you receive it, in any medium, provided that +you conspicuously and appropriately publish on each copy an +appropriate copyright notice and disclaimer of warranty; keep intact +all the notices that refer to this License and to the absence of any +warranty; and distribute a copy of this License along with the +Library. + + You may charge a fee for the physical act of transferring a copy, +and you may at your option offer warranty protection in exchange for a +fee. + + 2. You may modify your copy or copies of the Library or any portion +of it, thus forming a work based on the Library, and copy and +distribute such modifications or work under the terms of Section 1 +above, provided that you also meet all of these conditions: + + a) The modified work must itself be a software library. + + b) You must cause the files modified to carry prominent notices + stating that you changed the files and the date of any change. + + c) You must cause the whole of the work to be licensed at no + charge to all third parties under the terms of this License. + + d) If a facility in the modified Library refers to a function or a + table of data to be supplied by an application program that uses + the facility, other than as an argument passed when the facility + is invoked, then you must make a good faith effort to ensure that, + in the event an application does not supply such function or + table, the facility still operates, and performs whatever part of + its purpose remains meaningful. + + (For example, a function in a library to compute square roots has + a purpose that is entirely well-defined independent of the + application. Therefore, Subsection 2d requires that any + application-supplied function or table used by this function must + be optional: if the application does not supply it, the square + root function must still compute square roots.) + +These requirements apply to the modified work as a whole. If +identifiable sections of that work are not derived from the Library, +and can be reasonably considered independent and separate works in +themselves, then this License, and its terms, do not apply to those +sections when you distribute them as separate works. But when you +distribute the same sections as part of a whole which is a work based +on the Library, the distribution of the whole must be on the terms of +this License, whose permissions for other licensees extend to the +entire whole, and thus to each and every part regardless of who wrote +it. + +Thus, it is not the intent of this section to claim rights or contest +your rights to work written entirely by you; rather, the intent is to +exercise the right to control the distribution of derivative or +collective works based on the Library. + +In addition, mere aggregation of another work not based on the Library +with the Library (or with a work based on the Library) on a volume of +a storage or distribution medium does not bring the other work under +the scope of this License. + + 3. You may opt to apply the terms of the ordinary GNU General Public +License instead of this License to a given copy of the Library. To do +this, you must alter all the notices that refer to this License, so +that they refer to the ordinary GNU General Public License, version 2, +instead of to this License. (If a newer version than version 2 of the +ordinary GNU General Public License has appeared, then you can specify +that version instead if you wish.) Do not make any other change in +these notices. + + Once this change is made in a given copy, it is irreversible for +that copy, so the ordinary GNU General Public License applies to all +subsequent copies and derivative works made from that copy. + + This option is useful when you wish to copy part of the code of +the Library into a program that is not a library. + + 4. You may copy and distribute the Library (or a portion or +derivative of it, under Section 2) in object code or executable form +under the terms of Sections 1 and 2 above provided that you accompany +it with the complete corresponding machine-readable source code, which +must be distributed under the terms of Sections 1 and 2 above on a +medium customarily used for software interchange. + + If distribution of object code is made by offering access to copy +from a designated place, then offering equivalent access to copy the +source code from the same place satisfies the requirement to +distribute the source code, even though third parties are not +compelled to copy the source along with the object code. + + 5. A program that contains no derivative of any portion of the +Library, but is designed to work with the Library by being compiled or +linked with it, is called a "work that uses the Library". Such a +work, in isolation, is not a derivative work of the Library, and +therefore falls outside the scope of this License. + + However, linking a "work that uses the Library" with the Library +creates an executable that is a derivative of the Library (because it +contains portions of the Library), rather than a "work that uses the +library". The executable is therefore covered by this License. +Section 6 states terms for distribution of such executables. + + When a "work that uses the Library" uses material from a header file +that is part of the Library, the object code for the work may be a +derivative work of the Library even though the source code is not. +Whether this is true is especially significant if the work can be +linked without the Library, or if the work is itself a library. The +threshold for this to be true is not precisely defined by law. + + If such an object file uses only numerical parameters, data +structure layouts and accessors, and small macros and small inline +functions (ten lines or less in length), then the use of the object +file is unrestricted, regardless of whether it is legally a derivative +work. (Executables containing this object code plus portions of the +Library will still fall under Section 6.) + + Otherwise, if the work is a derivative of the Library, you may +distribute the object code for the work under the terms of Section 6. +Any executables containing that work also fall under Section 6, +whether or not they are linked directly with the Library itself. + + 6. As an exception to the Sections above, you may also combine or +link a "work that uses the Library" with the Library to produce a +work containing portions of the Library, and distribute that work +under terms of your choice, provided that the terms permit +modification of the work for the customer's own use and reverse +engineering for debugging such modifications. + + You must give prominent notice with each copy of the work that the +Library is used in it and that the Library and its use are covered by +this License. You must supply a copy of this License. If the work +during execution displays copyright notices, you must include the +copyright notice for the Library among them, as well as a reference +directing the user to the copy of this License. Also, you must do one +of these things: + + a) Accompany the work with the complete corresponding + machine-readable source code for the Library including whatever + changes were used in the work (which must be distributed under + Sections 1 and 2 above); and, if the work is an executable linked + with the Library, with the complete machine-readable "work that + uses the Library", as object code and/or source code, so that the + user can modify the Library and then relink to produce a modified + executable containing the modified Library. (It is understood + that the user who changes the contents of definitions files in the + Library will not necessarily be able to recompile the application + to use the modified definitions.) + + b) Use a suitable shared library mechanism for linking with the + Library. A suitable mechanism is one that (1) uses at run time a + copy of the library already present on the user's computer system, + rather than copying library functions into the executable, and (2) + will operate properly with a modified version of the library, if + the user installs one, as long as the modified version is + interface-compatible with the version that the work was made with. + + c) Accompany the work with a written offer, valid for at + least three years, to give the same user the materials + specified in Subsection 6a, above, for a charge no more + than the cost of performing this distribution. + + d) If distribution of the work is made by offering access to copy + from a designated place, offer equivalent access to copy the above + specified materials from the same place. + + e) Verify that the user has already received a copy of these + materials or that you have already sent this user a copy. + + For an executable, the required form of the "work that uses the +Library" must include any data and utility programs needed for +reproducing the executable from it. However, as a special exception, +the materials to be distributed need not include anything that is +normally distributed (in either source or binary form) with the major +components (compiler, kernel, and so on) of the operating system on +which the executable runs, unless that component itself accompanies +the executable. + + It may happen that this requirement contradicts the license +restrictions of other proprietary libraries that do not normally +accompany the operating system. Such a contradiction means you cannot +use both them and the Library together in an executable that you +distribute. + + 7. You may place library facilities that are a work based on the +Library side-by-side in a single library together with other library +facilities not covered by this License, and distribute such a combined +library, provided that the separate distribution of the work based on +the Library and of the other library facilities is otherwise +permitted, and provided that you do these two things: + + a) Accompany the combined library with a copy of the same work + based on the Library, uncombined with any other library + facilities. This must be distributed under the terms of the + Sections above. + + b) Give prominent notice with the combined library of the fact + that part of it is a work based on the Library, and explaining + where to find the accompanying uncombined form of the same work. + + 8. You may not copy, modify, sublicense, link with, or distribute +the Library except as expressly provided under this License. Any +attempt otherwise to copy, modify, sublicense, link with, or +distribute the Library is void, and will automatically terminate your +rights under this License. However, parties who have received copies, +or rights, from you under this License will not have their licenses +terminated so long as such parties remain in full compliance. + + 9. You are not required to accept this License, since you have not +signed it. However, nothing else grants you permission to modify or +distribute the Library or its derivative works. These actions are +prohibited by law if you do not accept this License. Therefore, by +modifying or distributing the Library (or any work based on the +Library), you indicate your acceptance of this License to do so, and +all its terms and conditions for copying, distributing or modifying +the Library or works based on it. + + 10. Each time you redistribute the Library (or any work based on the +Library), the recipient automatically receives a license from the +original licensor to copy, distribute, link with or modify the Library +subject to these terms and conditions. You may not impose any further +restrictions on the recipients' exercise of the rights granted herein. +You are not responsible for enforcing compliance by third parties with +this License. + + 11. If, as a consequence of a court judgment or allegation of patent +infringement or for any other reason (not limited to patent issues), +conditions are imposed on you (whether by court order, agreement or +otherwise) that contradict the conditions of this License, they do not +excuse you from the conditions of this License. If you cannot +distribute so as to satisfy simultaneously your obligations under this +License and any other pertinent obligations, then as a consequence you +may not distribute the Library at all. For example, if a patent +license would not permit royalty-free redistribution of the Library by +all those who receive copies directly or indirectly through you, then +the only way you could satisfy both it and this License would be to +refrain entirely from distribution of the Library. + +If any portion of this section is held invalid or unenforceable under any +particular circumstance, the balance of the section is intended to apply, +and the section as a whole is intended to apply in other circumstances. + +It is not the purpose of this section to induce you to infringe any +patents or other property right claims or to contest validity of any +such claims; this section has the sole purpose of protecting the +integrity of the free software distribution system which is +implemented by public license practices. Many people have made +generous contributions to the wide range of software distributed +through that system in reliance on consistent application of that +system; it is up to the author/donor to decide if he or she is willing +to distribute software through any other system and a licensee cannot +impose that choice. + +This section is intended to make thoroughly clear what is believed to +be a consequence of the rest of this License. + + 12. If the distribution and/or use of the Library is restricted in +certain countries either by patents or by copyrighted interfaces, the +original copyright holder who places the Library under this License may add +an explicit geographical distribution limitation excluding those countries, +so that distribution is permitted only in or among countries not thus +excluded. In such case, this License incorporates the limitation as if +written in the body of this License. + + 13. The Free Software Foundation may publish revised and/or new +versions of the Lesser General Public License from time to time. +Such new versions will be similar in spirit to the present version, +but may differ in detail to address new problems or concerns. + +Each version is given a distinguishing version number. If the Library +specifies a version number of this License which applies to it and +"any later version", you have the option of following the terms and +conditions either of that version or of any later version published by +the Free Software Foundation. If the Library does not specify a +license version number, you may choose any version ever published by +the Free Software Foundation. + + 14. If you wish to incorporate parts of the Library into other free +programs whose distribution conditions are incompatible with these, +write to the author to ask for permission. For software which is +copyrighted by the Free Software Foundation, write to the Free +Software Foundation; we sometimes make exceptions for this. Our +decision will be guided by the two goals of preserving the free status +of all derivatives of our free software and of promoting the sharing +and reuse of software generally. + + NO WARRANTY + + 15. BECAUSE THE LIBRARY IS LICENSED FREE OF CHARGE, THERE IS NO +WARRANTY FOR THE LIBRARY, TO THE EXTENT PERMITTED BY APPLICABLE LAW. +EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR +OTHER PARTIES PROVIDE THE LIBRARY "AS IS" WITHOUT WARRANTY OF ANY +KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE +LIBRARY IS WITH YOU. SHOULD THE LIBRARY PROVE DEFECTIVE, YOU ASSUME +THE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION. + + 16. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN +WRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY +AND/OR REDISTRIBUTE THE LIBRARY AS PERMITTED ABOVE, BE LIABLE TO YOU +FOR DAMAGES, INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR +CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OR INABILITY TO USE THE +LIBRARY (INCLUDING BUT NOT LIMITED TO LOSS OF DATA OR DATA BEING +RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A +FAILURE OF THE LIBRARY TO OPERATE WITH ANY OTHER SOFTWARE), EVEN IF +SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH +DAMAGES. + diff --git a/libraries/IRremote/examples/IRrecord/IRrecord.pde b/libraries/IRremote/examples/IRrecord/IRrecord.pde new file mode 100755 index 0000000..f27bd80 --- /dev/null +++ b/libraries/IRremote/examples/IRrecord/IRrecord.pde @@ -0,0 +1,174 @@ +/* + * IRrecord: record and play back IR signals as a minimal + * An IR detector/demodulator must be connected to the input RECV_PIN. + * An IR LED must be connected to the output PWM pin 3. + * A button must be connected to the input BUTTON_PIN; this is the + * send button. + * A visible LED can be connected to STATUS_PIN to provide status. + * + * The logic is: + * If the button is pressed, send the IR code. + * If an IR code is received, record it. + * + * Version 0.11 September, 2009 + * Copyright 2009 Ken Shirriff + * http://arcfn.com + */ + +#include <IRremote.h> + +int RECV_PIN = 11; +int BUTTON_PIN = 12; +int STATUS_PIN = 13; + +IRrecv irrecv(RECV_PIN); +IRsend irsend; + +decode_results results; + +void setup() +{ + Serial.begin(9600); + irrecv.enableIRIn(); // Start the receiver + pinMode(BUTTON_PIN, INPUT); + pinMode(STATUS_PIN, OUTPUT); +} + +// Storage for the recorded code +int codeType = -1; // The type of code +unsigned long codeValue; // The code value if not raw +unsigned int rawCodes[RAWBUF]; // The durations if raw +int codeLen; // The length of the code +int toggle = 0; // The RC5/6 toggle state + +// Stores the code for later playback +// Most of this code is just logging +void storeCode(decode_results *results) { + codeType = results->decode_type; + int count = results->rawlen; + if (codeType == UNKNOWN) { + Serial.println("Received unknown code, saving as raw"); + codeLen = results->rawlen - 1; + // To store raw codes: + // Drop first value (gap) + // Convert from ticks to microseconds + // Tweak marks shorter, and spaces longer to cancel out IR receiver distortion + for (int i = 1; i <= codeLen; i++) { + if (i % 2) { + // Mark + rawCodes[i - 1] = results->rawbuf[i]*USECPERTICK - MARK_EXCESS; + Serial.print(" m"); + } + else { + // Space + rawCodes[i - 1] = results->rawbuf[i]*USECPERTICK + MARK_EXCESS; + Serial.print(" s"); + } + Serial.print(rawCodes[i - 1], DEC); + } + Serial.println(""); + } + else { + if (codeType == NEC) { + Serial.print("Received NEC: "); + if (results->value == REPEAT) { + // Don't record a NEC repeat value as that's useless. + Serial.println("repeat; ignoring."); + return; + } + } + else if (codeType == SONY) { + Serial.print("Received SONY: "); + } + else if (codeType == RC5) { + Serial.print("Received RC5: "); + } + else if (codeType == RC6) { + Serial.print("Received RC6: "); + } + else { + Serial.print("Unexpected codeType "); + Serial.print(codeType, DEC); + Serial.println(""); + } + Serial.println(results->value, HEX); + codeValue = results->value; + codeLen = results->bits; + } +} + +void sendCode(int repeat) { + if (codeType == NEC) { + if (repeat) { + irsend.sendNEC(REPEAT, codeLen); + Serial.println("Sent NEC repeat"); + } + else { + irsend.sendNEC(codeValue, codeLen); + Serial.print("Sent NEC "); + Serial.println(codeValue, HEX); + } + } + else if (codeType == SONY) { + irsend.sendSony(codeValue, codeLen); + Serial.print("Sent Sony "); + Serial.println(codeValue, HEX); + } + else if (codeType == RC5 || codeType == RC6) { + if (!repeat) { + // Flip the toggle bit for a new button press + toggle = 1 - toggle; + } + // Put the toggle bit into the code to send + codeValue = codeValue & ~(1 << (codeLen - 1)); + codeValue = codeValue | (toggle << (codeLen - 1)); + if (codeType == RC5) { + Serial.print("Sent RC5 "); + Serial.println(codeValue, HEX); + irsend.sendRC5(codeValue, codeLen); + } + else { + irsend.sendRC6(codeValue, codeLen); + Serial.print("Sent RC6 "); + Serial.println(codeValue, HEX); + } + } + else if (codeType == UNKNOWN /* i.e. raw */) { + // Assume 38 KHz + irsend.sendRaw(rawCodes, codeLen, 38); + Serial.println("Sent raw"); + } +} + +int lastButtonState; + +void loop() { + // If button pressed, send the code. + int buttonState = digitalRead(BUTTON_PIN); + if (lastButtonState == HIGH && buttonState == LOW) { + Serial.println("Released"); + irrecv.enableIRIn(); // Re-enable receiver + } + + if (buttonState) { + Serial.println("Pressed, sending"); + digitalWrite(STATUS_PIN, HIGH); + sendCode(lastButtonState == buttonState); + digitalWrite(STATUS_PIN, LOW); + delay(50); // Wait a bit between retransmissions + } + else if (irrecv.decode(&results)) { + digitalWrite(STATUS_PIN, HIGH); + storeCode(&results); + irrecv.resume(); // resume receiver + digitalWrite(STATUS_PIN, LOW); + } + lastButtonState = buttonState; +} + + + + + + + diff --git a/libraries/IRremote/examples/IRrecvDemo/IRrecvDemo.pde b/libraries/IRremote/examples/IRrecvDemo/IRrecvDemo.pde new file mode 100755 index 0000000..f7b45b8 --- /dev/null +++ b/libraries/IRremote/examples/IRrecvDemo/IRrecvDemo.pde @@ -0,0 +1,28 @@ +/* + * IRremote: IRrecvDemo - demonstrates receiving IR codes with IRrecv + * An IR detector/demodulator must be connected to the input RECV_PIN. + * Version 0.1 July, 2009 + * Copyright 2009 Ken Shirriff + * http://arcfn.com + */ + +#include <IRremote.h> + +int RECV_PIN = 11; + +IRrecv irrecv(RECV_PIN); + +decode_results results; + +void setup() +{ + Serial.begin(9600); + irrecv.enableIRIn(); // Start the receiver +} + +void loop() { + if (irrecv.decode(&results)) { + Serial.println(results.value, HEX); + irrecv.resume(); // Receive the next value + } +} diff --git a/libraries/IRremote/examples/IRrecvDump/IRrecvDump.pde b/libraries/IRremote/examples/IRrecvDump/IRrecvDump.pde new file mode 100755 index 0000000..2c18895 --- /dev/null +++ b/libraries/IRremote/examples/IRrecvDump/IRrecvDump.pde @@ -0,0 +1,74 @@ +/* + * IRremote: IRrecvDump - dump details of IR codes with IRrecv + * An IR detector/demodulator must be connected to the input RECV_PIN. + * Version 0.1 July, 2009 + * Copyright 2009 Ken Shirriff + * http://arcfn.com + */ + +#include <IRremote.h> + +int RECV_PIN = 11; + +IRrecv irrecv(RECV_PIN); + +decode_results results; + +void setup() +{ + Serial.begin(9600); + irrecv.enableIRIn(); // Start the receiver +} + +// Dumps out the decode_results structure. +// Call this after IRrecv::decode() +// void * to work around compiler issue +//void dump(void *v) { +// decode_results *results = (decode_results *)v +void dump(decode_results *results) { + int count = results->rawlen; + if (results->decode_type == UNKNOWN) { + Serial.println("Could not decode message"); + } + else { + if (results->decode_type == NEC) { + Serial.print("Decoded NEC: "); + } + else if (results->decode_type == SONY) { + Serial.print("Decoded SONY: "); + } + else if (results->decode_type == RC5) { + Serial.print("Decoded RC5: "); + } + else if (results->decode_type == RC6) { + Serial.print("Decoded RC6: "); + } + Serial.print(results->value, HEX); + Serial.print(" ("); + Serial.print(results->bits, DEC); + Serial.println(" bits)"); + } + Serial.print("Raw ("); + Serial.print(count, DEC); + Serial.print("): "); + + for (int i = 0; i < count; i++) { + if ((i % 2) == 1) { + Serial.print(results->rawbuf[i]*USECPERTICK, DEC); + } + else { + Serial.print(-(int)results->rawbuf[i]*USECPERTICK, DEC); + } + Serial.print(" "); + } + Serial.println(""); +} + + +void loop() { + if (irrecv.decode(&results)) { + Serial.println(results.value, HEX); + dump(&results); + irrecv.resume(); // Receive the next value + } +} diff --git a/libraries/IRremote/examples/IRrelay/IRrelay.pde b/libraries/IRremote/examples/IRrelay/IRrelay.pde new file mode 100755 index 0000000..046fb5f --- /dev/null +++ b/libraries/IRremote/examples/IRrelay/IRrelay.pde @@ -0,0 +1,85 @@ +/* + * IRremote: IRrecvDemo - demonstrates receiving IR codes with IRrecv + * An IR detector/demodulator must be connected to the input RECV_PIN. + * Version 0.1 July, 2009 + * Copyright 2009 Ken Shirriff + * http://arcfn.com + */ + +#include <IRremote.h> + +int RECV_PIN = 11; +int RELAY_PIN = 4; + +IRrecv irrecv(RECV_PIN); +decode_results results; + +// Dumps out the decode_results structure. +// Call this after IRrecv::decode() +// void * to work around compiler issue +//void dump(void *v) { +// decode_results *results = (decode_results *)v +void dump(decode_results *results) { + int count = results->rawlen; + if (results->decode_type == UNKNOWN) { + Serial.println("Could not decode message"); + } + else { + if (results->decode_type == NEC) { + Serial.print("Decoded NEC: "); + } + else if (results->decode_type == SONY) { + Serial.print("Decoded SONY: "); + } + else if (results->decode_type == RC5) { + Serial.print("Decoded RC5: "); + } + else if (results->decode_type == RC6) { + Serial.print("Decoded RC6: "); + } + Serial.print(results->value, HEX); + Serial.print(" ("); + Serial.print(results->bits, DEC); + Serial.println(" bits)"); + } + Serial.print("Raw ("); + Serial.print(count, DEC); + Serial.print("): "); + + for (int i = 0; i < count; i++) { + if ((i % 2) == 1) { + Serial.print(results->rawbuf[i]*USECPERTICK, DEC); + } + else { + Serial.print(-(int)results->rawbuf[i]*USECPERTICK, DEC); + } + Serial.print(" "); + } + Serial.println(""); +} + +void setup() +{ + pinMode(RELAY_PIN, OUTPUT); + pinMode(13, OUTPUT); + Serial.begin(9600); + irrecv.enableIRIn(); // Start the receiver +} + +int on = 0; +unsigned long last = millis(); + +void loop() { + if (irrecv.decode(&results)) { + // If it's been at least 1/4 second since the last + // IR received, toggle the relay + if (millis() - last > 250) { + on = !on; + digitalWrite(RELAY_PIN, on ? HIGH : LOW); + digitalWrite(13, on ? HIGH : LOW); + dump(&results); + } + last = millis(); + irrecv.resume(); // Receive the next value + } +} diff --git a/libraries/IRremote/examples/IRsendDemo/IRsendDemo.pde b/libraries/IRremote/examples/IRsendDemo/IRsendDemo.pde new file mode 100755 index 0000000..6941382 --- /dev/null +++ b/libraries/IRremote/examples/IRsendDemo/IRsendDemo.pde @@ -0,0 +1,26 @@ +/* + * IRremote: IRsendDemo - demonstrates sending IR codes with IRsend + * An IR LED must be connected to Arduino PWM pin 3. + * Version 0.1 July, 2009 + * Copyright 2009 Ken Shirriff + * http://arcfn.com + */ + +#include <IRremote.h> + +IRsend irsend; + +void setup() +{ + Serial.begin(9600); +} + +void loop() { + if (Serial.read() != -1) { + for (int i = 0; i < 3; i++) { + irsend.sendSony(0xa90, 12); // Sony TV power code + delay(100); + } + } +} + diff --git a/libraries/IRremote/examples/IRtest/IRtest.pde b/libraries/IRremote/examples/IRtest/IRtest.pde new file mode 100755 index 0000000..4845a4a --- /dev/null +++ b/libraries/IRremote/examples/IRtest/IRtest.pde @@ -0,0 +1,190 @@ +/* + * IRremote: IRtest unittest + * Version 0.1 July, 2009 + * Copyright 2009 Ken Shirriff + * http://arcfn.com + * + * Note: to run these tests, edit IRremote/IRremote.h to add "#define TEST" + * You must then recompile the library by removing IRremote.o and restarting + * the arduino IDE. + */ + +#include <IRremote.h> +#include <IRremoteInt.h> + +// Dumps out the decode_results structure. +// Call this after IRrecv::decode() +// void * to work around compiler issue +//void dump(void *v) { +// decode_results *results = (decode_results *)v +void dump(decode_results *results) { + int count = results->rawlen; + if (results->decode_type == UNKNOWN) { + Serial.println("Could not decode message"); + } + else { + if (results->decode_type == NEC) { + Serial.print("Decoded NEC: "); + } + else if (results->decode_type == SONY) { + Serial.print("Decoded SONY: "); + } + else if (results->decode_type == RC5) { + Serial.print("Decoded RC5: "); + } + else if (results->decode_type == RC6) { + Serial.print("Decoded RC6: "); + } + Serial.print(results->value, HEX); + Serial.print(" ("); + Serial.print(results->bits, DEC); + Serial.println(" bits)"); + } + Serial.print("Raw ("); + Serial.print(count, DEC); + Serial.print("): "); + + for (int i = 0; i < count; i++) { + if ((i % 2) == 1) { + Serial.print(results->rawbuf[i]*USECPERTICK, DEC); + } + else { + Serial.print(-(int)results->rawbuf[i]*USECPERTICK, DEC); + } + Serial.print(" "); + } + Serial.println(""); +} + +IRrecv irrecv(0); +decode_results results; + +class IRsendDummy : +public IRsend +{ +public: + // For testing, just log the marks/spaces +#define SENDLOG_LEN 128 + int sendlog[SENDLOG_LEN]; + int sendlogcnt; + IRsendDummy() : + IRsend() { + } + void reset() { + sendlogcnt = 0; + } + void mark(int time) { + sendlog[sendlogcnt] = time; + if (sendlogcnt < SENDLOG_LEN) sendlogcnt++; + } + void space(int time) { + sendlog[sendlogcnt] = -time; + if (sendlogcnt < SENDLOG_LEN) sendlogcnt++; + } + // Copies the dummy buf into the interrupt buf + void useDummyBuf() { + int last = SPACE; + irparams.rcvstate = STATE_STOP; + irparams.rawlen = 1; // Skip the gap + for (int i = 0 ; i < sendlogcnt; i++) { + if (sendlog[i] < 0) { + if (last == MARK) { + // New space + irparams.rawbuf[irparams.rawlen++] = (-sendlog[i] - MARK_EXCESS) / USECPERTICK; + last = SPACE; + } + else { + // More space + irparams.rawbuf[irparams.rawlen - 1] += -sendlog[i] / USECPERTICK; + } + } + else if (sendlog[i] > 0) { + if (last == SPACE) { + // New mark + irparams.rawbuf[irparams.rawlen++] = (sendlog[i] + MARK_EXCESS) / USECPERTICK; + last = MARK; + } + else { + // More mark + irparams.rawbuf[irparams.rawlen - 1] += sendlog[i] / USECPERTICK; + } + } + } + if (irparams.rawlen % 2) { + irparams.rawlen--; // Remove trailing space + } + } +}; + +IRsendDummy irsenddummy; + +void verify(unsigned long val, int bits, int type) { + irsenddummy.useDummyBuf(); + irrecv.decode(&results); + Serial.print("Testing "); + Serial.print(val, HEX); + if (results.value == val && results.bits == bits && results.decode_type == type) { + Serial.println(": OK"); + } + else { + Serial.println(": Error"); + dump(&results); + } +} + +void testNEC(unsigned long val, int bits) { + irsenddummy.reset(); + irsenddummy.sendNEC(val, bits); + verify(val, bits, NEC); +} +void testSony(unsigned long val, int bits) { + irsenddummy.reset(); + irsenddummy.sendSony(val, bits); + verify(val, bits, SONY); +} +void testRC5(unsigned long val, int bits) { + irsenddummy.reset(); + irsenddummy.sendRC5(val, bits); + verify(val, bits, RC5); +} +void testRC6(unsigned long val, int bits) { + irsenddummy.reset(); + irsenddummy.sendRC6(val, bits); + verify(val, bits, RC6); +} + +void test() { + Serial.println("NEC tests"); + testNEC(0x00000000, 32); + testNEC(0xffffffff, 32); + testNEC(0xaaaaaaaa, 32); + testNEC(0x55555555, 32); + testNEC(0x12345678, 32); + Serial.println("Sony tests"); + testSony(0xfff, 12); + testSony(0x000, 12); + testSony(0xaaa, 12); + testSony(0x555, 12); + testSony(0x123, 12); + Serial.println("RC5 tests"); + testRC5(0xfff, 12); + testRC5(0x000, 12); + testRC5(0xaaa, 12); + testRC5(0x555, 12); + testRC5(0x123, 12); + Serial.println("RC6 tests"); + testRC6(0xfffff, 20); + testRC6(0x00000, 20); + testRC6(0xaaaaa, 20); + testRC6(0x55555, 20); + testRC6(0x12345, 20); +} + +void setup() +{ + Serial.begin(9600); + test(); +} + +void loop() { +} diff --git a/libraries/IRremote/keywords.txt b/libraries/IRremote/keywords.txt new file mode 100755 index 0000000..19ec63d --- /dev/null +++ b/libraries/IRremote/keywords.txt @@ -0,0 +1,37 @@ +####################################### +# Syntax Coloring Map For IRremote +####################################### + +####################################### +# Datatypes (KEYWORD1) +####################################### + +decode_results KEYWORD1 +IRrecv KEYWORD1 +IRsend KEYWORD1 + +####################################### +# Methods and Functions (KEYWORD2) +####################################### + +blink13 KEYWORD2 +decode KEYWORD2 +enableIRIn KEYWORD2 +resume KEYWORD2 +enableIROut KEYWORD2 +sendNEC KEYWORD2 +sendSony KEYWORD2 +sendRaw KEYWORD2 +sendRC5 KEYWORD2 +sendRC6 KEYWORD2 +# +####################################### +# Constants (LITERAL1) +####################################### + +NEC LITERAL1 +SONY LITERAL1 +RC5 LITERAL1 +RC6 LITERAL1 +UNKNOWN LITERAL1 +REPEAT LITERAL1 diff --git a/libraries/RobotMotor/ArduinoShieldR3/RobotMotor.cpp b/libraries/RobotMotor/ArduinoShieldR3/RobotMotor.cpp new file mode 100755 index 0000000..0f20a04 --- /dev/null +++ b/libraries/RobotMotor/ArduinoShieldR3/RobotMotor.cpp @@ -0,0 +1,75 @@ +/******************************************************* + RobotMotor.cpp // Arduino Motor Shield R3 version + low level motor driver for use with arduino Motor Shield + + Michael Margolis Sept 17 2012 +********************************************************/ + +#include <Arduino.h> +#include "RobotMotor.h" + +const int differential = 0; // % faster left motor turns compared to right + +/****** motor pin defines *************/ +// Pins connected to the motor driver. The PWM pins control the speed, and the +// other pins are select forward and reverse + +// Motor pins : B, A +const byte M_PWM_PIN[2] = {11,3}; // speed) +const byte M_DIR_PIN[2] = {13,12}; // Dir +const byte M_BRAKE_PIN[2] = {8,9}; // brake +/* end of motor pin defines */ + +int motorSpeed[2] = {0,0}; // motor speed stored here (0-100%) + +// tables hold time in ms to rotate robot 360 degrees at various speeds +// this enables conversion of rotation angle into timed motor movement +// The speeds are percent of max speed +// Note: low cost motors do not have enough torque at low speeds so +// the robot will not move below this value +// Interpolation is used to get a time for any speed from MIN_SPEED to 100% + +const int MIN_SPEED = 40; // first table entry is 40% speed +const int SPEED_TABLE_INTERVAL = 10; // each table entry is 10% faster speed +const int NBR_SPEEDS = 1 + (100 - MIN_SPEED)/ SPEED_TABLE_INTERVAL; + +int speedTable[NBR_SPEEDS] = {40, 50, 60, 70, 80, 90, 100}; // speeds +int rotationTime[NBR_SPEEDS] = {5500, 3300, 2400, 2000, 1750, 1550, 1150}; // time + +void motorBegin(int motor) +{ + pinMode(M_DIR_PIN[motor], OUTPUT); + pinMode(M_BRAKE_PIN[motor], OUTPUT); + motorStop(motor); +} + +// speed range is 0 to 100 +void motorSetSpeed(int motor, int speed) +{ + motorSpeed[motor] = speed; // save the value + speed = map(speed, 0,100, 0,255); // scale to PWM range + analogWrite(M_PWM_PIN[motor], speed); // write the value +} + +void motorForward(int motor, int speed) +{ + digitalWrite(M_DIR_PIN[motor], HIGH); + motorSetSpeed(motor, speed); +} + +void motorReverse(int motor, int speed) +{ + digitalWrite(M_DIR_PIN[motor], LOW); + motorSetSpeed(motor, speed); +} + +void motorStop(int motor) +{ + analogWrite(M_PWM_PIN[motor], 0); +} + +void motorBrake(int motor) +{ + analogWrite(M_PWM_PIN[motor], 0); + digitalWrite(M_BRAKE_PIN[motor], HIGH); +}
\ No newline at end of file diff --git a/libraries/RobotMotor/Ardumoto/RobotMotor.cpp b/libraries/RobotMotor/Ardumoto/RobotMotor.cpp new file mode 100755 index 0000000..90b579f --- /dev/null +++ b/libraries/RobotMotor/Ardumoto/RobotMotor.cpp @@ -0,0 +1,73 @@ +/******************************************************* + RobotMotor.cpp // Ardumoto version + low level motor driver for use with ardumoto motor shield and 2WD robot + + Michael Margolis May 8 2012 +********************************************************/ + +#include <Arduino.h> +#include "RobotMotor.h" + +const int differential = 0; // % faster left motor turns compared to right + +/****** motor pin defines *************/ +// Pins connected to the motor driver. The PWM pins control the speed, and the +// other pins are select forward and reverse + +// Motor uses pins : 3,11,12,13 +const byte M_PWM_PIN[2] = {11,3}; // ardumoto v13 +const byte M_DIR_PIN[2] = {13,12}; +/* end of motor pin defines */ + +int motorSpeed[2] = {0,0}; // motor speed stored here (0-100%) + +// tables hold time in ms to rotate robot 360 degrees at various speeds +// this enables conversion of rotation angle into timed motor movement +// The speeds are percent of max speed +// Note: low cost motors do not have enough torque at low speeds so +// the robot will not move below this value +// Interpolation is used to get a time for any speed from MIN_SPEED to 100% + +const int MIN_SPEED = 40; // first table entry is 40% speed +const int SPEED_TABLE_INTERVAL = 10; // each table entry is 10% faster speed +const int NBR_SPEEDS = 1 + (100 - MIN_SPEED)/ SPEED_TABLE_INTERVAL; + +int speedTable[NBR_SPEEDS] = {40, 50, 60, 70, 80, 90, 100}; // speeds +int rotationTime[NBR_SPEEDS] = {5500, 3300, 2400, 2000, 1750, 1550, 1150}; // time + +void motorBegin(int motor) +{ + pinMode(M_DIR_PIN[motor], OUTPUT); + motorStop(motor); +} + +// speed range is 0 to 100 +void motorSetSpeed(int motor, int speed) +{ + motorSpeed[motor] = speed; // save the value + speed = map(speed, 0,100, 0,255); // scale to PWM range + analogWrite(M_PWM_PIN[motor], speed); // write the value +} + +void motorForward(int motor, int speed) +{ + digitalWrite(M_DIR_PIN[motor], HIGH); + motorSetSpeed(motor, speed); +} + +void motorReverse(int motor, int speed) +{ + digitalWrite(M_DIR_PIN[motor], LOW); + motorSetSpeed(motor, speed); +} + +void motorStop(int motor) +{ + analogWrite(M_PWM_PIN[motor], 0); +} + +void motorBrake(int motor) +{ + // Ardumoto does not support brake, so just stop the motor + analogWrite(M_PWM_PIN[motor], 0); +}
\ No newline at end of file diff --git a/libraries/RobotMotor/CR_Servo/RobotMotor.cpp b/libraries/RobotMotor/CR_Servo/RobotMotor.cpp new file mode 100755 index 0000000..3df8b1a --- /dev/null +++ b/libraries/RobotMotor/CR_Servo/RobotMotor.cpp @@ -0,0 +1,76 @@ +/******************************************************* + RobotMotor.cpp // continuous rotation servo version + + low level motor driver for use with continuous rotation servos and 2WD robot + + Copyright Michael Margolis May 8 2012 +********************************************************/ + +#include <Arduino.h> +#include <Servo.h> +#include "RobotMotor.h" + +Servo myservo[2]; // create instances for two servos + +const int MAX_ANGLE = 60; // number of degrees that motor driven at max speed +const int servoPins[2] = {7,8}; // digital pins connected to servos:(left,right) + + // change sign to reverse direction of the motor +int motorSense[2] = {1,-1}; // 1 increases angle for forward, -1 decreaes + +int motorStopAngle[2] = {90,90}; // inc or dec so motor stops when motorStop is called + +int motorSpeed[2] = {0,0}; // left and right motor speeds stored here (0-100%) + +// tables hold time in ms to rotate robot 360 degrees at various speeds +// this enables conversion of rotation angle into timed motor movement +// The speeds are percent of max speed +// Note: low cost motors do not have enough torque at low speeds so +// the robot will not move below this value +// Interpolation is used to get a time for any speed from MIN_SPEED to 100% + +const int MIN_SPEED = 40; // first table entry is 40% speed +const int SPEED_TABLE_INTERVAL = 10; // each table entry is 10% faster speed +const int NBR_SPEEDS = 1 + (100 - MIN_SPEED)/ SPEED_TABLE_INTERVAL; + +int speedTable[NBR_SPEEDS] = {40, 50, 60, 70, 80, 90, 100}; // speeds +int rotationTime[NBR_SPEEDS] = {5500, 3300, 2400, 2000, 1750, 1550, 1150}; // time + +void motorBegin(int motor) +{ + myservo[motor].attach(servoPins[motor]); +} + +// speed range is 0 to 100 +void motorSetSpeed(int motor, int speed) +{ + motorSpeed[motor] = speed; // save the value +} + +void motorForward(int motor, int speed) +{ + motorSetSpeed(motor, speed); + int stopAngle = motorStopAngle[motor]; + int maxSpeedAngle = stopAngle + (MAX_ANGLE * motorSense[motor]); + int angle = map(speed, 0,100, stopAngle, maxSpeedAngle); + myservo[motor].write(angle); +} + +void motorReverse(int motor, int speed) +{ + motorSetSpeed(motor, speed); + int stopAngle = motorStopAngle[motor]; + int maxSpeedAngle = stopAngle - (MAX_ANGLE * motorSense[motor]); + int angle = map(speed, 0,100, stopAngle, maxSpeedAngle); + myservo[motor].write(angle); +} + +void motorStop(int motor) +{ + myservo[motor].write(motorStopAngle[motor]); +} + +void motorBrake(int motor) +{ + myservo[motor].write(motorStopAngle[motor]); +}
\ No newline at end of file diff --git a/libraries/RobotMotor/RobotMotor.cpp b/libraries/RobotMotor/RobotMotor.cpp new file mode 100755 index 0000000..3f0213b --- /dev/null +++ b/libraries/RobotMotor/RobotMotor.cpp @@ -0,0 +1,114 @@ +/******************************************************* + RobotMotor.cpp // Adafruit version for 2WD and 4WD chassis + low level motor driver for use with adafruit motor shield + + Motor constants used are defined AFMotor.h + + Copyright Michael Margolis May 8 2012 +********************************************************/ + +#include <Arduino.h> +#include <AFMotor.h> // adafruit motor shield library +#include "RobotMotor.h" + + +const int differential = 0; // % faster left motor turns compared to right + +// tables hold time in ms to rotate robot 360 degrees at various speeds +// this enables conversion of rotation angle into timed motor movement +// The speeds are percent of max speed +// Note: low cost motors do not have enough torque at low speeds so +// the robot will not move below this value +// Interpolation is used to get a time for any speed from MIN_SPEED to 100% + +// constants for 2 wheeled robot chassis +#if defined CHASSIS_2WD +const int MIN_SPEED = 40; // first table entry is 40% speed +const int SPEED_TABLE_INTERVAL = 10; // each table entry is 10% faster speed +const int NBR_SPEEDS = 1 + (100 - MIN_SPEED)/ SPEED_TABLE_INTERVAL; + +int speedTable[NBR_SPEEDS] = {40, 50, 60, 70, 80, 90, 100}; // speeds +int rotationTime[NBR_SPEEDS] = {5500, 3300, 2400, 2000, 1750, 1550, 1150}; // time + +AF_DCMotor motors[] = { + AF_DCMotor(1, MOTOR12_1KHZ), // left is Motor #1 + AF_DCMotor(2, MOTOR12_1KHZ) // right is Motor #2 + }; +// constants for 4 wheeled robot +#elif defined CHASSIS_4WD +const int MIN_SPEED = 60; // first table entry is 60% speed +const int SPEED_TABLE_INTERVAL = 10; // each table entry is 10% faster speed +const int NBR_SPEEDS = 1 + (100 - MIN_SPEED)/ SPEED_TABLE_INTERVAL; + +int speedTable[NBR_SPEEDS] = {60, 70, 80, 90, 100}; // speeds +int rotationTime[NBR_SPEEDS] = {5500, 3300, 2400, 2000, 1750}; // time + +AF_DCMotor motors[] = { + AF_DCMotor(4, MOTOR34_1KHZ), // left front is Motor #4 + AF_DCMotor(3, MOTOR34_1KHZ), // right front is Motor #3 + AF_DCMotor(1, MOTOR12_1KHZ), // left rear is Motor #1 + AF_DCMotor(2, MOTOR12_1KHZ) // right rear is Motor #2 +}; + +#else +#error "expected definition: CHASSIS_2WD or CHASSIS_4WD not found" +#endif + +int motorSpeed[2] = {0,0}; // left and right motor speeds stored here (0-100%) + +void motorBegin(int motor) +{ + motorStop(motor); // stop the front motor +#if defined CHASSIS_4WD + motorStop(motor+2); // stop the rear motor +#endif +} + +// speed range is 0 to 100 percent +void motorSetSpeed(int motor, int speed) +{ + if( motor == MOTOR_LEFT && speed > differential) + speed -= differential; + motorSpeed[motor] = speed; // save the value + int pwm = map(speed, 0,100, 0,255); // scale to PWM range + + motors[motor].setSpeed(pwm) ; +#if defined CHASSIS_4WD + motors[motor+2].setSpeed(pwm) ; +#endif +} + +void motorForward(int motor, int speed) +{ + motorSetSpeed(motor, speed); + motors[motor].run(FORWARD); +#if defined CHASSIS_4WD + motors[motor+2].run(FORWARD); +#endif +} + +void motorReverse(int motor, int speed) +{ + motorSetSpeed(motor, speed); + motors[motor].run(BACKWARD); +#if defined CHASSIS_4WD + motors[motor+2].run(BACKWARD); +#endif +} + +void motorStop(int motor) +{ + // todo set speed to 0 ??? + motors[motor].run(RELEASE); // stopped +#if defined CHASSIS_4WD + motors[motor+2].run(RELEASE); +#endif +} + +void motorBrake(int motor) +{ + motors[motor].run(BRAKE); // stopped +#if defined CHASSIS_4WD + motors[motor+2].run(BRAKE); +#endif +} diff --git a/libraries/RobotMotor/RobotMotor.h b/libraries/RobotMotor/RobotMotor.h new file mode 100755 index 0000000..d626e17 --- /dev/null +++ b/libraries/RobotMotor/RobotMotor.h @@ -0,0 +1,37 @@ +/******************************************************* + RobotMotor.h + low level motor driver interface + + Copyright Michael Margolis May 8 2012 +********************************************************/ + +/* if you have the 4WD chassis, change the line: + #define CHASSIS_2WD + to: + #define CHASSIS_4WD + */ + +#define CHASSIS_2WD // change suffix from 2WD to 4WD if using the 4WD chassis + +// defines for left and right motors +const int MOTOR_LEFT = 0; +const int MOTOR_RIGHT = 1; + +extern const int MIN_SPEED; +extern int speedTable[]; +extern int rotationTime[]; +extern const int SPEED_TABLE_INTERVAL; +extern const int NBR_SPEEDS; + +void motorBegin(int motor); + +// speed range is 0 to 100 percent +void motorSetSpeed(int motor, int speed); + +void motorForward(int motor, int speed); + +void motorReverse(int motor, int speed); + +void motorStop(int motor); + +void motorBrake(int motor);
\ No newline at end of file diff --git a/libraries/RobotMotor/RobotMotor2wd/RobotMotor.cpp b/libraries/RobotMotor/RobotMotor2wd/RobotMotor.cpp new file mode 100755 index 0000000..caa9939 --- /dev/null +++ b/libraries/RobotMotor/RobotMotor2wd/RobotMotor.cpp @@ -0,0 +1,74 @@ +/******************************************************* + RobotMotor.cpp // Adafruit 2WD version + low level motor driver for use with adafruit motor shield and 2WD robot + + Motor constants used are defined AFMotor.h + + Copyright Michael Margolis May 8 2012 +********************************************************/ + +#include <Arduino.h> +#include <AFMotor.h> // adafruit motor shield library +#include "RobotMotor.h" + +const int differential = 0; // % faster left motor turns compared to right + +// tables hold time in ms to rotate robot 360 degrees at various speeds +// this enables conversion of rotation angle into timed motor movement +// The speeds are percent of max speed +// Note: low cost motors do not have enough torque at low speeds so +// the robot will not move below this value +// Interpolation is used to get a time for any speed from MIN_SPEED to 100% + +const int MIN_SPEED = 40; // first table entry is 40% speed +const int SPEED_TABLE_INTERVAL = 10; // each table entry is 10% faster speed +const int NBR_SPEEDS = 1 + (100 - MIN_SPEED)/ SPEED_TABLE_INTERVAL; + +int speedTable[NBR_SPEEDS] = {40, 50, 60, 70, 80, 90, 100}; // speeds +int rotationTime[NBR_SPEEDS] = {5500, 3300, 2400, 2000, 1750, 1550, 1150}; // time + +AF_DCMotor motors[] = { + AF_DCMotor(1, MOTOR12_1KHZ), // left is Motor #1 + AF_DCMotor(2, MOTOR12_1KHZ) // right is Motor #2 + }; + +int motorSpeed[2] = {0,0}; // left and right motor speeds stored here (0-100%) + +void motorBegin(int motor) +{ + motorStop(motor); +} + +// speed range is 0 to 100 percent +void motorSetSpeed(int motor, int speed) +{ + if( motor == MOTOR_LEFT && speed > differential) + speed -= differential; + motorSpeed[motor] = speed; // save the value + int pwm = map(speed, 0,100, 0,255); // scale to PWM range + + motors[motor].setSpeed(pwm) ; +} + +void motorForward(int motor, int speed) +{ + motorSetSpeed(motor, speed); + motors[motor].run(FORWARD); +} + +void motorReverse(int motor, int speed) +{ + motorSetSpeed(motor, speed); + motors[motor].run(BACKWARD); +} + +void motorStop(int motor) +{ + // todo set speed to 0 ??? + motors[motor].run(RELEASE); // stopped +} + +void motorBrake(int motor) +{ + motors[motor].run(BRAKE); // stopped +}
\ No newline at end of file diff --git a/libraries/RobotMotor/RobotMotor4wd/RobotMotor.cpp b/libraries/RobotMotor/RobotMotor4wd/RobotMotor.cpp new file mode 100755 index 0000000..bdf2d1a --- /dev/null +++ b/libraries/RobotMotor/RobotMotor4wd/RobotMotor.cpp @@ -0,0 +1,82 @@ +/******************************************************* + RobotMotor.cpp // Adafruit 4WD version + low level motor driver for use with adafruit motor shield and 4WD robot + + Motor constants used are defined AFMotor.h + + Copyright Michael Margolis May 8 2012 +********************************************************/ + +#include <Arduino.h> +#include <AFMotor.h> // adafruit motor shield library +#include "RobotMotor.h" + +const int differential = 0; // % faster left motor turns compared to right + +// tables hold time in ms to rotate robot 360 degrees at various speeds +// this enables conversion of rotation angle into timed motor movement +// The speeds are percent of max speed +// Note: low cost motors do not have enough torque at low speeds so +// the robot will not move below this value +// Interpolation is used to get a time for any speed from MIN_SPEED to 100% + +const int MIN_SPEED = 60; // first table entry is 60% speed +const int SPEED_TABLE_INTERVAL = 10; // each table entry is 10% faster speed +const int NBR_SPEEDS = 1 + (100 - MIN_SPEED)/ SPEED_TABLE_INTERVAL; + +int speedTable[NBR_SPEEDS] = {60, 70, 80, 90, 100}; // speeds +int rotationTime[NBR_SPEEDS] = {5500, 3300, 2400, 2000, 1750}; // time + +AF_DCMotor motors[] = { + AF_DCMotor(4, MOTOR34_1KHZ), // left front is Motor #4 + AF_DCMotor(3, MOTOR34_1KHZ), // right front is Motor #3 + AF_DCMotor(1, MOTOR12_1KHZ), // left rear is Motor #1 + AF_DCMotor(2, MOTOR12_1KHZ) // right rear is Motor #2 + }; + +int motorSpeed[2] = {0,0}; // left and right motor speeds stored here (0-100%) + +void motorBegin(int motor) +{ + motorStop(motor); // stop the front motor + motorStop(motor+2); // stop the rear motor +} + +// speed range is 0 to 100 percent +void motorSetSpeed(int motor, int speed) +{ + if( motor == MOTOR_LEFT && speed > differential) + speed -= differential; + motorSpeed[motor] = speed; // save the value + int pwm = map(speed, 0,100, 0,255); // scale to PWM range + + motors[motor].setSpeed(pwm) ; + motors[motor+2].setSpeed(pwm) ; +} + +void motorForward(int motor, int speed) +{ + motorSetSpeed(motor, speed); + motors[motor].run(FORWARD); + motors[motor+2].run(FORWARD); +} + +void motorReverse(int motor, int speed) +{ + motorSetSpeed(motor, speed); + motors[motor].run(BACKWARD); + motors[motor+2].run(BACKWARD); +} + +void motorStop(int motor) +{ + // todo set speed to 0 ??? + motors[motor].run(RELEASE); // stopped + motors[motor+2].run(RELEASE); +} + +void motorBrake(int motor) +{ + motors[motor].run(BRAKE); // stopped + motors[motor+2].run(BRAKE); +}
\ No newline at end of file diff --git a/libraries/RobotMotor/RobotMotor4wd/RobotMotor.h b/libraries/RobotMotor/RobotMotor4wd/RobotMotor.h new file mode 100755 index 0000000..452a1e0 --- /dev/null +++ b/libraries/RobotMotor/RobotMotor4wd/RobotMotor.h @@ -0,0 +1,37 @@ +/******************************************************* + RobotMotor.h + low level motor driver interface + + Copyright Michael Margolis May 8 2012 +********************************************************/ + +/* if you have the 4WD chassis, change the line: + #define CHASSIS_2WD + to: + #define CHASSIS_4WD + */ + +#define CHASSIS_4WD // change suffix from 2WD to 4WD if using the 4WD chassis + +// defines for left and right motors +const int MOTOR_LEFT = 0; +const int MOTOR_RIGHT = 1; + +extern const int MIN_SPEED; +extern int speedTable[]; +extern int rotationTime[]; +extern const int SPEED_TABLE_INTERVAL; +extern const int NBR_SPEEDS; + +void motorBegin(int motor); + +// speed range is 0 to 100 percent +void motorSetSpeed(int motor, int speed); + +void motorForward(int motor, int speed); + +void motorReverse(int motor, int speed); + +void motorStop(int motor); + +void motorBrake(int motor);
\ No newline at end of file diff --git a/libraries/robotDefines/robotDefines.h b/libraries/robotDefines/robotDefines.h new file mode 100755 index 0000000..e9165ff --- /dev/null +++ b/libraries/robotDefines/robotDefines.h @@ -0,0 +1,25 @@ + +/***** Global Defines ****/ + +// defines to identify sensors +const int SENSE_IR_LEFT = 0; +const int SENSE_IR_RIGHT = 1; + +// defines for directions +const int DIR_LEFT = 0; +const int DIR_RIGHT = 1; +const int DIR_CENTER = 2; + +const char* locationString[] = {"Left", "Right", "Center"}; // Debug labels +// http://arduino.cc/en/Reference/String for more on character string arrays + +// obstacles constants +const int OBST_NONE = 0; // no obstacle detected +const int OBST_LEFT_EDGE = 1; // left edge detected +const int OBST_RIGHT_EDGE = 2; // right edge detected +const int OBST_FRONT_EDGE = 3; // edge detect at both left and right sensors + +const int LED_PIN = 13; + +/**** End of Global Defines ****************/ + diff --git a/sequenced_led/sequenced_led.ino b/sequenced_led/sequenced_led.ino index 565e376..8df81a9 100644 --- a/sequenced_led/sequenced_led.ino +++ b/sequenced_led/sequenced_led.ino @@ -7,22 +7,31 @@ void setup() { } } -void fade(int pin) { +void fade(int pin, int length = 30, int hold = 1000, int times = 1) { int brightness = 0; int fadeAmount = 7; + int done_times = 0; bool up = true; while (true) { analogWrite(pin, brightness); brightness = brightness + fadeAmount; // wait - delay(30); + delay(length); if (brightness <= 1 || brightness >= 252) { fadeAmount = -fadeAmount; up = !up; - if (up) { - analogWrite(pin, 0); // turn off the light - break; + if (up) { //reached bottom, going back up again. + done_times++; + // Serial.println("going back up?"); + if (done_times == times) { + analogWrite(pin, 0); // turn off the light + break; + } + else { // reached peak, going down + // Serial.println("Delaying."); + delay(hold); + } } } } @@ -30,10 +39,13 @@ void fade(int pin) { void loop() { for(int i = 0; i <= 2; i++) { - // Serial.print(i); - // Serial.print(" | "); + int delay = 30; + switch (i) { + case 0 : fade(pins[i], 50, 2000); break; + case 1 : fade(pins[i], 5, 200, 5); break; + case 2 : fade(pins[i], 60, 2000); break; + } + // Serial.print("Fading pin "); // Serial.println(pins[i]); - // analogWrite(pins[i], 254); - fade(pins[i]); } } |