/*
Wheeled-Robot Arduino Code
version 2012.12.12.09.32
Created 2012 08 21
by Basile Laderchi
Binary sketch size: 13654 bytes
controls:
motors arm leds analog test presets autoDrive
w y u i 1 2 ~ v p1-4 0
a s d h j k
x n m
+ - ,
Needed:
NewPing library: http://code.google.com/p/arduino-new-ping/
Useful things:
readAccelerometer code from: http://learn.parallax.com/kickstart/28017
readVcc code from: http://provideyourown.com/2012/secret-arduino-voltmeter-measure-battery-voltage/
longSharp_AnalogToCm code from: http://luckylarry.co.uk/arduino-projects/arduino-using-a-sharp-ir-sensor-for-distance-calculation/
*/
#include <Servo.h>
#include <NewPing.h>
const int centerMotorSpeed = 1500;
const int incdecMotorSpeed = 100;
const int minMotorSpeed = 300;
const int maxMotorSpeed = 500;
const int motorEnabledStatusDelay = 100;
const int minHeadLRPosition = 0;
const int maxHeadLRPosition = 180;
const int centerHeadLRPosition = 100;
const int incdecHeadLR = 10;
const int minHeadS1Position = 50;
const int maxHeadS1Position = 180;
const int centerHeadS1Position = 180;
const int incdecHeadS1 = 10;
const int minHeadS2Position = 0;
const int maxHeadS2Position = 180;
const int centerHeadS2Position = 150;
const int incdecHeadS2 = 10;
const int minHeadS3Position = 0;
const int maxHeadS3Position = 180;
const int centerHeadS3Position = 150;
const int incdecHeadS3 = 10;
const int maxSonarRange = 300;
const double headSweepSpeed = 0.001;
const int minDistance = 25;
const byte USB_PIN_D0 = 0;
const byte USB_PIN_D1 = 1;
const byte pinLed1 = 2;
const byte pinHeadS2 = 3;
const byte pinLed2 = 4;
const byte pinHeadS3 = 5;
const byte pinHeadLR = 6;
const byte pinAccelX = 7;
const byte pinAccelY = 8;
const byte pinMotorL = 9;
const byte pinMotorR = 10;
const byte pinHeadS1 = 11;
const byte pinMotorStop = 12;
const byte NOT_USED_D13 = 13;
const byte pinLeftSharp = A0;
const byte pinMiddleSharp = A1;
const byte pinRightSharp = A2;
const byte pinBackSharp = A3;
const byte pinSonar = A4;
const byte NOT_USED_A5 = A5;
const int armServos = 3;
/*
{140, 110, 120},
{ 60, 60, 170},
{ 90, 60, 140},
{ 60, 10, 130}
*/
const int armPresets[][armServos] = {
{140, 80, 120},
{ 60, 0, 140},
{ 90, 60, 110},
{160, 70, 110}
};
const int numArmPresets = (sizeof(armPresets) / armServos) / sizeof(int);
Servo motorL;
Servo motorR;
Servo headLR;
Servo headS1;
Servo headS2;
Servo headS3;
NewPing sonar = NewPing(pinSonar, pinSonar, maxSonarRange);
unsigned long headLRTimer = 0;
char dir = 's';
char preset = '_';
char lastMotorMovement = 's';
int headLRLastPosition = centerHeadLRPosition;
byte headLRSweepEnabled = 0;
int headLRRotation;
int headS1LastPosition = centerHeadS1Position;
int headS2LastPosition = centerHeadS2Position;
int headS3LastPosition = centerHeadS3Position;
double headLRSpeed = 0;
double headLRRange = 0;
int motorLLastSpeed = centerMotorSpeed;
int motorRLastSpeed = centerMotorSpeed;
int motorLastSpeed = minMotorSpeed;
int lastMotorEnabledStatus = LOW;
int motorEasingSpeed;
int lastLed1Status = LOW;
int lastLed2Status = LOW;
int lastLeftSharpValue = 0;
int lastMiddleSharpValue = 0;
int lastRightSharpValue = 0;
int lastBackSharpValue = 0;
unsigned int lastSonarValue = 0;
int lastAccelXValue = 0;
int lastAccelYValue = 0;
byte autoDriveEnabled = 0;
double Vcc;
void setup() {
Serial.begin(9600);
pinMode(pinLed1, OUTPUT);
pinMode(pinLed2, OUTPUT);
pinMode(pinAccelX, INPUT);
pinMode(pinAccelY, INPUT);
pinMode(pinMotorStop, OUTPUT);
motorL.attach(pinMotorL);
motorR.attach(pinMotorR);
headLR.attach(pinHeadLR);
headS1.attach(pinHeadS1);
headS2.attach(pinHeadS2);
headS3.attach(pinHeadS3);
motorStop();
headCenter();
Vcc = readVcc() / 1000.0;
}
void loop() {
if (Serial.available()) {
dir = Serial.read();
disableAutoDrive();
move(dir);
}
//oscillatingAutopilot();
readAccelerometer();
autoDrive();
}
void move(char dir) {
switch (dir) {
case 'w': // Forward
motorForward();
lastMotorMovement = 'w';
break;
case 'x': // Back
motorReverse();
lastMotorMovement = 'x';
break;
case 's': // Stop
motorStop();
lastMotorMovement = 's';
break;
case 'a': // Turn Left
motorTurnLeft();
lastMotorMovement = 'a';
break;
case 'd': // Turn Right
motorTurnRight();
lastMotorMovement = 'd';
break;
case 'y': // Head S1 Up
headS1Up();
break;
case 'h': // Head S1 Down
headS1Down();
break;
case 'u': // Head S2 Up
headS2Up();
break;
case 'j': // Head S2 Down
headS2Down();
break;
case 'i': // Head S3 Up
headS3Up();
break;
case 'k': // Head S3 Down
headS3Down();
break;
case 'n': // Head Left
headLeft();
break;
case 'm': // Head Right
headRight();
break;
case ',': // Head Center
case '<':
headCenter();
break;
case '+':
case '=':
increaseSpeed();
break;
case '-':
case '_':
decreaseSpeed();
break;
case '1':
led1();
break;
case '2':
led2();
break;
case '0':
enableAutoDrive();
case 'p':
presets();
break;
case '~':
case '`':
analog();
break;
case 'v':
// testHeadSweep();
break;
}
}
/*
void oscillatingAutopilot() {
if (headLRSweepEnabled == 1) {
headLRTimer++;
headLRRotation = sin(headLRTimer * headLRSpeed) * headLRRange;
Serial.print("Sweep: ");
Serial.print(headLRTimer);
Serial.print(", ");
Serial.print(headLRRotation);
if (headLR.read() != (centerHeadLRPosition + headLRRotation)) {
headLR.write(centerHeadLRPosition + headLRRotation);
Serial.print(", 1");
} else {
Serial.print(", 0");
}
Serial.println();
}
}
*/
void readAccelerometer() {
lastAccelXValue = pulseIn(pinAccelX, HIGH);
lastAccelYValue = pulseIn(pinAccelY, HIGH);
/*
Serial.print("Accelerometer: ");
Serial.print(lastAccelXValue);
Serial.print(",");
Serial.println(lastAccelYValue);
*/
}
void motorForward() {
motorChangeSpeed(motorLastSpeed, motorLastSpeed, LOW);
}
void motorReverse() {
motorChangeSpeed(- motorLastSpeed, - motorLastSpeed, LOW);
}
void motorStop() {
motorChangeSpeed(0, 0, HIGH);
}
void motorTurnLeft() {
motorChangeSpeed(- motorLastSpeed, motorLastSpeed, LOW);
}
void motorTurnRight() {
motorChangeSpeed(motorLastSpeed, - motorLastSpeed, LOW);
}
void motorChangeSpeed(int motorLSpeed, int motorRSpeed, int motorEnabledStatus) {
if (motorEnabledStatus == LOW) {
if (lastMotorEnabledStatus != motorEnabledStatus) {
digitalWrite(pinMotorStop, motorEnabledStatus);
delay(motorEnabledStatusDelay);
}
}
motorLLastSpeed = centerMotorSpeed + motorLSpeed;
motorRLastSpeed = centerMotorSpeed + motorRSpeed;
motorL.writeMicroseconds(motorLLastSpeed);
motorR.writeMicroseconds(motorRLastSpeed);
if (motorEnabledStatus == HIGH) {
if (lastMotorEnabledStatus != motorEnabledStatus) {
delay(motorEnabledStatusDelay);
digitalWrite(pinMotorStop, motorEnabledStatus);
}
}
lastMotorEnabledStatus = motorEnabledStatus;
}
void headS1Up() {
headS1LastPosition += incdecHeadS1;
headS1LastPosition = min(headS1LastPosition, maxHeadS1Position);
headS1.write(headS1LastPosition);
}
void headS1Down() {
headS1LastPosition -= incdecHeadS1;
headS1LastPosition = max(headS1LastPosition, minHeadS1Position);
headS1.write(headS1LastPosition);
}
void headS2Up() {
headS2LastPosition -= incdecHeadS2;
headS2LastPosition = max(headS2LastPosition, minHeadS2Position);
headS2.write(headS2LastPosition);
}
void headS2Down() {
headS2LastPosition += incdecHeadS2;
headS2LastPosition = min(headS2LastPosition, maxHeadS2Position);
headS2.write(headS2LastPosition);
}
void headS3Up() {
headS3LastPosition += incdecHeadS3;
headS3LastPosition = min(headS3LastPosition, maxHeadS3Position);
headS3.write(headS3LastPosition);
}
void headS3Down() {
headS3LastPosition -= incdecHeadS3;
headS3LastPosition = max(headS3LastPosition, minHeadS3Position);
headS3.write(headS3LastPosition);
}
void headCenter() {
headLRLastPosition = centerHeadLRPosition;
headLR.write(headLRLastPosition);
headS1LastPosition = centerHeadS1Position;
headS1.write(headS1LastPosition);
headS2LastPosition = centerHeadS2Position;
headS2.write(headS2LastPosition);
headS3LastPosition = centerHeadS3Position;
headS3.write(headS3LastPosition);
}
void headLeft() {
headLRSweepEnabled = 0;
headLRLastPosition -= incdecHeadLR;
headLRLastPosition = max(headLRLastPosition, minHeadLRPosition);
headLR.write(headLRLastPosition);
}
void headRight() {
headLRSweepEnabled = 0;
headLRLastPosition += incdecHeadLR;
headLRLastPosition = min(headLRLastPosition, maxHeadLRPosition);
headLR.write(headLRLastPosition);
}
void increaseSpeed() {
motorLastSpeed += incdecMotorSpeed;
motorLastSpeed = min(motorLastSpeed, maxMotorSpeed);
move(lastMotorMovement);
}
void decreaseSpeed() {
motorLastSpeed -= incdecMotorSpeed;
motorLastSpeed = max(motorLastSpeed, minMotorSpeed);
move(lastMotorMovement);
}
void led1() {
lastLed1Status = ~lastLed1Status;
digitalWrite(pinLed1, lastLed1Status);
}
void led2() {
lastLed2Status = ~lastLed2Status;
digitalWrite(pinLed2, lastLed2Status);
}
void presets() {
if (Serial.available()) {
preset = Serial.read();
headPreset(preset - '0');
}
}
void headPreset(int preset) {
if (preset > 0 && preset <= numArmPresets) {
headLRLastPosition = centerHeadLRPosition;
headLR.write(headLRLastPosition);
headS1LastPosition = armPresets[preset][0];
headS1.write(headS1LastPosition);
headS2LastPosition = armPresets[preset][1];
headS2.write(headS2LastPosition);
headS3LastPosition = armPresets[preset][2];
headS3.write(headS3LastPosition);
}
}
void enableAutoDrive() {
autoDriveEnabled = 1;
}
void disableAutoDrive() {
autoDriveEnabled = 0;
}
void autoDrive() {
if (autoDriveEnabled == 0) {
return;
}
int leftDistance,
middleDistance,
rightDistance;
byte autoDriveMode,
rnd;
readAnalogSensors();
leftDistance = longSharp_AnalogToCm(lastLeftSharpValue);
middleDistance = longSharp_AnalogToCm(lastMiddleSharpValue);
rightDistance = longSharp_AnalogToCm(lastRightSharpValue);
autoDriveMode = 0;
if (leftDistance >= minDistance &&
middleDistance >= minDistance &&
rightDistance >= minDistance) {
autoDriveMode = 1;
}
if (leftDistance < minDistance &&
middleDistance >= minDistance &&
rightDistance >= minDistance) {
autoDriveMode = 2;
}
if (leftDistance >= minDistance &&
middleDistance < minDistance &&
rightDistance >= minDistance) {
autoDriveMode = 4;
}
if (leftDistance >= minDistance &&
middleDistance >= minDistance &&
rightDistance < minDistance) {
autoDriveMode = 3;
}
if (leftDistance < minDistance &&
middleDistance < minDistance &&
rightDistance < minDistance) {
autoDriveMode = 4;
}
Serial.print("autoDrive =>");
Serial.print(" left: ");
Serial.print(lastLeftSharpValue);
Serial.print(" - ");
Serial.print(leftDistance);
Serial.print(" middle: ");
Serial.print(lastMiddleSharpValue);
Serial.print(" - ");
Serial.print(middleDistance);
Serial.print(" right: ");
Serial.print(lastRightSharpValue);
Serial.print(" - ");
Serial.print(rightDistance);
Serial.print(" mode: ");
Serial.println(autoDriveMode);
delay(100);
switch (autoDriveMode) {
case 1:
motorForward();
break;
case 2:
motorStop();
motorTurnRight();
break;
case 3:
motorStop();
motorTurnLeft();
break;
case 4:
motorStop();
motorReverse();
delay(500);
motorStop();
rnd = random(0, 2);
switch (rnd) {
case 0:
motorTurnLeft();
break;
case 1:
motorTurnRight();
break;
}
delay(100);
default:
motorStop();
break;
}
}
void analog() {
readAnalogSensors();
Serial.print("Left Sharp: ");
Serial.print(lastLeftSharpValue);
Serial.print(",");
Serial.println(longSharp_AnalogToCm(lastLeftSharpValue));
Serial.print("Middle Sharp: ");
Serial.print(lastMiddleSharpValue);
Serial.print(",");
Serial.println(longSharp_AnalogToCm(lastMiddleSharpValue));
Serial.print("Right Sharp: ");
Serial.print(lastRightSharpValue);
Serial.print(",");
Serial.println(longSharp_AnalogToCm(lastRightSharpValue));
Serial.print("Back Sharp: ");
Serial.print(lastBackSharpValue);
Serial.print(",");
Serial.println(longSharp_AnalogToCm(lastBackSharpValue));
Serial.print("Sonar: ");
Serial.print(lastSonarValue);
Serial.print(",");
Serial.println(sonar.convert_cm(lastSonarValue));
}
void readAnalogSensors() {
lastLeftSharpValue = analogRead(pinLeftSharp);
lastMiddleSharpValue = analogRead(pinMiddleSharp);
lastRightSharpValue = analogRead(pinRightSharp);
lastBackSharpValue = analogRead(pinBackSharp);
lastSonarValue = sonar.ping_median();
}
/*
void testHeadSweep() {
// set speed to 0 and it stops sweeping
// or set rot to 0 and speed to 0 and it centers
// sweep_speed should be about 0.01 for a start
if (headLRSweepEnabled == 0) {
headLRSweepEnabled = 1;
headLRTimer = 0;
headLRSpeed = headLRSweepSpeed;
headLRRange = 10;
Serial.println("Sweep Start");
} else {
headLRSweepEnabled = 0;
headLRSpeed = 0;
headLRRange = 0;
Serial.println("Sweep Stop");
}
}
*/
double longSharp_AnalogToCm(int valueToConvert) {
double valueVoltage;
double result;
valueVoltage = (valueToConvert / 1023.0) * Vcc;
result = 65 * pow(valueVoltage, -1.10);
return result;
}
long readVcc() {
// Read 1.1V reference against AVcc
// set the reference to Vcc and the measurement to the internal 1.1V reference
#if defined(__AVR_ATmega32U4__) || defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
ADMUX = _BV(REFS0) | _BV(MUX4) | _BV(MUX3) | _BV(MUX2) | _BV(MUX1);
#elif defined (__AVR_ATtiny24__) || defined(__AVR_ATtiny44__) || defined(__AVR_ATtiny84__)
ADMUX = _BV(MUX5) | _BV(MUX0);
#elif defined (__AVR_ATtiny25__) || defined(__AVR_ATtiny45__) || defined(__AVR_ATtiny85__)
ADMUX = _BV(MUX3) | _BV(MUX2);
#else
ADMUX = _BV(REFS0) | _BV(MUX3) | _BV(MUX2) | _BV(MUX1);
#endif
delay(2); // Wait for Vref to settle
ADCSRA |= _BV(ADSC); // Start conversion
while (bit_is_set(ADCSRA, ADSC)); // measuring
int low = ADCL; // must read ADCL first - it then locks ADCH
int high = ADCH; // unlocks both
long result = (high<<8) | low;
result = 1125300L / result; // Calculate Vcc (in mV); 1125300 = 1.1*1023*1000
return result; // Vcc in millivolts
}