Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- /*
- 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
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement