Commit 713fcffb authored by O'Reilly Media, Inc's avatar O'Reilly Media, Inc

Initial commit

parents
/****************************************************
HelloRobot.ino: Initial Robot test sketch
Michael Margolis 4 July 2012
*****************************************************/
// include motor libraries
#include <AFMotor.h> // adafruit motor shield library
#include <RobotMotor.h> // 2wd or 4wd motor library
/***** Global Defines ****/
// defines to identify sensors
const int SENSE_IR_LEFT = 0;
const int SENSE_IR_RIGHT = 1;
const int SENSE_IR_CENTER = 2;
// 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 ****************/
// 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
}
}
/****************************
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 thresholds 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;
}
\ No newline at end of file
/*
* LearningRemote.cpp
*/
#include <IRremote.h> // IR remote control library
const int irPin = A3; // analog input pin 3 (digital 17)
const long NO_KEY = -1;
const long TIMEOUT = 5000; //max number of milliseconds to wait for a key (5 secs)
const int KEYCOUNT = 7; // the number of key codes supported
long irKeyCodes[ KEYCOUNT]; // this will store raw codes for all keys
char * remoteKeyNames[KEYCOUNT] =
{"Forward", "Back", "Left", "Right", "PivotCW", "PivotCCW", "Halt" };
// not used: Slower, Faster
IRrecv irrecv(irPin); // create the IR receive object
decode_results results; // ir data goes here
void setup()
{
Serial.begin(9600);
while(!Serial); // only needed for leonardo
irrecv.enableIRIn(); // Start the ir receiver
learnKeycodes();
printConstants();
Serial.println();
Serial.println("Now press the remote keys to verify correct detection");
}
void loop()
{
long key = getIrKeycode(TIMEOUT);
if( key!= NO_KEY)
{
int index = findKey(key);
if( index != NO_KEY)
{
Serial.println(remoteKeyNames[index]);
}
}
}
// get remote control codes
// the key map should be set to zero before calling this
void learnKeycodes()
{
Serial.println("Ready to learn remote codes");
for(int i = 0; i < KEYCOUNT; )
{
//delay(100);
Serial.println();
Serial.print("press remote key for ");
Serial.print( remoteKeyNames[i]);
long key = getIrKeycode(TIMEOUT);
if( key > 0 )
{
Serial.println(", release key ...");
long temp;
do {
temp = getIrKeycode(200);
}
while( temp == key);
if( findKey(key) == NO_KEY)
{
Serial.print(" -> using code ");
Serial.print( key );
Serial.print(" for ");
Serial.println(remoteKeyNames[i]);
irKeyCodes[i] = key;
i++;
}
else
{
Serial.println("key already assigned");
}
}
else continue;
}
Serial.println("Done\n");
}
// wait up to timeout milliseconds for a key
long getIrKeycode(long timeout)
{
flushKeys();
long key = NO_KEY;
unsigned long startTime = millis();
while( millis() - startTime < timeout )
{
if( irrecv.decode(&results) ) {
key = results.value;
//Serial.println(key, HEX);
irrecv.resume(); // Receive the next value
if(key != NO_KEY ) {
break;
}
}
}
return key;
}
//clear the buffer
void flushKeys()
{
while(irrecv.decode(&results))
irrecv.resume();
results.value = -1;
}
// returns the index for the given key code if found
// returns NO_KEY if code is not found
int findKey(long code)
{
for(int i=0; i < KEYCOUNT; i++ )
{
if(irKeyCodes[i] == code)
return i;
}
return NO_KEY;
}
void printConstants()
{
int i = 0;
Serial.println("//IR remote keycodes:");
Serial.print("const long IR_MOVE_FORWARD = "); Serial.print(irKeyCodes[i++]);
Serial.println(";");
Serial.print("const long IR_MOVE_BACK = "); Serial.print(irKeyCodes[i++]);
Serial.println(";");
Serial.print("const long IR_MOVE_LEFT = "); Serial.print(irKeyCodes[i++]);
Serial.println(";");
Serial.print("const long IR_MOVE_RIGHT = "); Serial.print(irKeyCodes[i++]);
Serial.println(";");
Serial.print("const long IR_PIVOT_CW = "); Serial.print(irKeyCodes[i++]);
Serial.println(";");
Serial.print("const long IR_PIVOT_CCW = "); Serial.print(irKeyCodes[i++]);
Serial.println(";");
Serial.print("const long IR_HALT = "); Serial.print(irKeyCodes[i++]);
Serial.println(";");
Serial.println(); Serial.println("Copy the above lines to the Remote tab");
}
/*
* LearningRemote.cpp
*
* captures remote commands and stores decoded values in EEPROM
*/
#include <EEPROM.h>
#include <IRremote.h> // IR remote control library
const int irPin = 14;
const long NO_KEY = -1;
const long TIMEOUT = 5000; //max number of milliseconds to wait for a key (5 secs)
const int KEYCOUNT = 7; // the number of key codes supported
// the following is for a temp user interface for learning mode
char * remoteKeyNames[KEYCOUNT] = {"Forward", "Back", "Left", "Right", "PivotCCW, PivotCW", "Halt" };
// not used: Slower, Faster
const char keyId[2] = {'i','r'};
const int eepromKeyMapHeader = 32;
const int eepromKeyMapHeaderSize = 3;
const int eepromKeyMap = eepromKeyMapHeader + eepromKeyMapHeaderSize ;
long irKeyMap[KEYCOUNT] ; // you can define default values here
IRrecv irrecv(irPin); // create the IR receive object
decode_results results; // ir data goes here
void setup()
{
Serial.begin(9600);
irrecv.enableIRIn(); // Start the ir receiver
learnKeycodes();
showRemoteCodes();
}
void loop()
{
long code = getIrKeycode(TIMEOUT);
int key = mapCodeToKey(code);
if( key >= 0)
Serial.println(remoteKeyNames[key]);
}
// wait up to timeout milliseconds for a key
long getIrKeycode(long timeout)
{
flushKeys();
long key = -1;
unsigned long startTime = millis();
while( millis() - startTime < timeout )
{
if( irrecv.decode(&results) ) {
key = results.value;
//Serial.println(key, HEX);
irrecv.resume(); // Receive the next value
if(key != -1 ) {
break;
}
}
}
return key;
}
//clear the buffer
void flushKeys()
{
while(irrecv.decode(&results))
irrecv.resume();
results.value = -1;
}
// returns the nuber of keys stored in eeprom
// returns 0 if no keys stored
int getStoredKeycount()
{
if( EEPROM.read(eepromKeyMap) == keyId[0] && EEPROM.read(eepromKeyMap+1) == keyId[1] )
return EEPROM.read( eepromKeyMap+2);
}
// get remote control codes
void learnKeycodes()
{
Serial.println("Ready to learn remote codes");
for(int i = 0; i < KEYCOUNT;)
{
delay(100);
Serial.println();
Serial.print("press remote key for ");
Serial.print( remoteKeyNames[i]);
long key = getIrKeycode(TIMEOUT);
long k = -1;
if( key != -1)
{
// Serial.println(key, HEX);
Serial.println(" release key ...");
do {
k = getIrKeycode(1000);
// Serial.println(k, HEX);
}
while( k == key);
Serial.println(" press key again");
if(getIrKeycode(TIMEOUT) == key)
{
Serial.print(" -> usi4ng 0x");
Serial.print( key, HEX);
Serial.print(" for ");
Serial.println(remoteKeyNames[i]);
irKeyMap[i] = key;
i++;
}
else
Serial.println("Keys did not match");
}
}
Serial.println("Learning complete\n");
saveRemoteCodes();
}
// for debug only
void showRemoteCodes()
{
Serial.println("Remote codes are:");
for(int i = 0; i < KEYCOUNT; i++)
{
Serial.print("key ");
Serial.print( remoteKeyNames[i]);
Serial.print(" = 0x");
Serial.println(irKeyMap[i], HEX);
}
}
void saveRemoteCodes()
{
byte *cfgPtr = (byte*)irKeyMap;
for(int i=0; i < sizeof(irKeyMap); i++)
EEPROM.write( eepromKeyMap + i, cfgPtr[i] );
}
void restoreRemoteCodes()
{
byte *cfgPtr = (byte*)irKeyMap;
for(int i=0; i < sizeof(irKeyMap); i++)
cfgPtr[i] = EEPROM.read( eepromKeyMap + i );
}
// returns a key index or NO_KEY ( -1) if no digit received
int getRemoteKeypress()
{
int ret = NO_KEY; // default
if (irrecv.decode(&results))
{
//Serial.println(results.value, HEX);
#ifdef IR_DEBUG
dumpRawRemoteData(&results);
#endif
irrecv.resume(); // Receive the next value
ret = mapCodeToKey(results.value);
}
return ret;
}
void dumpRawRemoteData(decode_results *results) {
int count = results->rawlen;
if(results->bits == 0)
return;
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("0x");
Serial.print(results->value, HEX);
Serial.print(" (");
Serial.print(results->bits, DEC);
Serial.print(" bits)");
}
#ifdef RAW_REMOTE_CTRL_DEBUG
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("");
#endif;
}
// converts an ir remote code to a logical key code (or NO_KEY if no digit received)
int mapCodeToKey(long code)
{
int key = NO_KEY; // no key pressed
if( code != REPEAT) // NEC protocol sends this when a button is held down
{
for( int i=0; i < KEYCOUNT; i++)
{
if( code == irKeyMap[i])
{
key = i;
break;
}
}
}
#ifdef IR_DEBUG
if( code != -1)
{
Serial.print("Code ");
Serial.print(code,HEX);
Serial.print(" mapped to key ");
Serial.println(key);
}
#endif
return key;
}
/*******************************************
* Basic motor test
* Copy AFMotor library and RobotMotor libraries to Arduino library folder
* Navigate to RobotMotor:
* for 4WD, copy RobotMotor4wd/RobotMotor.cpp into RobotMotor folder
* for 2WD, copy RobotMotor2wd/RobotMotor.cpp into RobotMotor fold
* Compile and run the sketch :
* robot rotates clockwise for 2 seconds
* pause 2 seconds
* robot rotates counter-clockwise for 2 seconds
* pause 2 seconds
* robot runs forward for 2 seconds
* pause 2 seconds
* robot runs backwards for 2 seconds
* pause 5 seconds
* Michael Margolis 24 July 2012
********************************************/
// include motor libraries
#include <AFMotor.h> // adafruit motor shield library
#include <RobotMotor.h> // 2wd or 4wd motor library
int moveSpeed = 60; // the speed to run (as a % of maximum speed)
int pauseMs = 2000; // time in ms for each movement
void setup()
{
Serial.begin(9600);
blinkNumber(8); // open port while flashing. Needed for Leonardo only
motorBegin(MOTOR_LEFT);
motorBegin(MOTOR_RIGHT);
test();
}
void loop()
{
}
// 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);
}
}
void test()
{
rotateLeft();
delay(pauseMs); // run for 2 seconds
stopAndWait(pauseMs);
rotateRight();
delay(pauseMs); // run for 2 seconds
stopAndWait(pauseMs);
moveForward();
delay(pauseMs); // run for 2 seconds
stopAndWait(pauseMs);
moveBackward();
delay(pauseMs); // run for 2 seconds
stopAndWait(pauseMs);
}
void moveForward()
{
Serial.println("forward");
motorForward(MOTOR_LEFT, moveSpeed);