For loop and serial.print problem

Forum 2005-2010 (read only) Software Syntax & Programs
1

Hello everyone.
I have built a quadrotor helicopter that uses the Arduino as a control platform and I am having some problems with the code, which I will show bellow:

#include <Wire.h> //Library for I2C implementation using analog pins 4 (SDA) and 5 (SCL)
#include <MegaServo.h>

#define NBR_SERVOS 4  // the maximum number of servos (up to 12 on Arduino Diecimilia)
#define FIRST_SERVO_PIN 6 // define motor pins on the Arduino board
#define SECOND_SERVO_PIN 9 
#define THIRD_SERVO_PIN 10
#define FOURTH_SERVO_PIN 11

MegaServo Servos[NBR_SERVOS] ; // max servos

//Accelerometer variables
float ax,ay,az; //Acceleration values in m/s^2 for all three axis
float as=0.004883; //Arduino analog resolution = 5Volts/1024 (10bits)
float zg=1.65; //Sensor zero g bias = Supply Voltage/2 =3.3/2 (V)
float sR=0.329; //Sensor resolution (V/g)

//Compass variables
int compassAddress = 0x42 >> 1;  // compass I2C slave adress: 0x42 
                                 // because the wire.h library only uses the 7 bits most significant bits
                                 // a shift necessary is to get the most significant bits.
int psi = 0;                 // Compass heading = yaw (direct readings from the compass in degrees)
float psirad;           // Compass heading = yaw in radians

//Euler angles Roll and pitch
float phi; // (rad)
float theta; // (rad)

//Kalman filter data
const float A[6][6] = {
{1.0000,0.0000,0.0000,-0.5985,-0.0000,0.0000},
{0.0000,1.0000,-0.0000,-0.0000,-0.5985,0.0000},
{-0.0000,-0.0000,1.0000,-0.0000,-0.0000,-0.0479},
{0.0500,0.0000,0.0000,0.3384,-0.0000,-0.0000},
{0.0000,0.0500,-0.0000,-0.0000,0.3384,0.0000},
{-0.0000,0.0000,0.0500,0.0000,0.0000,0.9147}
};//(Kalman state-space matrix Ak)

const float B[6][10] = {
{-0.0000,-0.0188,-0.0000,0.0188,0.0000,0.0302,-0.0000,0.0031,-0.0000,-0.0000},
{0.0194,0.0006,-0.0194,-0.0006,-0.0302,-0.0000,-0.0000,-0.0000,0.0031,-0.0000},
{0.0009,-0.0009,0.0009,-0.0009,-0.0000,0.0000,0.0000,0.0000,0.0000,0.0479},
{-0.0000,0.0002,-0.0000,-0.0002,0.0000,0.0334,-0.0000,0.0034,-0.0000,0.0000},
{0.0005,0.0006,-0.0005,-0.0006,-0.0334,-0.0000,-0.0000,-0.0000,0.0034,-0.0000},
{0.0000,-0.0000,0.0000,-0.0000,0.0000,0.0000,0.0000,0.0000,-0.0000,0.0853}
}; //(Kalman state-space matrix Bk)

float x[6] = {0,0,0,0,0,0}; //state vector: Roll speed, Pitch Speed, Yaw speed, Roll angle, Pitch angle, Yaw angle
float u[10] = {0,0,0,0,0,0,0,0,0,0}; //input vector: w1 (motor 1 speed),w2,w3,w4,ax,ay,az,phi,theta,yaw
float At[6] = {0,0,0,0,0,0}; //temp variable
float Bt[6] = {0,0,0,-0.01,-0.04,0}; //temp variable

//LQR gain matrix
const float K[4][6] = {
{0.0000,7.1030,20.4163,0.0000,18.2839,14.4659},
{-7.1030,0.0000,-20.4163,-18.2839,0.0000,-14.4659},
{0.0000,-7.1030,20.4163,0.0000,-18.2839,14.4659},
{7.1030,0.0000,-20.4163,18.2839,0.0000,-14.4659}
};

float w1,w2,w3,w4; //Motor inputs from LQR controller (rad.s^-1)
float w0; //Throttle to the motors (from joystick)

//Motor inputs for PWM signal
float wc1=0;
float wc2=0;
float wc3=0;
float wc4=0; 

//PWM signals
int p1 = 200;
int p2 = 200;
int p3 = 200;
int p4 = 200;

//Reference
float yr[6]={0,0,0,0,0,0};

void setup()
{
  Wire.begin();
  Serial.begin(9600); // Initialize serial communications with setup
  
  Servos[0].attach( FIRST_SERVO_PIN, 800, 2200); //Motor 1 - North - Clockwise rotation
  Servos[1].attach( SECOND_SERVO_PIN, 800, 2200); //Motor 2 - East - Counterclockwise rotation
  Servos[2].attach( THIRD_SERVO_PIN, 800, 2200); //Motor 3 - South - Clockwise rotation
  Servos[3].attach( FOURTH_SERVO_PIN, 800, 2200); // Motor 4 - West - Counterclockwise rotation
  
  //Put the motors in full stop
  Servos[0].write(0);  
  Servos[1].write(0);  
  Servos[2].write(0); 
  Servos[3].write(0);
}

void loop()
{
  // get the most recent acceleration values for all three axis
  ax = ((analogRead(0)*as-zg)/sR)*9.81; //acceleration x-axis (m.s^-2)
  ay = ((analogRead(1)*as-zg)/sR)*9.81; //acceleration y-axis (m.s^-2)
  az = ((analogRead(2)*as-zg)/sR)*9.81; //acceleration z-axis (m.s^-2)
  
  ax=-ax; //This correction allows the accelerometer to provide positive acceleration in the x-axis direction
  
  // Compass task 1: connect to the HMC6352 sensor
  Wire.beginTransmission(compassAddress);
  Wire.send('A');             // The ascii character "A" tells the compass sensor to send data
  Wire.endTransmission();
  
  // Compass task 2: wait for data processing
  delay(50);   // Compass datasheet says we need to wait at least 6000 microsegundos (0.006s) for data processing the sensor
               //We can also take this opotunity to implement the sampling time (10Hz)
  
  
  // Compass task 3: Request heading
  Wire.requestFrom(compassAddress, 2);    // request 2 bytes of data
  
  // Compass task 4: Get heading data 
  if(2 <= Wire.available())    // if 2 bytes are available
  { 
    //16bit numbers can be broken in two 8 bit chunks. The first is called high byte and the second low byte
    psi = Wire.receive();  // get high byte
    psi = psi << 8;    // shift high byte
    psi += Wire.receive(); // get low byte
    psi /= 10;           // comment this line if you wish to get the heading in decidegrees instead of degrees
  }
  psi=0;
  
  psirad = deg2rad((float)psi); // Map the compass reading from 0 to 359 degrees to -pi radians to pi radians
  phi=atan(ay/az); //Calculate roll angle from the accelerations provided by the accelerometer (rad)
  theta=-atan(ax/az); //Calculate pitch angle from the accelerations provided by the accelerometer (rad)
  
  //Perform Kalman filtering
  
  At[0]=A[0][0]*x[0]+A[0][1]*x[1]+A[0][2]*x[2]+A[0][3]*x[3]+A[0][4]*x[4]+A[0][5]*x[5];  
  At[1]=A[1][0]*x[0]+A[1][1]*x[1]+A[1][2]*x[2]+A[1][3]*x[3]+A[1][4]*x[4]+A[1][5]*x[5]; 
  At[2]=A[2][0]*x[0]+A[2][1]*x[1]+A[2][2]*x[2]+A[2][3]*x[3]+A[2][4]*x[4]+A[2][5]*x[5];  
  At[3]=A[3][0]*x[0]+A[3][1]*x[1]+A[3][2]*x[2]+A[3][3]*x[3]+A[3][4]*x[4]+A[3][5]*x[5]; 
  At[4]=A[4][0]*x[0]+A[4][1]*x[1]+A[4][2]*x[2]+A[4][3]*x[3]+A[4][4]*x[4]+A[4][5]*x[5]; 
  At[5]=A[5][0]*x[0]+A[5][1]*x[1]+A[5][2]*x[2]+A[5][3]*x[3]+A[5][4]*x[4]+A[5][5]*x[5];
  
  /*for(int i=0;i<=5;i++)
  {
    At[i]=A[i][0]*x[0]+A[i][1]*x[1]+A[i][2]*x[2]+A[i][3]*x[3]+A[i][4]*x[4]+A[i][5]*x[5];
  }*/
  
    //Get throttle from the joystick
  if (Serial.available() > 0) 
  {
            // read the incoming byte:
            w0 = map((float)Serial.read(), 0, 50, 0, 600);
                w0=constrain(w0,0,600); //Limit maximum velocity to 50 rad/s

    Serial.flush();
  }
  else
  w0 = 0;
  
  //place known inputs and measurements in the input vector u
  u[0]=wc1-w0;
  u[1]=wc2-w0;
  u[2]=wc3-w0;
  u[3]=wc4-w0;
  u[4]=ax-yr[0];
  u[5]=ay-yr[1];
  u[6]=az-yr[2];
  u[7]=phi-yr[3];
  u[8]=theta-yr[4];
  u[9]=psirad-yr[5];; 
      
  Bt[0]=B[0][0]*u[0]+B[0][1]*u[1]+B[0][2]*u[2]+B[0][3]*u[3]+B[0][4]*u[4]+B[0][5]*u[5]+B[0][6]*u[6]+B[0][7]*u[7]+B[0][8]*u[8]+B[0][9]*u[9];
  Bt[1]=B[1][0]*u[0]+B[1][1]*u[1]+B[1][2]*u[2]+B[1][3]*u[3]+B[1][4]*u[4]+B[1][5]*u[5]+B[1][6]*u[6]+B[1][7]*u[7]+B[1][8]*u[8]+B[1][9]*u[9];
  Bt[2]=B[2][0]*u[0]+B[2][1]*u[1]+B[2][2]*u[2]+B[2][3]*u[3]+B[2][4]*u[4]+B[2][5]*u[5]+B[2][6]*u[6]+B[2][7]*u[7]+B[2][8]*u[8]+B[2][9]*u[9];
  Bt[3]=B[3][0]*u[0]+B[3][1]*u[1]+B[3][2]*u[2]+B[3][3]*u[3]+B[3][4]*u[4]+B[3][5]*u[5]+B[3][6]*u[6]+B[3][7]*u[7]+B[3][8]*u[8]+B[3][9]*u[9];
  Bt[4]=B[4][0]*u[0]+B[4][1]*u[1]+B[4][2]*u[2]+B[4][3]*u[3]+B[4][4]*u[4]+B[4][5]*u[5]+B[4][6]*u[6]+B[4][7]*u[7]+B[4][8]*u[8]+B[4][9]*u[9];
  Bt[5]=B[5][0]*u[0]+B[5][1]*u[1]+B[5][2]*u[2]+B[5][3]*u[3]+B[5][4]*u[4]+B[5][5]*u[5]+B[5][6]*u[6]+B[5][7]*u[7]+B[5][8]*u[8]+B[5][9]*u[9];
  
  /*for(int i=0;i<=5;i++)
  {
    Bt[i]=B[i][0]*u[0]+B[i][1]*u[1]+B[i][2]*u[2]+B[i][3]*u[3]+B[i][4]*u[4]+B[i][5]*u[5]+B[i][6]*u[6]+B[i][7]*u[7]+B[i][8]*u[8]+B[i][9]*u[9];
  }*/
  
  //Get estimated states x(k+1)=A.x(k)+B.u(k) 
  x[0]=At[0]+Bt[0];
  x[1]=At[1]+Bt[1];
  x[2]=At[2]+Bt[2];
  x[3]=At[3]+Bt[3];
  x[4]=At[4]+Bt[4];
  x[5]=At[5]+Bt[5];
  /*for(int i=0;i<=5;i++)
  {
    x[i]=At[i]+Bt[i];
  }*/
  
  //Controlo LQR
  w1=K[0][0]*x[0]+K[0][1]*x[1]+K[0][2]*x[2]+K[0][3]*x[3]+K[0][4]*x[4]+K[0][5]*x[5];  
  w2=K[1][0]*x[0]+K[1][1]*x[1]+K[1][2]*x[2]+K[1][3]*x[3]+K[1][4]*x[4]+K[1][5]*x[5]; 
  w3=K[2][0]*x[0]+K[2][1]*x[1]+K[2][2]*x[2]+K[2][3]*x[3]+K[2][4]*x[4]+K[2][5]*x[5];  
  w4=K[3][0]*x[0]+K[3][1]*x[1]+K[3][2]*x[2]+K[3][3]*x[3]+K[3][4]*x[4]+K[3][5]*x[5];
  
  
  //Adjust speeds with the throttle
  wc1=w0-w1;
  wc2=w0-w2;
  wc3=w0-w3;
  wc4=w0-w4;
  
  //Calculate and send pulse width to the motors 
  p1=(int)((wc1/2.0276)+1252); // (speed of motor 1 (rad/s) / transfer function gain) + Dead-zone pulse width
  p2=(int)((wc2/1.8693)+1262); 
  p3=(int)((wc3/2.0018)+1255); 
  p4=(int)((wc4/1.9986)+1255);
  
  
  if(w0==0) //Forçar paragem
  {
    p1=400;
    p2=400;
    p3=400;
    p4=400;
    }
  
  Servos[0].write(p1);
  Servos[1].write(p2);
  Servos[2].write(p3);
  Servos[3].write(p4);
    
  // output sensor data
  Serial.print(ax);
  Serial.print(" ");
  Serial.print(ay); 
  Serial.print(" ");
  Serial.print(az);
  Serial.print(" ");
  Serial.print(phi);
  Serial.print(" ");
  Serial.print(theta);
  Serial.print(" ");
  Serial.print(psirad);
  Serial.println("");
}
2

This code includes some Kalman filtering and estimation, as well as a LQR controller (I will be happy to explain the code if anyone asks).
Particularly, I have reached the space limit available for a program in the Arduino. As such, I am trying to make the code more efficient, thus releasing extra program space. If you give a quick look at the program you[ch8217]ll see that I am working with large matrix operations. Right next to them I have the equivalent code in the form of a [ch8220]for loop[ch8221], which I hoped to implement as an effort to release extra space, which worked quite well. The problem is that as soon as I change to [ch8220]for loops[ch8221] instead of the detailed matrix operation, I can no longer print variables through the serial port.
Example:
x[0]=At[0]+Bt[0];
x[1]=At[1]+Bt[1];
x[2]=At[2]+Bt[2];
x[3]=At[3]+Bt[3];
x[4]=At[4]+Bt[4];
x[5]=At[5]+Bt[5];
With this code I can print variables through [ch8220]serial.print [ch8220], but with this one:
for(int i=0;i<=5;i++)
{
x=At_+Bt*;
 }*
I cannot. Is this a bug or am I doing something wrong?
Thanks in advance._

3

Jael,

What kind of Arduino are you using?

-Mike

4

Hi Mike,

I'm using the Duemilanove board.

5

Jael,

The reason I asked was to see if you might be running out of RAM. But, with 2K available, the code you published should not be exceeding the limits.

I can't see any problem with your code. I agree that the 'for' loops should be the same as the inline assignments.

Of course, there must be much more code than you showed, since I can't imagine that code exceeding 32k of program memory. Does the additional code also use a lot of RAM?

Regards,

-Mike

6

Hello,

the main reason is in missing ram... Instead of const float you should use this: Flash | Arduiniana . It will store the huge ammmount of const variables to Flash instead of loading them all to RAM.

I had exactly the same problem before, when i used LCD and i had a lot of texts in my program. The behavior was exactly same, some basic for loops stopped working.

Hope this will help.

Jan

PS: if you are in foubt with memory, try to get how much memory is free (some for cycle with malloc and free and rising ammount of allocationg memory). If there was free less than 200b, the program i had was doing some unexpected things.

Mooving all three conft float arrays will free at least 500b of memory!

Jan

7

Using flash to hold the const arrays will slow down the calculations. That may not matter for an LCD display but could be unacceptable in this application.

Although the symptoms do look like insufficient RAM, the code as posted should fit on a Duemilinove (unless the unposted code does consume a lot more ram.

BTW, you may want to check that you are using the latest version of the MegaServo code (the header file should have MegaServo_VERSION 2 – the Arduino Servo library uses this version and it uses slightly less RAM when less than 12 servos are used.
You can shave off a few more bytes (up to 40 bytes) if you recompile the library after defining MAX_SERVOS to 4 (the default is 12) but this limits the number of servos to 4 and should only be necessary if you are right up against the limit of RAM

8

First of all, let me thank you for your help.

The reason I asked was to see if you might be running out of RAM. But, with 2K available, the code you published should not be exceeding the limits.

I can't see any problem with your code. I agree that the 'for' loops should be the same as the inline assignments.

Of course, there must be much more code than you showed, since I can't imagine that code exceeding 32k of program memory. Does the additional code also use a lot of RAM?

Actually, i just noticed i missed a little bit of code in the end, but that is it:

float deg2rad (float x) //function that receives an angle between 0 and 359 degrees and resturns the same angle between -pi and pi radians
{
  if(x>=0 && x<=180)
  {
  x=-x*3.14/180;
  }
  else
  {
  x = (360-x)*3.14/180;
  }
  return x;
}

Also, if i don't use the "for loops", i can't compile the program because it will exceed the 14336 Bytes available for the bynary sketch file. In this case i have to comment a few of my serial.prints in order to fit the program into the Arduino.

Hello,

the main reason is in missing ram... Instead of const float you should use this: Flash | Arduiniana . It will store the huge ammmount of const variables to Flash instead of loading them all to RAM.

I had exactly the same problem before, when i used LCD and i had a lot of texts in my program. The behavior was exactly same, some basic for loops stopped working.

Hope this will help.

Jan

PS: if you are in foubt with memory, try to get how much memory is free (some for cycle with malloc and free and rising ammount of allocationg memory). If there was free less than 200b, the program i had was doing some unexpected things.

Mooving all three conft float arrays will free at least 500b of memory!

Jan

Jan, as mem mentioned, i need my calculations to be as quick as possible in order to mantain a given sample time. Thank you for your help :slight_smile: i didn't even know i could store variables in the flash memory of the Arduino.

Using flash to hold the const arrays will slow down the calculations. That may not matter for an LCD display but could be unacceptable in this application.

Although the symptoms do look like insufficient RAM, the code as posted should fit on a Duemilinove (unless the unposted code does consume a lot more ram.

BTW, you may want to check that you are using the latest version of the MegaServo code (the header file should have MegaServo_VERSION 2 [ch8211] the Arduino Servo library uses this version and it uses slightly less RAM when less than 12 servos are used.
You can shave off a few more bytes (up to 40 bytes) if you recompile the library after defining MAX_SERVOS to 4 (the default is 12) but this limits the number of servos to 4 and should only be necessary if you are right up against the limit of RAM

mem, actually i'm using the MegaServo Library to drive fours brushless motors. I just have to figure out how to recompile the lib as you suggested. Nevertheless, if i use the "for loops" i can free about 2000 bytes in the binary sketch, which i would hope to use to do some more extra calculations.. but if i am getting out of RAM, i guess that won't be possible..

9

Does your Duemilanove have an Atmega168 chip? If so, then you are running out of memory and it will be easier to upgrade that to an Atmega328 chip then it will be to squeeze everything into the 168 (the 328 has twice the ram and flash).

To recompile the library, you delete the file named MegaServo.o in the libraries/MegaServo . Change the header file as per the earlier post and then rebuild your sketch.

10

Yes, my Duemilanove has an Atmega168 chip. I guess i will have to order the 328. If i am successful with the upgrade, i'll report it here in the Forum.

11

With ATMega 168 you are definitely out of memory.

If you are running out of flash, you can use 1k bootloader, which wil gie you extra 1k flash http://spiffie.org/know/arduino_1k_bootloader/ . I used it with 168 and it worked...

Does enybody know, how slower will it be to use flash memory instead of variables?

Jan

12

BTW... as i saw the data in const float arrays... wont it use less memory when store integers - the best it would be int8_t, which is -127..127 and divide it by float constant? It will use less memory, integer aritmetics is much more faster... And finaly divide it by some constant to have result you need?

Jan

13

int8_t, which is -127..127

sp. "-128..127"

14

yes... -128..127, but i am never sure which border is higher... :slight_smile:

15

Jan, if i understand it correctly, in order to burn the bootloader i must have an AVR ISP In-System Programmer. Currently i do not have this hardware, but i thank you for your suggestion, i did not know about this bootloader that can free an extra 1K of RAM.

BTW... as i saw the data in const float arrays... wont it use less memory when store integers - the best it would be int8_t, which is -127..127 and divide it by float constant? It will use less memory, integer aritmetics is much more faster... And finaly divide it by some constant to have result you need?

I think i understand what you are saying.. for example, let us consider these variables:

A=1

A'=0.01

Then:

(A/100)*(A/100) provides the same result as (A'*A') while using less RAM.

16

Does enybody know, how slower will it be to use flash memory instead of variables?

reading an array of ints from progmem is eight times slower then reading from RAM

17

Or... just instead of using the A array just use:

  At[0]=x[0]-0.5985*x[3];  
  At[1]=x[1]-0.5985*x[4];
  At[2]=x[2]-0.0479*x[5];  
  At[3]=0.05*x[0]+0.3384*x[3];
  At[4]=0.05*x[1]+0.3384*x[4];
  At[5]=0.05*x[2]+0.9147*x[5];

The same bith Bt and B.

The code will be little less readable... BUT it will be MUCH, MUCH faster and MUCH, MUCH lower code and MUCH, MUCH lower RAM.

:slight_smile:

18

and const float can make less code:

const float as=0.004883; //Arduino analog resolution = 5Volts/1024 (10bits)
const float zg=1.65; //Sensor zero g bias = Supply Voltage/2 =3.3/2 (V)
const float sR=0.329; //Sensor resolution (V/g)

Better code will definitely fit in ATMega 168 chip and can be much faster.

Jan

19

That code does reduce few hundred bytes on my code, thanks :). Nevertheless i still can't figure out how does less code (e.g. by using the for loops) spends more RAM then without the for loops. I mean.. i am not allocating huge amounts of data, it is just a simple for loop.

Thank you for your help.

20

OK, you can use:

float A[2][6] = {{1.0,1.0,1.0,0.05,0.05,0.05},{-0.5985,-0.5985,-0.0479,0.3384,0.3384,0.9147}};

for(a=0; a<6; a++)
    At[a]=A[0][a]*x[a%3]+A[1][a]*x[a%3+3];

It will little increase RAM, but it will reduce size.

But dont use the matrixes with a lot of zeros...

Jan