Go Down

Topic: code for analog imu need to convert to digital imu, been reading for days :( (Read 809 times) previous topic - next topic

fozz18

I'm trying to build a self balancing unicycle. I found good code from what I see (however I am still very new to the code) Problem is I have a digital IMU 6DOF ITG3200/ADXL345 and I'm trying to get my project off the ground. I would greatly appreciate some help code is listed below.

Code: [Select]

int accly = 1;
int acclz = 2;
int gyrox = 3;

int voltsaccly;
int voltsacclz;
int voltsgyrox;

float gaccly;
float gaccly2;
float gacclz;
float gacclz2;

int ggyrox;

float GYRO_X_INIT_VAL = 423.0;
//--------------------------------------
const float INIT_ANGLE = 0.05; //Adjust this number to your balance point

const int ANGLE_BUFFER_LENGTH = 1;  //  
//-------------------------------------
const float SumErrMax = 20;
const float SumErrMin = -20;

//Tuning Knobs
const int analogInPin1 = 0;  // Analog input pin that the potentiometer is attached to
const int analogInPin2 = 4;  // Analog input pin that the potentiometer is attached to
const int analogInPin3 = 5;  // Analog input pin that the potentiometer is attached to

int sensorValue1 = 0;        // value read from the pot
int sensorValue2 = 0;        // value read from the pot
int sensorValue3 = 0;        // value read from the pot
int outputValue1 = 0;        // value output to the P
int outputValue2 = 0;        // value output to the I
int outputValue3 = 0;        // value output to the D

// <editor-fold defaultstate="collapsed" desc="Global Variables">

int VxAcc = 0, VyAcc = 0, VzAcc = 0;
int VxGyro = 0, VzGyro = 0;
float degX = 0.0f, degY = 0.0f, degZ = 0.0f;

float throttle = 0.0;

float accAngle = 0.0, currAngle = 0.0, prevAngle = 0.0, elapsedTimeSec=0.0;
unsigned long prevTimeStamp = 0, currTimeStamp = 0, elapsedTime = 0;

boolean isFirstTime = true;

float angleBuffer[ANGLE_BUFFER_LENGTH];
int angleBufferIndex = 0;

boolean MotorOff = false;

float curErr = 0, prevErr = 0, SumErr = 0;
float integralTerm = 0, derivativeTerm = 0;
float Cn = 0;

//float Kp = 0.0, Ki = 0.0,  Kd = 0.0;  //hard code pid numbers instead of pot


//MOTOR SETUP
const int bhi = 8;    //on 5v
const int ahi = 7;    //on 5v
const int ali = 6;    //reverse pwm
const int bli = 5;    //forward pwm
const int dis = 4;    //disable 5v=yes=coast

enum WheelDirection {
   DIR_FORWARD,
   DIR_BACKWARD,
   DIR_STOP
};

// CALIBRATE GYRO hold still for 3 seconds on startup. gyro will average 25 readings

float CalGyro() {

   float val = 0.0;

   delay(500);

   for (int i = 0; i < 25; i++) {

       val += analogRead(gyrox);

       delay(10);

     }

   val /= 25.0;

   
   return val;

}

float GetAvgAngle() {  //angle buffer used as a low pass filter
                      //increase anglebufferlength to average over more readings
   int count = 0;    //set anglebuffer to 1 for no averaging

   float a = 0;



   for (int i = 0; i < ANGLE_BUFFER_LENGTH; i++) {

       if (angleBuffer[i] != 0) {

           count++;

           a += angleBuffer[i];

       }

   }
   
   if (count > 0)

       return a / float(count);

   else
   
       return 0.0;

}

float GetAccAngle() {

   float r = sqrt((float) gacclz2 * (float) gacclz2 + (float) gaccly2 * (float) gaccly2);

   accAngle = (float) gacclz2 / r; //approximates sine
   

   return accAngle;

}

float EstimateAngle() {

   currTimeStamp = micros();

   elapsedTime = currTimeStamp - prevTimeStamp;

   prevTimeStamp = currTimeStamp;



   elapsedTimeSec = (float) elapsedTime / 1e6f;



   float angle = GetAccAngle();

   if (isFirstTime) {

       currAngle = angle;

       isFirstTime = false;

   } else {

       VxGyro = analogRead(gyrox);

       degX = (float) (VxGyro - GYRO_X_INIT_VAL) * 3300.0f / 512.0f / 180.0f * (float) elapsedTimeSec;
       degX = degX * - 1.0f;    // Invert Gyro Reading
       currAngle = 0.995 * (prevAngle + degX) + 0.005 * angle;

   }

   prevAngle = currAngle;


   if (MotorOff) { //for debugging purposes

       Serial.print("\tcurrAngleAvg=");

       Serial.print(GetAvgAngle(), 4);

       Serial.print("\tcurrAngle=");

       Serial.print(currAngle, 4);

       Serial.print("\taccAngle=");

       Serial.print(accAngle);

       Serial.print("\tgyroAngle=");

       Serial.print(degX + prevAngle);    //* 2864.788975 = degrees

       Serial.print("\tVxGyro=");

       Serial.print(VxGyro);

       Serial.print("\telapsedTime=");

       Serial.print(elapsedTime);

       Serial.print("\tthrottle=");

       Serial.println(throttle);

   }

   return currAngle;
   }

// MOTOR DRIVE
void MoveWheels(WheelDirection (dir), int speed) {

   if (MotorOff) return;

   if (speed > 255) speed = 255;

   else if (speed < 0) speed = 0;

   if (dir == DIR_STOP) {

       digitalWrite(ahi, HIGH);      //set ahi and bhi osmc
       digitalWrite(bhi, HIGH);
       digitalWrite(dis, LOW);
       digitalWrite(ali, LOW);
       digitalWrite(bli, LOW);

   } else if (dir == DIR_FORWARD) {

       digitalWrite(ahi, HIGH);      //set ahi and bhi osmc
       digitalWrite(bhi, HIGH);
       digitalWrite(dis, LOW);
       digitalWrite(ali, LOW);
       analogWrite(bli, speed);


   } else if (dir == DIR_BACKWARD) {

       digitalWrite(ahi, HIGH);      //set ahi and bhi osmc
       digitalWrite(bhi, HIGH);
       digitalWrite(dis, LOW);
       analogWrite(ali, speed);
       digitalWrite(bli, LOW);
   }

}


void setup()
{
 Serial.begin(19200);

 pinMode(accly, INPUT);
 pinMode(acclz, INPUT);
 pinMode(gyrox, INPUT);
 
 analogReference (EXTERNAL);    //gyro and acl use 3.3vref, connect 3.3v to vref

GYRO_X_INIT_VAL = CalGyro();    //gyro calibrates once on startup hold still for 3 seconds

       pinMode(ahi, OUTPUT);        //enable motor control pins
       pinMode(bhi, OUTPUT);
       pinMode(dis, OUTPUT);
       pinMode(ali, OUTPUT);
       pinMode(bli, OUTPUT);
       
       digitalWrite(ahi, HIGH);      //set motor to stop
       digitalWrite(bhi, HIGH);
       digitalWrite(dis, LOW);
       digitalWrite(ali, LOW);
       digitalWrite(bli, LOW);
 

}

void loop()
{

// READ ACCELEROMETERS AND GYRO  

// Y ACCELEROMETER
voltsaccly = analogRead (1);
gaccly = voltsaccly * 3.3 / 1024;  
gaccly2 = (gaccly - 1.368) / .3;

// Z ACCELEROMETER
voltsacclz = analogRead (2);
gacclz = voltsacclz * 3.3 / 1024;  
gacclz2 = (gacclz - 1.460) / .3;

// read the tuning knob value:
 sensorValue1 = analogRead (analogInPin1);
 sensorValue2 = analogRead (analogInPin2);
 sensorValue3 = analogRead (analogInPin3);

 
 // map tuning knob to the range of the analog out:
 outputValue1 = map(sensorValue1, 0, 1023, 0, 2000);    //P Knob
 outputValue2 = map(sensorValue2, 0, 1023, 0, 20000);    //I Knob
 outputValue3 = map(sensorValue3, 0, 1023, 0, 1000);    //D Knob
 

//PID CALCULATION
   
   //Get Estimated Angle
   float a = EstimateAngle() - INIT_ANGLE;


   //Buffer the angle get an average
   angleBuffer[angleBufferIndex] = a;

   angleBufferIndex = (angleBufferIndex + 1) % ANGLE_BUFFER_LENGTH;

   float ang = GetAvgAngle();


   //CURRENT ERROR
   curErr = ang - INIT_ANGLE; //error  

   SumErr += curErr;

   if (SumErr > SumErrMax) SumErr = SumErrMax;

   else if (SumErr < SumErrMin) SumErr = SumErrMin;

   // Ki*SumE/(Kp*Fs*X)
   
   // INTEGRAL TERM
   integralTerm = SumErr * elapsedTimeSec * float(outputValue2) / float(outputValue1);
   
   // DERIVATIVE TERM
   derivativeTerm = curErr - prevErr;

   // Kd(curErr-prevErr)*Ts/(Kp*X)
   
   derivativeTerm = derivativeTerm * 10000 * float(outputValue3) * elapsedTimeSec / float(outputValue1);

   //PID CALC
   Cn = (curErr + integralTerm + derivativeTerm) * float(outputValue1);

   //MOTOR DRIVE

   WheelDirection dir;

   if (Cn > 0) dir = DIR_FORWARD;

   else if (Cn < -0) dir = DIR_BACKWARD;

   else dir = DIR_STOP;

   throttle = abs(Cn);

   if (abs(ang) > 0.7)  MoveWheels(DIR_STOP, 0); //if angle too large to correct, stop motor      

   else MoveWheels(dir, throttle);
   
   prevErr = curErr;
   
}

PeterH


Problem is I have a digital IMU 6DOF ITG3200/ADXL345 and I'm trying to get my project off the ground.


How is that a problem?

You've chosen a difficult project. If you don't feel able to tackle it yourself I suggest you ask for help in the Gigs and Collaborations section of the forum. Note that most people would expect to be paid for doing your project for you. If you just want advice to help you tackle specific problems, you need to explain what the problem is. However, while you will find plenty of people here offering advice to help you fix the problem, don't expect people to solve the problem for you.
I only provide help via the forum - please do not contact me for private consultancy.

Erdin

A few notes:

- Please add your code between the code tags. If you write a post, above the text input fields are buttons. One of the buttons is the '#' button for the code tags. Please modify your first post and add those tags around your code.
- Are you using the analog inputs for "accly = 1", "acclz = 2", "gyrox = 3" ? So you need 3-axis accelerometer and 3-axis gyro ?
- You would need two parts of code, for the ITG3200 and the ADXL345.

ITG3200
http://www.sparkfun.com/tutorials/265

ADXL345
Search for ADXL345 in the Playground section, http://playground.arduino.cc//Main/InterfacingWithHardware#InputTOC
https://www.sparkfun.com/products/9156
http://www.adafruit.com/products/1231

This combines both, with a link to code, http://www.watterott.com/de/IMU-Digital-Combo-Board-6-Degrees-of-Freedom-ITG3200/ADXL345

You have a lot more reading to do.....

fozz18

do you know anyone that would be interested in making some money that is proficient enough for this project? For the most part people can tell me to use a analog imu but they are discontinued

Erdin

Those IMU or 6DOF or 9DOF boards are digitial. But you still can buy breakout boards with an only a analog gyro or with only analog a accel sensor. So you need perhaps two breakout boards.

fozz18

would you mind posting a couple of links?
I found
https://www.sparkfun.com/products/9652        ( for the accelerometer)
https://www.sparkfun.com/products/11341       ( for the gyro)

Erdin

You can start with this list in the Playground section, http://playground.arduino.cc//Main/InterfacingWithHardware#motion

A cheap analog accel sensor is the MMA7360 and MMA7361.
The cost only a few dollars on Ebay.
The ADXL335 costs a little more.

The analog gyro sensors cost a lot more than the digital ones. And they often are just one or two axis.
I found an analog IMU on Ebay, search for "5DOF ADXL335".

PeterH


do you know anyone that would be interested in making some money that is proficient enough for this project?


If you are looking for somebody to do the project for you, post details of what you want in the Gigs and Collaborations section of the forum.
I only provide help via the forum - please do not contact me for private consultancy.

Go Up