arduino pid

system · February 19, 2012, 6:33am

Hello,
I am working on a quadrotor stabilisation project, for this I am using a 6DOF IMU digital combo board, I have obtained the complementary filtered angle values for it, however when I try to define 3 or more PID’s, the arduino returns no output, the serial monitor remains empty, but when I cut the pid’s to 2 it starts working again, how can i define more pid’s? Is the arduino not working because of the load of all the: IMU digital reading code, complementary filter code and pid all together?
Here I am using 2 pid’s for a single axis because, for setpoint 0, when angle is >0, a reverse pid is required and for angles<0 ,a direct pid is required. Is this logic true? Please help…

-Arvind Sanjeev

system · February 19, 2012, 9:05am

You could be running out of memory.

I'm sorry but I can't comment on the pid thing (that's something I'm planning to refresh soon).

system · February 19, 2012, 10:57am

Not because of memory because it shows 12000 bytes of a 30700 bytes maximum.

retrolefty · February 19, 2012, 11:01am

ars: Not because of memory because it shows 12000 bytes of a 30700 bytes maximum.

The 2K SRAM limit is the memory problem most run up against. That SRAM is used for variables, constants, arrays, stack space, etc and the compiler gives you no stats on SRAM usage. The stat you showed is the FLASH memory consumption for holding the program code.

Lefty

system · February 19, 2012, 1:45pm

S=But i have seen people making quadrotors using arduino, how would have they done it?

system · February 19, 2012, 1:50pm

Which Arduino do you have?

What code are you running?

It's really hard to diagnose problems given practically no information.

system · February 19, 2012, 6:06pm

Im having an arduino decimilia with an ATMega 328

#include <Wire.h> 

#include <PID_v1.h>


double Setpoint, Input, Output,Setpoint1, Input1, Output1,Setpoint0, Input0, Output0,Setpoint2, Input2, Output2;


//double aggKp=4, aggKi=0.2, aggKd=1;
double consKp=1, consKi=2, consKd=0.1;


PID myPID(&Input, &Output, &Setpoint, consKp, consKi, consKd, DIRECT);//motor 4
PID myPID0(&Input0, &Output0, &Setpoint0, consKp, consKi, consKd, REVERSE);//motor 2
PID myPID1(&Input1, &Output1, &Setpoint1, consKp, consKi, consKd, DIRECT);//motor 3
PID myPID2(&Input2, &Output2, &Setpoint2, consKp, consKi, consKd, REVERSE);//motor 1

 #include <Servo.h>
Servo g_servo1, g_servo2, g_servo3, g_servo4;







#define ACC (0x53)   
#define A_TO_READ (6)       
#define GYRO 0x69 
#define G_SMPLRT_DIV 0x15
#define G_DLPF_FS 0x16
#define G_INT_CFG 0x17
#define G_PWR_MGM 0x3E

#define G_TO_READ 8 

int g_offx = 120;
int g_offy = 20;
int g_offz = 93;


#define RAD_TO_DEG 57.295779513082320876798154814105
int x,y;
int gyroZeroX;
float gyroXadc;
float gyroXrate;
float gyroXangle;

int gyroZeroY;
float gyroYadc;
float gyroYrate;
float gyroYangle;

int gyroZeroZ;
float gyroZadc;
float gyroZrate;
float gyroZangle;



int accZeroX;
float accXadc;
float accXval;
float accXangle;

int accZeroY;
float accYadc;
float accYval;
float accYangle;

int accZeroZ;
float accZadc;
float accZval;
float accZangle;



float compAngleX;
float compAngleY;
float compAngleZ;

float R;

unsigned long timer=0;
unsigned long dtime=0;
int acc[3];
int gyro[4];





void getGyroscopeData(int * result)
{
  int regAddress = 0x1B;
  int temp, x, y, z;
  byte buff[G_TO_READ];
  readFrom(GYRO, regAddress, G_TO_READ, buff);
  result[0] = ((buff[2] << 8) | buff[3]) + g_offx;
  result[1] = ((buff[4] << 8) | buff[5]) + g_offy;
  result[2] = ((buff[6] << 8) | buff[7]) + g_offz;
  result[3] = (buff[0] << 8) | buff[1]; 
 }



void initAcc() {
  writeTo(ACC, 0x2D, 0);      
  writeTo(ACC, 0x2D, 16);
  writeTo(ACC, 0x2D, 8);
 }




void getAccelerometerData(int * result) {
  int regAddress = 0x32;   
  byte buff[A_TO_READ];
  readFrom(ACC, regAddress, A_TO_READ, buff); 
  result[0] = (((int)buff[1]) << 8) | buff[0];   
  result[1] = (((int)buff[3])<< 8) | buff[2];
  result[2] = (((int)buff[5]) << 8) | buff[4];
}




void initGyro()
{
   writeTo(GYRO, G_PWR_MGM, 0x00);
  writeTo(GYRO, G_SMPLRT_DIV, 0x07); 
  writeTo(GYRO, G_DLPF_FS, 0x1E); 
  writeTo(GYRO, G_INT_CFG, 0x00);
}








void setup()
{
  
  g_servo1.attach(3);
  g_servo2.attach(4);
  g_servo3.attach(5);
  g_servo4.attach(6); 
  
  Serial.begin(57600);
  Wire.begin();
  initAcc();
  initGyro();
  
 
  gyroZeroX = calibrateGyroX();
  gyroZeroY = calibrateGyroY();  
  gyroZeroZ = calibrateGyroZ();
  
  accZeroX = calibrateAccX();
  accZeroY = calibrateAccY();
  accZeroZ = calibrateAccZ();
 /* myPID.SetMode(AUTOMATIC);
  myPID1.SetMode(AUTOMATIC); 
   myPID0.SetMode(AUTOMATIC);
   myPID2.SetMode(AUTOMATIC);
  
    Input=x;
  Input0=x;
  Input1=y;
  Input2=y;
  Setpoint=3;
  Setpoint0=3;
  Setpoint1=-9;
  Setpoint2=-9;
  
  myPID.SetOutputLimits(63,80);
   myPID1.SetOutputLimits(63,80);
   myPID0.SetOutputLimits(63,80);
    myPID2.SetOutputLimits(63,80);*/
  
  timer=millis();
}

void loop()
{
 timer = millis();
  if(timer<5000)
 {
  g_servo1.write(10);
   g_servo3.write(10);
   g_servo2.write(10);
   g_servo4.write(10); 
 Serial.println(timer);
 }
  
  else{


  
  
  
    getAccelerometerData(acc);
  getGyroscopeData(gyro);

  
  
  gyroXadc = gyro[0];
  gyroXrate = (gyroXadc-gyroZeroX)/1.0323;
  gyroYadc = gyro[1];
  gyroYrate = (gyroYadc-gyroZeroY)/1.0323;
  
  gyroZadc = gyro[2];
  gyroZrate = (gyroZadc-gyroZeroZ)/1.0323;
  
  accXadc = acc[0];
  accXval = (accXadc-accZeroX)/102.3;
  
  accYadc = acc[1];
  accYval = (accYadc-accZeroY)/102.3;
  
  accZadc = acc[2];
  accZval = (accZadc-accZeroZ)/102.3;
  accZval++;
  
  R = sqrt(pow(accXval,2)+pow(accYval,2)+pow(accZval,2));
  accXangle = acos(accXval/R)*RAD_TO_DEG-90;
  accYangle = acos(accYval/R)*RAD_TO_DEG-90;
  accZangle = acos(accZval/R)*RAD_TO_DEG;
  


  
  dtime = millis()-timer;
  timer = millis();
  
  compAngleX = (0.98*(compAngleX+(gyroXrate*dtime/1000)))+(0.02*(accXangle));
  compAngleY = (0.98*(compAngleY+(gyroYrate*dtime/1000)))+(0.02*(accYangle));
  compAngleZ = (0.98*(compAngleZ+(gyroZrate*dtime/1000)))+(0.02*(accZangle));

  x=compAngleX/100;
  y=compAngleY/100;


/*  Input=x;
  Input0=x;
  Input1=y;
  Input2=y;
  Setpoint=3;
  Setpoint0=3;
  Setpoint1=-9;
  Setpoint2=-9;

 myPID.SetTunings(consKp, consKi, consKd);
 myPID0.SetTunings(consKp, consKi, consKd);
 myPID1.SetTunings(consKp, consKi, consKd);
 myPID2.SetTunings(consKp, consKi, consKd);

myPID.Compute();
myPID0.Compute();
myPID1.Compute();
myPID2.Compute();




g_servo4.write(Output);
g_servo2.write(Output0);
g_servo3.write(Output1);
g_servo1.write(Output2);*/

//Serial.print(Output);Serial.print("\t");Serial.print(Output0);Serial.print("\t");Serial.print(Output1);Serial.print("\t");Serial.print(Output2);Serial.println("");
 Serial.print(x);Serial.print("\t");
  Serial.print(y);Serial.print("\t");
  Serial.print(compAngleZ);Serial.print("\t");
  Serial.print(Output);Serial.print("\t");

  }





}





void writeTo(int DEVICE, byte address, byte val) {
   Wire.beginTransmission(DEVICE); 
   Wire.send(address);       
   Wire.send(val);        
   Wire.endTransmission(); 
}



void readFrom(int DEVICE, byte address, int num, byte buff[]) {
  Wire.beginTransmission(DEVICE);  
  Wire.send(address);       
  Wire.endTransmission(); 
  
  Wire.beginTransmission(DEVICE); 
  Wire.requestFrom(DEVICE, num);   
  
  int i = 0;
  while(Wire.available())    
  { 
    buff[i] = Wire.receive();
    i++;
  }
  Wire.endTransmission();
}

system · February 19, 2012, 6:17pm

IMHO it’s important to see PID_v1 too…

system · February 19, 2012, 6:26pm

I'm pretty sure that all the quadracopters I've looked at use Mega 2560s, so have 8KB of SRAM to play with, instead of 2KB in the Atmega 328 based Arduinos.

And mromani is right. We need to see the library you are using.

system · February 19, 2012, 6:39pm

Also, IIRC most of the quadcopter projects I saw on the 'net are open, i.e. they provide details about the devices and the code. I have no idea about how to build a quadcopter, but I think if I'd start to work on one then I'd certainly have a look at some of the other people's code...

system · February 19, 2012, 7:48pm

mromani: Also, IIRC most of the quadcopter projects I saw on the 'net are open, i.e. they provide details about the devices and the code. I have no idea about how to build a quadcopter, but I think if I'd start to work on one then I'd certainly have a look at some of the other people's code...

Quite right. Take a look at diy drones (http://diydrones.com/) for a start, there is a load of info there.

system · February 19, 2012, 8:04pm

ars: S=But i have seen people making quadrotors using arduino, how would have they done it?

Following the link given by dxw00d I've landed here:

http://store.diydrones.com/ArduPilot_Mega_kit_p/kt-apm-01.htm

I see only "mega" mentioned there... ;-)

system · February 20, 2012, 6:16am

Thanks a lot, so I guess Mega is the way to go..

system · May 6, 2021, 11:26am