Want more features on Pastebin? Sign Up, it's FREE!

Wheeled-Robot Arduino Code

By: basile on Aug 21st, 2012  |  syntax: C  |  size: 15.27 KB  |  views: 239  |  expires: Never
download  |  raw  |  embed  |  report abuse  |  print
Text below is selected. Please press Ctrl+C to copy to your clipboard. (⌘+C on Mac)
  1. /*
  2.   Wheeled-Robot Arduino Code
  3.   version 2012.12.12.09.32
  4.  
  5.   Created 2012 08 21
  6.   by Basile Laderchi
  7.  
  8.   Binary sketch size: 13654 bytes
  9.  
  10.   controls:
  11.   motors    arm     leds  analog  test  presets  autoDrive
  12.     w      y u i    1 2     ~      v     p1-4        0
  13.   a s d    h j k
  14.     x       n m
  15.    + -       ,
  16.  
  17.   Needed:
  18.   NewPing library: http://code.google.com/p/arduino-new-ping/
  19.  
  20.   Useful things:
  21.   readAccelerometer code from: http://learn.parallax.com/kickstart/28017
  22.   readVcc code from: http://provideyourown.com/2012/secret-arduino-voltmeter-measure-battery-voltage/
  23.   longSharp_AnalogToCm code from: http://luckylarry.co.uk/arduino-projects/arduino-using-a-sharp-ir-sensor-for-distance-calculation/
  24. */
  25.  
  26. #include <Servo.h>
  27. #include <NewPing.h>
  28.  
  29. const int centerMotorSpeed = 1500;
  30. const int incdecMotorSpeed = 100;
  31. const int minMotorSpeed = 300;
  32. const int maxMotorSpeed = 500;
  33. const int motorEnabledStatusDelay = 100;
  34.  
  35. const int minHeadLRPosition = 0;
  36. const int maxHeadLRPosition = 180;
  37. const int centerHeadLRPosition = 100;
  38. const int incdecHeadLR = 10;
  39.  
  40. const int minHeadS1Position = 50;
  41. const int maxHeadS1Position = 180;
  42. const int centerHeadS1Position = 180;
  43. const int incdecHeadS1 = 10;
  44.  
  45. const int minHeadS2Position = 0;
  46. const int maxHeadS2Position = 180;
  47. const int centerHeadS2Position = 150;
  48. const int incdecHeadS2 = 10;
  49.  
  50. const int minHeadS3Position = 0;
  51. const int maxHeadS3Position = 180;
  52. const int centerHeadS3Position = 150;
  53. const int incdecHeadS3 = 10;
  54.  
  55. const int maxSonarRange = 300;
  56.  
  57. const double headSweepSpeed = 0.001;
  58.  
  59. const int minDistance = 25;
  60.  
  61. const byte USB_PIN_D0 = 0;
  62. const byte USB_PIN_D1 = 1;
  63. const byte pinLed1 = 2;
  64. const byte pinHeadS2 = 3;
  65. const byte pinLed2 = 4;
  66. const byte pinHeadS3 = 5;
  67. const byte pinHeadLR = 6;
  68. const byte pinAccelX = 7;
  69. const byte pinAccelY = 8;
  70. const byte pinMotorL = 9;
  71. const byte pinMotorR = 10;
  72. const byte pinHeadS1 = 11;
  73. const byte pinMotorStop = 12;
  74. const byte NOT_USED_D13 = 13;
  75.  
  76. const byte pinLeftSharp = A0;
  77. const byte pinMiddleSharp = A1;
  78. const byte pinRightSharp = A2;
  79. const byte pinBackSharp = A3;
  80. const byte pinSonar = A4;
  81. const byte NOT_USED_A5 = A5;
  82.  
  83. const int armServos = 3;
  84. /*
  85.   {140, 110, 120},
  86.   { 60,  60, 170},
  87.   { 90,  60, 140},
  88.   { 60,  10, 130}
  89. */
  90. const int armPresets[][armServos] = {
  91.   {140,  80, 120},
  92.   { 60,   0, 140},
  93.   { 90,  60, 110},
  94.   {160,  70, 110}
  95. };
  96. const int numArmPresets = (sizeof(armPresets) / armServos) / sizeof(int);
  97.  
  98. Servo motorL;
  99. Servo motorR;
  100. Servo headLR;
  101. Servo headS1;
  102. Servo headS2;
  103. Servo headS3;
  104. NewPing sonar = NewPing(pinSonar, pinSonar, maxSonarRange);
  105.  
  106. unsigned long headLRTimer = 0;
  107.  
  108. char dir = 's';
  109. char preset = '_';
  110. char lastMotorMovement = 's';
  111.  
  112. int headLRLastPosition = centerHeadLRPosition;
  113. byte headLRSweepEnabled = 0;
  114. int headLRRotation;
  115. int headS1LastPosition = centerHeadS1Position;
  116. int headS2LastPosition = centerHeadS2Position;
  117. int headS3LastPosition = centerHeadS3Position;
  118. double headLRSpeed = 0;
  119. double headLRRange = 0;
  120.  
  121. int motorLLastSpeed = centerMotorSpeed;
  122. int motorRLastSpeed = centerMotorSpeed;
  123. int motorLastSpeed = minMotorSpeed;
  124. int lastMotorEnabledStatus = LOW;
  125.  
  126. int motorEasingSpeed;
  127.  
  128. int lastLed1Status = LOW;
  129. int lastLed2Status = LOW;
  130. int lastLeftSharpValue = 0;
  131. int lastMiddleSharpValue = 0;
  132. int lastRightSharpValue = 0;
  133. int lastBackSharpValue = 0;
  134. unsigned int lastSonarValue = 0;
  135. int lastAccelXValue = 0;
  136. int lastAccelYValue = 0;
  137.  
  138. byte autoDriveEnabled = 0;
  139.  
  140. double Vcc;
  141.  
  142. void setup() {
  143.   Serial.begin(9600);
  144.   pinMode(pinLed1, OUTPUT);
  145.   pinMode(pinLed2, OUTPUT);
  146.   pinMode(pinAccelX, INPUT);
  147.   pinMode(pinAccelY, INPUT);
  148.   pinMode(pinMotorStop, OUTPUT);
  149.   motorL.attach(pinMotorL);
  150.   motorR.attach(pinMotorR);
  151.   headLR.attach(pinHeadLR);
  152.   headS1.attach(pinHeadS1);
  153.   headS2.attach(pinHeadS2);
  154.   headS3.attach(pinHeadS3);
  155.   motorStop();
  156.   headCenter();
  157.  
  158.   Vcc = readVcc() / 1000.0;
  159. }
  160.  
  161. void loop() {
  162.   if (Serial.available()) {
  163.     dir = Serial.read();
  164.     disableAutoDrive();
  165.     move(dir);
  166.   }
  167.   //oscillatingAutopilot();
  168.   readAccelerometer();
  169.   autoDrive();
  170. }
  171.  
  172. void move(char dir) {
  173.   switch (dir) {
  174.     case 'w':  // Forward
  175.       motorForward();
  176.       lastMotorMovement = 'w';
  177.       break;
  178.     case 'x':  // Back
  179.       motorReverse();
  180.       lastMotorMovement = 'x';
  181.       break;
  182.     case 's':  // Stop
  183.       motorStop();
  184.       lastMotorMovement = 's';
  185.       break;
  186.     case 'a':  // Turn Left
  187.       motorTurnLeft();
  188.       lastMotorMovement = 'a';
  189.       break;
  190.     case 'd':  // Turn Right
  191.       motorTurnRight();
  192.       lastMotorMovement = 'd';
  193.       break;
  194.     case 'y':  // Head S1 Up
  195.       headS1Up();
  196.       break;
  197.     case 'h':  // Head S1 Down
  198.       headS1Down();
  199.       break;
  200.     case 'u':  // Head S2 Up
  201.       headS2Up();
  202.       break;
  203.     case 'j':  // Head S2 Down
  204.       headS2Down();
  205.       break;
  206.     case 'i':  // Head S3 Up
  207.       headS3Up();
  208.       break;
  209.     case 'k':  // Head S3 Down
  210.       headS3Down();
  211.       break;
  212.     case 'n':  // Head Left
  213.       headLeft();
  214.       break;
  215.     case 'm':  // Head Right
  216.       headRight();
  217.       break;
  218.     case ',':  // Head Center
  219.     case '<':
  220.       headCenter();
  221.       break;
  222.     case '+':
  223.     case '=':
  224.       increaseSpeed();
  225.       break;
  226.     case '-':
  227.     case '_':
  228.       decreaseSpeed();
  229.       break;
  230.     case '1':
  231.       led1();
  232.       break;
  233.     case '2':
  234.       led2();
  235.       break;
  236.     case '0':
  237.       enableAutoDrive();
  238.     case 'p':
  239.       presets();
  240.       break;
  241.     case '~':
  242.     case '`':
  243.       analog();
  244.       break;
  245.     case 'v':
  246.       // testHeadSweep();
  247.       break;
  248.   }
  249. }
  250.  
  251. /*
  252. void oscillatingAutopilot() {
  253.   if (headLRSweepEnabled == 1) {
  254.     headLRTimer++;
  255.     headLRRotation = sin(headLRTimer * headLRSpeed) * headLRRange;
  256.     Serial.print("Sweep: ");
  257.     Serial.print(headLRTimer);
  258.     Serial.print(", ");
  259.     Serial.print(headLRRotation);
  260.     if (headLR.read() != (centerHeadLRPosition + headLRRotation)) {
  261.       headLR.write(centerHeadLRPosition + headLRRotation);
  262.       Serial.print(", 1");
  263.     } else {
  264.       Serial.print(", 0");
  265.     }
  266.     Serial.println();
  267.   }
  268. }
  269. */
  270.  
  271. void readAccelerometer() {
  272.   lastAccelXValue = pulseIn(pinAccelX, HIGH);
  273.   lastAccelYValue = pulseIn(pinAccelY, HIGH);
  274.   /*
  275.   Serial.print("Accelerometer: ");
  276.   Serial.print(lastAccelXValue);
  277.   Serial.print(",");
  278.   Serial.println(lastAccelYValue);
  279.   */
  280. }
  281.  
  282. void motorForward() {
  283.   motorChangeSpeed(motorLastSpeed, motorLastSpeed, LOW);
  284. }
  285.  
  286. void motorReverse() {
  287.   motorChangeSpeed(- motorLastSpeed, - motorLastSpeed, LOW);
  288. }
  289.  
  290. void motorStop() {
  291.   motorChangeSpeed(0, 0, HIGH);
  292. }
  293.  
  294. void motorTurnLeft() {
  295.   motorChangeSpeed(- motorLastSpeed, motorLastSpeed, LOW);
  296. }
  297.  
  298. void motorTurnRight() {
  299.   motorChangeSpeed(motorLastSpeed, - motorLastSpeed, LOW);
  300. }
  301.  
  302. void motorChangeSpeed(int motorLSpeed, int motorRSpeed, int motorEnabledStatus) {
  303.   if (motorEnabledStatus == LOW) {
  304.     if (lastMotorEnabledStatus != motorEnabledStatus) {
  305.       digitalWrite(pinMotorStop, motorEnabledStatus);
  306.       delay(motorEnabledStatusDelay);
  307.     }
  308.   }
  309.  
  310.   motorLLastSpeed = centerMotorSpeed + motorLSpeed;
  311.   motorRLastSpeed = centerMotorSpeed + motorRSpeed;
  312.   motorL.writeMicroseconds(motorLLastSpeed);
  313.   motorR.writeMicroseconds(motorRLastSpeed);
  314.  
  315.   if (motorEnabledStatus == HIGH) {
  316.     if (lastMotorEnabledStatus != motorEnabledStatus) {
  317.       delay(motorEnabledStatusDelay);
  318.       digitalWrite(pinMotorStop, motorEnabledStatus);
  319.     }
  320.   }
  321.  
  322.   lastMotorEnabledStatus = motorEnabledStatus;
  323. }
  324.  
  325. void headS1Up() {
  326.   headS1LastPosition += incdecHeadS1;
  327.   headS1LastPosition = min(headS1LastPosition, maxHeadS1Position);
  328.   headS1.write(headS1LastPosition);
  329. }
  330.  
  331. void headS1Down() {
  332.   headS1LastPosition -= incdecHeadS1;
  333.   headS1LastPosition = max(headS1LastPosition, minHeadS1Position);
  334.   headS1.write(headS1LastPosition);
  335. }
  336.  
  337. void headS2Up() {
  338.   headS2LastPosition -= incdecHeadS2;
  339.   headS2LastPosition = max(headS2LastPosition, minHeadS2Position);
  340.   headS2.write(headS2LastPosition);
  341. }
  342.  
  343. void headS2Down() {
  344.   headS2LastPosition += incdecHeadS2;
  345.   headS2LastPosition = min(headS2LastPosition, maxHeadS2Position);
  346.   headS2.write(headS2LastPosition);
  347. }
  348.  
  349. void headS3Up() {
  350.   headS3LastPosition += incdecHeadS3;
  351.   headS3LastPosition = min(headS3LastPosition, maxHeadS3Position);
  352.   headS3.write(headS3LastPosition);
  353. }
  354.  
  355. void headS3Down() {
  356.   headS3LastPosition -= incdecHeadS3;
  357.   headS3LastPosition = max(headS3LastPosition, minHeadS3Position);
  358.   headS3.write(headS3LastPosition);
  359. }
  360.  
  361. void headCenter() {
  362.   headLRLastPosition = centerHeadLRPosition;
  363.   headLR.write(headLRLastPosition);
  364.   headS1LastPosition = centerHeadS1Position;
  365.   headS1.write(headS1LastPosition);
  366.   headS2LastPosition = centerHeadS2Position;
  367.   headS2.write(headS2LastPosition);
  368.   headS3LastPosition = centerHeadS3Position;
  369.   headS3.write(headS3LastPosition);
  370. }
  371.  
  372. void headLeft() {
  373.   headLRSweepEnabled = 0;
  374.   headLRLastPosition -= incdecHeadLR;
  375.   headLRLastPosition = max(headLRLastPosition, minHeadLRPosition);
  376.   headLR.write(headLRLastPosition);
  377. }
  378.  
  379. void headRight() {
  380.   headLRSweepEnabled = 0;
  381.   headLRLastPosition += incdecHeadLR;
  382.   headLRLastPosition = min(headLRLastPosition, maxHeadLRPosition);
  383.   headLR.write(headLRLastPosition);
  384. }
  385.  
  386. void increaseSpeed() {
  387.   motorLastSpeed += incdecMotorSpeed;
  388.   motorLastSpeed = min(motorLastSpeed, maxMotorSpeed);
  389.   move(lastMotorMovement);
  390. }
  391.  
  392. void decreaseSpeed() {
  393.   motorLastSpeed -= incdecMotorSpeed;
  394.   motorLastSpeed = max(motorLastSpeed, minMotorSpeed);
  395.   move(lastMotorMovement);
  396. }
  397.  
  398. void led1() {
  399.   lastLed1Status = ~lastLed1Status;
  400.   digitalWrite(pinLed1, lastLed1Status);
  401. }
  402.  
  403. void led2() {
  404.   lastLed2Status = ~lastLed2Status;
  405.   digitalWrite(pinLed2, lastLed2Status);
  406. }
  407.  
  408. void presets() {
  409.   if (Serial.available()) {
  410.     preset = Serial.read();
  411.     headPreset(preset - '0');
  412.   }
  413. }
  414.  
  415. void headPreset(int preset) {
  416.   if (preset > 0 && preset <= numArmPresets) {
  417.     headLRLastPosition = centerHeadLRPosition;
  418.     headLR.write(headLRLastPosition);
  419.     headS1LastPosition = armPresets[preset][0];
  420.     headS1.write(headS1LastPosition);
  421.     headS2LastPosition = armPresets[preset][1];
  422.     headS2.write(headS2LastPosition);
  423.     headS3LastPosition = armPresets[preset][2];
  424.     headS3.write(headS3LastPosition);
  425.   }
  426. }
  427.  
  428. void enableAutoDrive() {
  429.   autoDriveEnabled = 1;
  430. }
  431.  
  432. void disableAutoDrive() {
  433.   autoDriveEnabled = 0;
  434. }
  435.  
  436. void autoDrive() {
  437.   if (autoDriveEnabled == 0) {
  438.     return;
  439.   }
  440.  
  441.   int leftDistance,
  442.       middleDistance,
  443.       rightDistance;
  444.   byte autoDriveMode,
  445.        rnd;
  446.  
  447.   readAnalogSensors();
  448.  
  449.   leftDistance = longSharp_AnalogToCm(lastLeftSharpValue);
  450.   middleDistance = longSharp_AnalogToCm(lastMiddleSharpValue);
  451.   rightDistance = longSharp_AnalogToCm(lastRightSharpValue);
  452.   autoDriveMode = 0;
  453.  
  454.   if (leftDistance >= minDistance &&
  455.       middleDistance >= minDistance &&
  456.       rightDistance >= minDistance) {
  457.     autoDriveMode = 1;
  458.   }
  459.   if (leftDistance < minDistance &&
  460.       middleDistance >= minDistance &&
  461.       rightDistance >= minDistance) {
  462.     autoDriveMode = 2;
  463.   }
  464.   if (leftDistance >= minDistance &&
  465.       middleDistance < minDistance &&
  466.       rightDistance >= minDistance) {
  467.     autoDriveMode = 4;
  468.   }
  469.   if (leftDistance >= minDistance &&
  470.       middleDistance >= minDistance &&
  471.       rightDistance < minDistance) {
  472.     autoDriveMode = 3;
  473.   }
  474.   if (leftDistance < minDistance &&
  475.       middleDistance < minDistance &&
  476.       rightDistance < minDistance) {
  477.     autoDriveMode = 4;
  478.   }
  479.  
  480.   Serial.print("autoDrive =>");
  481.   Serial.print("  left: ");
  482.   Serial.print(lastLeftSharpValue);
  483.   Serial.print(" - ");
  484.   Serial.print(leftDistance);
  485.   Serial.print("  middle: ");
  486.   Serial.print(lastMiddleSharpValue);
  487.   Serial.print(" - ");
  488.   Serial.print(middleDistance);
  489.   Serial.print("  right: ");
  490.   Serial.print(lastRightSharpValue);
  491.   Serial.print(" - ");
  492.   Serial.print(rightDistance);
  493.   Serial.print("  mode: ");
  494.   Serial.println(autoDriveMode);
  495.   delay(100);
  496.  
  497.   switch (autoDriveMode) {
  498.     case 1:
  499.       motorForward();
  500.       break;
  501.     case 2:
  502.       motorStop();
  503.       motorTurnRight();
  504.       break;
  505.     case 3:
  506.       motorStop();
  507.       motorTurnLeft();
  508.       break;
  509.     case 4:
  510.       motorStop();
  511.       motorReverse();
  512.       delay(500);
  513.       motorStop();
  514.       rnd = random(0, 2);
  515.       switch (rnd) {
  516.         case 0:
  517.           motorTurnLeft();
  518.           break;
  519.         case 1:
  520.           motorTurnRight();
  521.           break;
  522.       }
  523.       delay(100);
  524.     default:
  525.       motorStop();
  526.       break;
  527.   }
  528. }
  529.  
  530. void analog() {
  531.   readAnalogSensors();
  532.   Serial.print("Left Sharp: ");
  533.   Serial.print(lastLeftSharpValue);
  534.   Serial.print(",");
  535.   Serial.println(longSharp_AnalogToCm(lastLeftSharpValue));
  536.   Serial.print("Middle Sharp: ");
  537.   Serial.print(lastMiddleSharpValue);
  538.   Serial.print(",");
  539.   Serial.println(longSharp_AnalogToCm(lastMiddleSharpValue));
  540.   Serial.print("Right Sharp: ");
  541.   Serial.print(lastRightSharpValue);
  542.   Serial.print(",");
  543.   Serial.println(longSharp_AnalogToCm(lastRightSharpValue));
  544.   Serial.print("Back Sharp: ");
  545.   Serial.print(lastBackSharpValue);
  546.   Serial.print(",");
  547.   Serial.println(longSharp_AnalogToCm(lastBackSharpValue));
  548.   Serial.print("Sonar: ");
  549.   Serial.print(lastSonarValue);
  550.   Serial.print(",");
  551.   Serial.println(sonar.convert_cm(lastSonarValue));
  552. }
  553.  
  554. void readAnalogSensors() {
  555.   lastLeftSharpValue = analogRead(pinLeftSharp);
  556.   lastMiddleSharpValue = analogRead(pinMiddleSharp);
  557.   lastRightSharpValue = analogRead(pinRightSharp);
  558.   lastBackSharpValue = analogRead(pinBackSharp);
  559.   lastSonarValue = sonar.ping_median();
  560. }
  561.  
  562. /*
  563. void testHeadSweep() {
  564.   // set speed to 0 and it stops sweeping
  565.   // or set rot to 0 and speed to 0 and it centers
  566.   // sweep_speed should be about 0.01 for a start
  567.   if (headLRSweepEnabled == 0) {
  568.     headLRSweepEnabled = 1;
  569.     headLRTimer = 0;
  570.     headLRSpeed = headLRSweepSpeed;
  571.     headLRRange = 10;
  572.     Serial.println("Sweep Start");
  573.   } else {
  574.     headLRSweepEnabled = 0;
  575.     headLRSpeed = 0;
  576.     headLRRange = 0;
  577.     Serial.println("Sweep Stop");
  578.   }
  579. }
  580. */
  581.  
  582. double longSharp_AnalogToCm(int valueToConvert) {
  583.   double valueVoltage;
  584.   double result;
  585.  
  586.   valueVoltage = (valueToConvert / 1023.0) * Vcc;
  587.   result = 65 * pow(valueVoltage, -1.10);
  588.   return result;
  589. }
  590.  
  591. long readVcc() {
  592.   // Read 1.1V reference against AVcc
  593.   // set the reference to Vcc and the measurement to the internal 1.1V reference
  594. #if defined(__AVR_ATmega32U4__) || defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
  595.   ADMUX = _BV(REFS0) | _BV(MUX4) | _BV(MUX3) | _BV(MUX2) | _BV(MUX1);
  596. #elif defined (__AVR_ATtiny24__) || defined(__AVR_ATtiny44__) || defined(__AVR_ATtiny84__)
  597.   ADMUX = _BV(MUX5) | _BV(MUX0);
  598. #elif defined (__AVR_ATtiny25__) || defined(__AVR_ATtiny45__) || defined(__AVR_ATtiny85__)
  599.   ADMUX = _BV(MUX3) | _BV(MUX2);
  600. #else
  601.   ADMUX = _BV(REFS0) | _BV(MUX3) | _BV(MUX2) | _BV(MUX1);
  602. #endif
  603.  
  604.   delay(2); // Wait for Vref to settle
  605.   ADCSRA |= _BV(ADSC); // Start conversion
  606.   while (bit_is_set(ADCSRA, ADSC)); // measuring
  607.  
  608.   int low  = ADCL; // must read ADCL first - it then locks ADCH
  609.   int high = ADCH; // unlocks both
  610.  
  611.   long result = (high<<8) | low;
  612.  
  613.   result = 1125300L / result; // Calculate Vcc (in mV); 1125300 = 1.1*1023*1000
  614.   return result; // Vcc in millivolts
  615. }
clone this paste RAW Paste Data