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

A simple script to read from each of the devices from the SEN-10724 and additional pressure sensors made from SEN-9375.

DARPA Team Mojavaton (http://www.mojavaton.org) code for reading sensors on the robot.

  1. // Reading 9DOF sensor stick from Sparkfun's
  2. // https://www.sparkfun.com/products/10724
  3. // Used to provide orientation of the chassis and pressure
  4. // on leg sensors built around Sparkfun's
  5. // https://www.sparkfun.com/products/9375
  6.  
  7. // Thanks to Sparkfun for the support
  8.  
  9. #include <Wire.h>
  10.  
  11. #define GYROADDR 0x68
  12. #define COMPASSADDR 0x1e
  13. #define ACCELADDR 0x53
  14.  
  15. union XYZBuffer {
  16.   struct {
  17.     short x,y,z;
  18.   } value;
  19.   byte buff[6];
  20. };
  21.  
  22. void changeEndian(union XYZBuffer *xyz) {
  23.   for (int i=0;i<6;i+=2) {
  24.     byte t=xyz->buff[i];
  25.     xyz->buff[i]=xyz->buff[i+1];
  26.     xyz->buff[i+1]=t;
  27.   }
  28. }
  29.  
  30. // Generically useful reading into a union type
  31. void readXYZ(int device,union XYZBuffer *xyz) {    
  32.   Wire.requestFrom(device, 6);      
  33.   long start=millis();
  34.   while (!Wire.available() && (millis()-start)<100);
  35.   if (millis()-start<100) {
  36.     for (int i=0;i<6;i++)
  37.       xyz->buff[i]=Wire.read();
  38.   }
  39. }
  40.  
  41. void setupAccel(int device) {
  42.   // Check ID to see if we are communicating
  43.   Wire.beginTransmission(device);
  44.   Wire.write(0x00); // One Reading
  45.   Wire.endTransmission();
  46.   Wire.requestFrom(device,1);
  47.   while (!Wire.available());  
  48.   byte ch=Wire.read();
  49.   Serial.print("Accel id is 0x");
  50.   Serial.println(ch,HEX);
  51.   // Should output E5
  52.  
  53.   // https://www.sparkfun.com/datasheets/Sensors/Accelerometer/ADXL345.pdf
  54.   // Page 16
  55.   Wire.beginTransmission(device);
  56.   Wire.write(0x2d);
  57.   Wire.write(0x08);
  58.   Wire.endTransmission();
  59.   Wire.beginTransmission(device);
  60.   Wire.write(0x38);
  61.   Wire.write(0x84);
  62.   Wire.endTransmission();
  63.  
  64. }
  65. void readAccel(int device,union XYZBuffer *xyz) {
  66.   Wire.beginTransmission(device);
  67.   Wire.write(0x32); // One Reading
  68.   Wire.endTransmission();
  69.   readXYZ(device,xyz);
  70. }
  71.  
  72. void setupCompass(int device) {
  73.   // Check ID to see if we are communicating
  74.   Serial.print("Compass id is ");
  75.   Wire.beginTransmission(device);
  76.   Wire.write(10); // One Reading
  77.   Wire.endTransmission();
  78.   Wire.requestFrom(device,2);
  79.   while (!Wire.available());
  80.   char ch=Wire.read();
  81.   Serial.print(ch);  
  82.   ch=Wire.read();
  83.   Serial.println(ch);
  84.   // Should output H4  
  85.  
  86. // Page 18
  87. // at https://dlnmh9ip6v2uc.cloudfront.net/datasheets/Sensors/Magneto/HMC5883L-FDS.pdf
  88.   Wire.beginTransmission(device);
  89.   Wire.write(0x00); Wire.write(0x70);
  90.   Wire.endTransmission();
  91.   Wire.beginTransmission(device);
  92.   Wire.write(0x01); Wire.write(0xA0);
  93.   Wire.endTransmission();
  94.   Wire.beginTransmission(device);
  95.   Wire.write(0x02); Wire.write(0x00); //  Reading
  96.   Wire.endTransmission();
  97.   delay(6);
  98. }
  99. void readCompass(int device,union XYZBuffer *xyz) {
  100.   readXYZ(device,xyz);
  101.   changeEndian(xyz);
  102.   Wire.beginTransmission(device);
  103.   Wire.write(0x03);
  104.   Wire.endTransmission();
  105. }
  106.  
  107. void setupGyro(int device) {
  108.   // Check ID to see if we are communicating
  109.   Wire.beginTransmission(device);
  110.   Wire.write(0x00); // One Reading
  111.   Wire.endTransmission();
  112.   Wire.requestFrom(device,1);
  113.   while (!Wire.available());  
  114.   byte ch=Wire.read();
  115.   Serial.print("Gyro id is 0x");
  116.   Serial.println(ch,HEX);  
  117.   // Should output 69
  118. }
  119. void readGyro(int device,union XYZBuffer *xyz) {
  120.   // https://www.sparkfun.com/datasheets/Sensors/Gyro/PS-ITG-3200-00-01.4.pdf
  121.   // page 20
  122.   Wire.beginTransmission(device);
  123.   Wire.write(0x1d);
  124.   Wire.endTransmission();
  125.   readXYZ(device,xyz);
  126.   changeEndian(xyz);  
  127. }
  128.  
  129. void pad(int width,int number) {
  130.   int n=abs(number);
  131.   int w=width;
  132.   if (number<0) w--;
  133.   while (n>0) {
  134.     w--;
  135.     n/=10;
  136.   }
  137.   if (number==0) w--;
  138.   for (int i=0;i<w;i++) Serial.print(' ');
  139. }
  140.  
  141. void output(union XYZBuffer xyz) {
  142. //  pad(6,xyz.value.x);
  143.   Serial.print(xyz.value.x);
  144.   Serial.print(',');
  145. //  pad(6,xyz.value.y);
  146.   Serial.print(xyz.value.y);
  147.   Serial.print(',');
  148. //  pad(6,xyz.value.z);
  149.   Serial.print(xyz.value.z);
  150. }
  151.  
  152. void setup()
  153. {
  154.   Serial.begin(9600);  // start serial for output
  155.   Wire.begin();        // join i2c bus (address optional for master)
  156.   setupCompass(COMPASSADDR);
  157.   setupAccel(ACCELADDR);
  158.   setupGyro(GYROADDR);
  159. }
  160.  
  161. void loop()
  162. {
  163.   union XYZBuffer compass,gyro,accel;
  164.   int l1,l2,l3,l4;
  165.   l1=analogRead(0);
  166.   l2=analogRead(1);
  167.   l3=analogRead(2);
  168.   l4=analogRead(3);
  169.   readAccel(ACCELADDR,&accel);
  170.   readCompass(COMPASSADDR,&compass);
  171.   readGyro(GYROADDR,&gyro);
  172.   Serial.print("A,");
  173.   output(accel);
  174.   Serial.print(",C,");
  175.   output(compass);
  176.   Serial.print(",G,");  
  177.   output(gyro);
  178.   Serial.print(",L,");
  179.   Serial.print(l1);
  180.   Serial.print(",");
  181.   Serial.print(l2);
  182.   Serial.print(",");
  183.   Serial.print(l3);
  184.   Serial.print(",");
  185.   Serial.print(l4);
  186.   Serial.println();
  187. }
[Get Code]

Notice the different endianess between the devices on this board.

If you want to see the robot in action checkout http://www.youtube.com/mojavaton