Arduino Playground is read-only starting December 31st, 2018. For more info please look at this Forum Post

Concept

This robot is an addition to the robot project posted here called 4LegWalker. Basically we added a small servo, a IR distance sensor and a I2C digital compass.

Hardware Layout

The layout is pretty simple we added a servo and then mounted (using velcro) the distance sensor and the digital GPS to an extended servo horn.

Figure 1, 2, and 3 are the head in different positions

Figure 1

Figure 2

Figure 3

Software

  1. #include <Servo.h>
  2. #include <Wire.h>
  3.  
  4. Servo waist,front,back,neck;
  5.  
  6. #define wCenter 80
  7. #define fCenter 70
  8. #define bCenter 80
  9. #define nCenter 80
  10. #define wSwing 40
  11. #define fSwing 35
  12. #define bSwing 35
  13. #define nSwing 40
  14. #define FORWARD 0
  15. #define BACKWARD 1
  16. #define LEFT 2
  17. #define RIGHT 3
  18. #define STOP 4
  19. int wPos,fPos,bPos,cycle,dir;
  20. long start;
  21.  
  22. int getMode() {
  23.   pinMode(5,OUTPUT);
  24.   digitalWrite(5,LOW);
  25.   digitalWrite(6,HIGH);
  26.   pinMode(6,INPUT);
  27.   if (digitalRead(6)==0) return 1;
  28.   digitalWrite(7,HIGH);
  29.   pinMode(7,INPUT);
  30.   if (digitalRead(7)==0) return 2;
  31.   digitalWrite(8,HIGH);
  32.   pinMode(8,INPUT);
  33.   if (digitalRead(8)==0) return 3;
  34.   digitalWrite(9,HIGH);
  35.   pinMode(9,INPUT);
  36.   if (digitalRead(9)==0) return 4;
  37.   return 0;
  38. }
  39.  
  40. int compassAddress = 0x42 >> 1;  // From datasheet compass address is 0x42
  41.                                  // shift the address 1 bit right, the Wire library only needs the 7
  42.                                  // most significant bits for the address
  43. void compassSetup()
  44. {
  45.   Wire.begin();                // join i2c bus (address optional for master)
  46. }
  47.  
  48. int getCompass()
  49. {
  50.   int reading;
  51.   Wire.beginTransmission(compassAddress);
  52.   Wire.send('A');
  53.   Wire.endTransmission();
  54.   delay(10);
  55.   Wire.requestFrom(compassAddress, 2);  // request 2 bytes from slave device #33
  56.   if(2 <= Wire.available())     // if two bytes were received
  57.   {
  58.     reading = Wire.receive();   // receive high byte (overwrites previous reading)
  59.     reading = reading << 8;     // shift high byte to be high 8 bits
  60.     reading += Wire.receive();  // receive low byte as lower 8 bits
  61.     reading /= 10;
  62.     return reading;
  63.   }
  64.   return -1;
  65. }
  66.  
  67.  
  68. int mode;
  69. int initialHeading;
  70. int sequenceLength;
  71.  
  72. void setup()
  73. {
  74.   waist.attach(13);
  75.   front.attach(12);
  76.   back.attach(11);
  77.   neck.attach(0);
  78.   wPos=wCenter;
  79.   fPos=fCenter;
  80.   bPos=bCenter;
  81.   dir=STOP;
  82.   cycle=0;
  83.   waist.write(wCenter);
  84.   front.write(fCenter);
  85.   back.write(bCenter);
  86.   neck.write(nCenter);
  87.   compassSetup();
  88.   neck.write(nCenter-nSwing);
  89.   delay(500);
  90.   neck.write(nCenter+nSwing);
  91.   delay(500);
  92.   neck.write(nCenter);
  93.   delay(500);
  94.   initialHeading=getCompass();
  95.   delay(40);
  96.   mode=getMode();
  97.   start=millis();
  98.   dir=FORWARD;
  99.   sequenceLength=2000;
  100.   if (mode==2 || mode==3)
  101.     Serial.begin(9600);
  102. }
  103.  
  104. void centerTest() {
  105.   waist.write(wCenter);
  106.   front.write(fCenter);
  107.   back.write(bCenter);
  108.   neck.write(nCenter);
  109.   delay(10);
  110. }
  111.  
  112. void doForward(int cycle) {
  113.   if (cycle<12) wPos=wCenter+wSwing;
  114.   else if (cycle<37) wPos=wCenter-wSwing;
  115.   else if (cycle<62) wPos=wCenter+wSwing;
  116.   else if (cycle<87) wPos=wCenter-wSwing;
  117.   else          wPos=wCenter+wSwing;
  118.   if (cycle<25) fPos=fCenter+fSwing;
  119.   else if (cycle<50) fPos=fCenter-fSwing;
  120.   else if (cycle<75) fPos=fCenter+fSwing;
  121.   else fPos=fCenter-fSwing;
  122.   if (cycle<25) bPos=bCenter-bSwing;
  123.   else if (cycle<50) bPos=bCenter+bSwing;
  124.   else if (cycle<75) bPos=bCenter-bSwing;
  125.   else bPos=bCenter+bSwing;
  126. }
  127.  
  128. void doBackward(int cycle) {
  129.   doForward(100-cycle);
  130. }
  131.  
  132. void doLeft(int cycle) {
  133.   doForward(cycle);
  134.   if (wPos>wCenter) wPos=wCenter;
  135. }
  136.  
  137. void doRight(int cycle) {
  138.   doForward(cycle);
  139.   if (wPos<wCenter) wPos=wCenter;
  140. }
  141.  
  142. void doStop(){
  143.   wPos=wCenter;
  144.   fPos=fCenter;
  145.   bPos=bCenter;
  146. }
  147.  
  148. void posUpdate() {
  149.   cycle=(cycle+3)%100;  // 100 step cycle
  150.   switch(dir) {
  151.     case FORWARD: doForward(cycle);  break;
  152.     case BACKWARD: doBackward(cycle); break;
  153.     case LEFT: doLeft(cycle); break;
  154.     case RIGHT: doRight(cycle); break;
  155.     case STOP: doStop(); break;
  156.   }
  157. }
  158.  
  159. #define TOOCLOSE 500
  160. #define NOTCLEAR 400
  161. int getDistance() {
  162.   return analogRead(3);
  163. }
  164.  
  165. void lookLeft() {
  166.   neck.write(nCenter-nSwing);
  167.   delay(200);
  168. }
  169.  
  170. void lookRight() {
  171.   neck.write(nCenter+nSwing);
  172.   delay(200);
  173. }
  174.  
  175. void lookForward() {
  176.   neck.write(nCenter);
  177.   delay(200);
  178. }
  179.  
  180. void walkTest() {
  181.   posUpdate();
  182.   waist.write(wPos);
  183.   delay(20);  
  184.   front.write(fPos);
  185.   delay(20);  
  186.   back.write(bPos);
  187.   delay(20);
  188.   if (millis()-start<10000) dir=FORWARD;
  189.   else if (millis()-start<20000) dir=LEFT;
  190.   else if (millis()-start<30000) dir=RIGHT;
  191.   else if (millis()-start<40000) dir=BACKWARD;
  192.   else if (millis()-start<50000) {
  193.     dir=FORWARD;
  194.     start=millis();
  195.   }
  196. }
  197.  
  198.  
  199. void run() {
  200.   posUpdate();
  201.   waist.write(wPos);
  202.   delay(20);  
  203.   front.write(fPos);
  204.   delay(20);  
  205.   back.write(bPos);
  206.   delay(20);
  207.   if (millis()-start>sequenceLength) {
  208.     lookForward();
  209.     int f=getDistance();
  210.     if (f>TOOCLOSE) { // We got too close
  211.       dir=BACKWARD;
  212.       lookLeft();
  213.       int l=getDistance();
  214.       lookRight();
  215.       int r=getDistance();
  216.       // If we are in a corner do a big backup
  217.       if (l>TOOCLOSE && r>TOOCLOSE) sequenceLength=20000;
  218.     }
  219.     else if (f>NOTCLEAR){  // The way forward is not clear
  220.       sequenceLength=2000;
  221.       lookLeft();
  222.       int l=getDistance();
  223.       lookRight();
  224.       int r=getDistance();
  225.       if (l<r) dir=LEFT;
  226.       else     dir=RIGHT;
  227.     } else {  // We are free to get back on course
  228.       sequenceLength=2000;
  229.       int heading=getCompass();
  230.       dir=FORWARD;
  231.       if (heading-initialHeading>20) dir=LEFT;
  232.       if (heading-initialHeading<-20) dir=RIGHT;
  233.     }
  234.     start=millis();
  235.   }
  236. }
  237.  
  238. void distanceTest() {
  239.   int d=getDistance();
  240.   Serial.print("Distance ");
  241.   Serial.println(d);
  242.   delay(100);
  243. }
  244.  
  245. void compassTest() {
  246.   int c=getCompass();
  247.   Serial.print("Compass ");
  248.   Serial.println(c);
  249.   delay(100);
  250. }
  251.  
  252. void loop()
  253. {
  254.   switch(mode) {
  255.     case 1: centerTest(); break;
  256.     case 2: distanceTest(); break;
  257.     case 3: compassTest(); break;
  258.     case 4: walkTest(); break;
  259.     default: run(); break;
  260.   }
  261. }
[Get Code]

What is next

We are going to make the walker even smarter by adding the ability for it to search the space as if is a trinary tree. Where the robot chooses to move to the left, right or forward. Then if it gets stuck it could back track out of it location by reversing the stored moves. We are not going to concern ourselves too much with keeping a precise location until we perfect the Acoustic GPS.

Karl http://www.coloradomesa.edu/~kcastlet