UM6 and the Arduino

system · November 9, 2012, 10:49pm

Hello! I am new to both the arduino and the CH Robotics UM6 Sensor. I have this code that I have been trying to get to simply print out the sensor values. I have the CH Robotics UM6 Orientaton Sensor connected to the Arduino Mega 2560 via the MISO and MOSI pins. When I run the program, which prints out the values from the sensors, I get something along the lines of :
Data
255
255
255
255
255
255
255
255
255
255
255
255
255
255
255
Data
0
0
0
0
128
128
0
0
0
0
128
128
0
0
128
Data
0
0
0
0
128
128
0
0
0
0
128
128
0
0
128
Data
255
255
255
255
255
255
255
255
255
255
255
255
255
255
255

With the 255 repeating until I terminate the program. These values are not manipulated. I took out the conversion factors to see if I was receiving the right values from the sensor. I have checked to make sure the the UM6 is grounded to the Arduino and that the MOSI, MISO, SS, and SCK are all connected to the right pins. Any help would be appreciated. I have been completely lost for the past couple weeks. Below is the datasheet and my code. Thank you!

datasheet:
http://www.chrobotics.com/docs/UM6_datasheet.pdf

#include <SPI.h>

#define SPI_MOSI   51  //pin 3
#define SPI_MISO   50  //pin 4
#define SPI_SCK    52  //pin 5
#define SPI_SS     53  
//#define BAUD    115200
#define BAUD    57600

#define GYRO_RAW_XY         0x56  //2's Compliment
#define GYRO_RAW_Z          0x57  //2's Compliment
#define ACCEL_RAW_XY        0x58  //2's Compliment
#define ACCEL_RAW_Z         0x59  //2's Compliment
#define GYRO_PROC_XY        0x5C
#define GYRO_PROC_Z         0x5D
#define ACCEL_PROC_XY       0x5E
#define ACCEL_PROC_Z        0x5F
#define REG_EULER_PHI_THETA 0X62
#define REG_EULER_PSI       0x63

#define FLASH_COMMIT        0xAB
#define ZERO_GYROS          0xAC
#define SET_ACCEL_REF       0xAF

#define BAD_CHECKSUM        0xFD
#define UNKNOWN_ADDRESS     0xFE
#define INVALID_BATCH_SIZE  0xFF


#define GYRO_SCALE_FACTOR   0.0610352     // Convert register data to Degrees per Second
#define ACCEL_SCALE_FACTOR  0.000183105   // Convert register data to Gravities
#define EULER_SCALE_FACTOR  0.0109863     // Convert register data to Degrees
#define PT_HAS_DATA         0x80  //10000000
#define PT_IS_BATCH         0x40  //01000000
//#define PT_BATCH_LEN        0x10  //Batch length = 4
#define PT_BATCH_LEN        0x08  //Batch length = 2

int hold[4];  //byte or convert immediately, 1st byte multiply by 2^7, 2nd convert to decimal and add 

struct EULER{
  int data;
} phi, theta, psi;

struct GYRO{
  int data;
} gx, gy, gz, gxp, gyp, gzp;

struct ACCEL{
  int data;
} ax, ay, az, axp, ayp, azp;

void setup(){                                                                   // http://en.wikipedia.org/wiki/Serial_Peripheral_Interface_Bus
  SPI.begin();
  SPI.setBitOrder(MSBFIRST);            // Options LSB or MSB
  SPI.setDataMode(SPI_MODE1);           // Options 0, 1, 2, 3
  SPI.setClockDivider(SPI_CLOCK_DIV8);  // Options 2, 4, 8, 16, 32, 64, 128
  digitalWrite(SPI_SS, HIGH);
  Serial.begin(9600);
}

void loop(){
  readRegister(GYRO_RAW_XY);                  // FIGURE OUT HOW TO ACTUALLY READ THE REGISTER FROM THE UM6.
    gx.data = convert(1);
    gy.data = convert(2);
  readRegister(GYRO_RAW_Z);
    gz.data = convert(1);
  
  readRegister(ACCEL_RAW_XY);
    ax.data = convert(1);
    ay.data = convert(2);  
  readRegister(ACCEL_RAW_Z);
    az.data = convert(1);
  
  readRegister(GYRO_PROC_XY);
    gxp.data = convert(1);// * GYRO_SCALE_FACTOR;
    gyp.data = convert(2); // * GYRO_SCALE_FACTOR;
  readRegister(GYRO_PROC_Z);
    gzp.data = convert(1); // * GYRO_SCALE_FACTOR;
  
  readRegister(ACCEL_PROC_XY);
    axp.data = convert(1) * ACCEL_SCALE_FACTOR;
    ayp.data = convert(2) * ACCEL_SCALE_FACTOR;
  readRegister(ACCEL_PROC_Z);
    azp.data = convert(1) * ACCEL_SCALE_FACTOR;
  
  readRegister(REG_EULER_PHI_THETA);
    phi.data = convert(1); // * EULER_SCALE_FACTOR;
    theta.data = convert(2); // * EULER_SCALE_FACTOR;
  readRegister(REG_EULER_PSI);
    psi.data = convert(1); // * EULER_SCALE_FACTOR;
    
  
  Serial.println("Data");
  Serial.println(gx.data, DEC);
  Serial.println(gxp.data, DEC);
  Serial.println(gy.data, DEC);
  Serial.println(gyp.data, DEC);
  Serial.println(gz.data, DEC);
  Serial.println(gzp.data, DEC);
  Serial.println(ax.data, DEC);
  Serial.println(axp.data, DEC);
  Serial.println(ay.data, DEC);
  Serial.println(ayp.data, DEC);
  Serial.println(az.data, DEC);
  Serial.println(azp.data, DEC);
  Serial.println(phi.data, DEC);
  Serial.println(theta.data, DEC);
  Serial.println(psi.data, DEC);
  delay(1000);
  
  hold[0] = 0;
  hold[1] = 0;
  hold[2] = 0;
  hold[3] = 0;
}

void readRegister(int reg){
  digitalWrite(SPI_SS, LOW);
    SPI.transfer(0x00);                                //  DEFINE
    SPI.transfer(reg);
    //SPI.transfer(0x00);
    hold[0] = SPI.transfer(0x00);
    hold[1] = SPI.transfer(0x00);
    hold[2] = SPI.transfer(0x00);
    hold[3] = SPI.transfer(0x00);
    //SPI.transfer(0x00);
  digitalWrite(SPI_SS, HIGH);
}

int convert(int pos) {                               // Fix this so that it works.
  int sum = 0;
  
  if(pos == 1){  
    hold[0] << 8;
    hold[0] |= hold[1];
    sum = hold[0]; 
  }
  
  if(pos == 2){
    hold[2] << 8;
    hold[2] |= hold[3];
    sum = hold[2];
  }
      
  return sum;
}

nickgammon · November 9, 2012, 11:15pm

From the datasheet:

The UM6 SPI clock (SCK) is active low, with data clocked in on the first falling edge.

That's mode 2, not mode 1 (SPI_MODE2).

The maximum clock rate is 400kHz.

Your clock is too fast. Try SPI_CLOCK_DIV64.

system · November 12, 2012, 9:44pm

Ok, I fixed the clock and data mode and now I am getting different numbers!

Data
255
255
255
255
255
255
255
255
255
255
255
255
255
255
255
Data
128
128
128
128
128
128
128
128
128
128
128
128
128
128
128
Data
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0

Is there the possibility that I am converting the information wrong?

Thank you for all of your help!

nickgammon · November 12, 2012, 10:26pm

    hold[0] << 8;

That won't do much. You want:

    hold[0] <<= 8;

Ditto in all the similar places.

system · November 13, 2012, 12:14am

ok, I changed the hold[0] <<= 8; part. However, I am still getting the same number for all of the values I am trying to gather from the sensor. This time they are all either -3460, -1, or 0. Would I be reading the register/data wrong?

nickgammon · November 13, 2012, 5:13am

Can you show your changed code please? And the new output.

system · November 14, 2012, 9:15pm

Yes!
Here is the output:

Data
0
0
26463
26463
15104
0
0
0
26463
26463
0
0
0
26463
0
Data
0
0
26463
26463
15104
0
0
0
26463
26463
0
0
0
26463
0

And, at random times every value will be -1.

Here is the updated code:

#include <SPI.h>

#define SPI_MOSI   51  //pin 3
#define SPI_MISO   50  //pin 4
#define SPI_SCK    52  //pin 5
#define SPI_SS     53  
//#define BAUD    115200
#define BAUD    57600

#define GYRO_RAW_XY         0x56  //2's Compliment
#define GYRO_RAW_Z          0x57  //2's Compliment
#define ACCEL_RAW_XY        0x58  //2's Compliment
#define ACCEL_RAW_Z         0x59  //2's Compliment
#define GYRO_PROC_XY        0x5C
#define GYRO_PROC_Z         0x5D
#define ACCEL_PROC_XY       0x5E
#define ACCEL_PROC_Z        0x5F
#define REG_EULER_PHI_THETA 0X62
#define REG_EULER_PSI       0x63

#define FLASH_COMMIT        0xAB
#define ZERO_GYROS          0xAC
#define SET_ACCEL_REF       0xAF

#define BAD_CHECKSUM        0xFD
#define UNKNOWN_ADDRESS     0xFE
#define INVALID_BATCH_SIZE  0xFF


#define GYRO_SCALE_FACTOR   0.0610352     // Convert register data to Degrees per Second
#define ACCEL_SCALE_FACTOR  0.000183105   // Convert register data to Gravities
#define EULER_SCALE_FACTOR  0.0109863     // Convert register data to Degrees
#define PT_HAS_DATA         0x80  //10000000
#define PT_IS_BATCH         0x40  //01000000
//#define PT_BATCH_LEN        0x10  //Batch length = 4
#define PT_BATCH_LEN        0x08  //Batch length = 2

int hold[4];  //byte or convert immediately, 1st byte multiply by 2^7, 2nd convert to decimal and add 

struct EULER{
  int data;
} phi, theta, psi;

struct GYRO{
  int data;
} gx, gy, gz, gxp, gyp, gzp;

struct ACCEL{
  int data;
} ax, ay, az, axp, ayp, azp;

void setup(){                                                                   // http://en.wikipedia.org/wiki/Serial_Peripheral_Interface_Bus
  SPI.begin();
  SPI.setBitOrder(MSBFIRST);            // Options LSB or MSB
  SPI.setDataMode(SPI_MODE2);           // Options 0, 1, 2, 3
  SPI.setClockDivider(SPI_CLOCK_DIV64);  // Options 2, 4, 8, 16, 32, 64, 128
  digitalWrite(SPI_SS, HIGH);
  Serial.begin(9600);
}

void loop(){
  readRegister(GYRO_RAW_XY);                  // FIGURE OUT HOW TO ACTUALLY READ THE REGISTER FROM THE UM6.
    gx.data = convert(1);
    gy.data = convert(2);
  readRegister(GYRO_RAW_Z);
    gz.data = convert(1);
  
  readRegister(ACCEL_RAW_XY);
    ax.data = convert(1);
    ay.data = convert(2);  
  readRegister(ACCEL_RAW_Z);
    az.data = convert(1);
  
  readRegister(GYRO_PROC_XY);
    gxp.data = convert(1); //* GYRO_SCALE_FACTOR;
    gyp.data = convert(2);// * GYRO_SCALE_FACTOR;
  readRegister(GYRO_PROC_Z);
    gzp.data = convert(1);// * GYRO_SCALE_FACTOR;
  
  readRegister(ACCEL_PROC_XY);
    axp.data = convert(1);// * ACCEL_SCALE_FACTOR;
    ayp.data = convert(2);// * ACCEL_SCALE_FACTOR;
  readRegister(ACCEL_PROC_Z);
    azp.data = convert(1);// * ACCEL_SCALE_FACTOR;
  
  readRegister(REG_EULER_PHI_THETA);
    phi.data = convert(1);// * EULER_SCALE_FACTOR;
    theta.data = convert(2);// * EULER_SCALE_FACTOR;
  readRegister(REG_EULER_PSI);
    psi.data = convert(1);// * EULER_SCALE_FACTOR;
    
  
  Serial.println("Data");
  Serial.println(gx.data, DEC);
  Serial.println(gxp.data, DEC);
  Serial.println(gy.data, DEC);
  Serial.println(gyp.data, DEC);
  Serial.println(gz.data, DEC);
  Serial.println(gzp.data, DEC);
  Serial.println(ax.data, DEC);
  Serial.println(axp.data, DEC);
  Serial.println(ay.data, DEC);
  Serial.println(ayp.data, DEC);
  Serial.println(az.data, DEC);
  Serial.println(azp.data, DEC);
  Serial.println(phi.data, DEC);
  Serial.println(theta.data, DEC);
  Serial.println(psi.data, DEC);
  delay(1000);
  
  hold[0] = 0;
  hold[1] = 0;
  hold[2] = 0;
  hold[3] = 0;
}

void readRegister(int reg){
  digitalWrite(SPI_SS, LOW);
    SPI.transfer(0x00);                                //  DEFINE
    SPI.transfer(reg);
    //SPI.transfer(0x00);
    hold[0] = SPI.transfer(0x00);
    hold[1] = SPI.transfer(0x00);
    hold[2] = SPI.transfer(0x00);
    hold[3] = SPI.transfer(0x00);
    //SPI.transfer(0x00);
  digitalWrite(SPI_SS, HIGH);
}

int convert(int pos) {                               // Fix this so that it works.
  int sum = 0;
  
  if(pos == 1){  
    hold[0] <<= 8;
    hold[0] |= hold[1];
    sum = hold[0]; 
  }
  
  if(pos == 2){
    hold[2] <<= 8;
    hold[2] |= hold[3];
    sum = hold[2];
  }
      
  return sum;
}

nickgammon · November 15, 2012, 12:11am

I can't see anything obviously wrong with your code. Can you take a photo? In your shoes I would be using the logic analyzer to see what is going on, if anything. Something seems to be because the data changes.

Topic		Replies	Views
How to Interface the CH Robotics UM6 with the Arduino Programming	10	6299	May 5, 2021
Polling chr-um6 General Guidance	2	910	May 5, 2021
Polling a UM6-LT with an Arduino Uno (code included) Sensors	3	8591	May 6, 2021
Quaternions, Euler Angles and CH Robotics UM6 Programming	3	8645	May 5, 2021
Arudino UNO and CHR-6d IMU Programming	9	1111	May 5, 2021

UM6 and the Arduino

Related topics