Strange compound addition with float's

Using Arduino Programming Questions
1

Hello everyone!
I am building a quadrocopter controlled by arduino, everything went well and I am almost done. But now I have a problem, sofar I managed to stabilize the quadrocopter over one axis. Now I need to communicate with the aduino, to tell the quadrocopter where to go. So I wrote a simple program to send strings like <352> via serial so arduino can read them. No problems yet, I can store the received value in an int, but when I want to do math with it, things go wrong. The current angle, is stored in the float x_gyro_final. To controle the quadrocopter, I want to manipulate this variable, by appending the controle value to it. Since the controle value can by -1000 to 1000 , I want to divide it by a thousand. When I try to do this the angle changes in a strange way. When The controle value is positive, the angle sometimes decreases instead of increases. When the controll value is negative the angle decrements, but way to fast.
I dont get what is wrong. because when I divide the controll value by 10, everything works fine.

I am so confused, please help me

Here is my code

#include <Servo.h>

Servo ESC1;
Servo ESC2;

double AngleSpeedX = 0; 
double hoekX = 0;
double X = 0;
double total = 0;
double zero = 0;
double x_gyro_final = 0.0;
double x_gyro_final_2 = 0;
double x_acc = 0;
double curr_as = 0;
double prev_as = 0;
double ESC1_Value = 0;
int count = 0;
int count2 = 20;
int delay_value = 0;
int target_acc = 0;
int target_speed = 0;
unsigned long previousMillis, currMillis;

String serIn = "";
String x_cont_s = "";
int x_cont = 0;
int x_cont_i = 0;
double x_div = 1000.00;
int string_length = 0;
char buf[] = "";

int test = 0;

void setup()
{
  
 
  ESC1.attach(3);
  ESC2.attach(5);
  ESC1.writeMicroseconds(1000);
  ESC2.writeMicroseconds(1000);
                            
 //   while(count2 > 0) //20 seconden delay
 // {
  //  count2 -= 1;
  //  Serial.print(count2);
   // delay(1000);
  //}                     
  
  
  
  analogReference(EXTERNAL);
  Serial.begin(9600);
  while(count < 1000)
  {
    count ++;
    total += analogRead(A4) * ((3.3 / 1024)* 1000) ;
    zero = total / count;
    delay(10);
  }
}

void loop()
{
  // serial part
  x_cont = 0;
  if(Serial.available()) {  
    
    while (Serial.available()>0)
    {      
    serIn += byte( Serial.read());
    delay(5);		
    }
     delay(1);
     
     if(serIn.startsWith("<") && serIn.endsWith(">") )
     {
       string_length = serIn.length() - 1;
       x_cont_s = serIn.substring(1,string_length);
       x_cont_s.toCharArray(buf,9);
       
      
       x_cont_s = "";
     }
     
  serIn = "";
  }  
  
  
  
  //angle part
  AngleSpeedX = (analogRead(A4) *((3.3 / 1024) * 1000)) - zero;
  
  X = AngleSpeedX / 9.1;
  currMillis = millis();
  delay_value = currMillis - previousMillis;
  previousMillis = currMillis;
  x_cont_i = atoi(buf);
  x_gyro_final += ((X * ( 1000 / delay_value))/ 450)  ;
   
     //problem
     x_gyro_final += x_cont_i / 1000.0; //problem when I change 10.0 to a larger number, I get strange results.
     //problem
  
  curr_as = AngleSpeedX;
  x_acc = curr_as - prev_as;
  prev_as = curr_as;
  
   Serial.println(x_gyro_final); // debug
  
  if(x_gyro_final < 0)
  {
    target_speed = 2;
  }
  else if(x_gyro_final > 0)
  {
    target_speed = -2;
  }
  if(X < target_speed)
  {
    target_acc = 2;
  }
  if( X > target_speed )
  {
    target_acc = -2;
  }
  
  ESC1_Value += (target_acc - x_acc) / 5  ; // esc value to pwm 
  // safty
  if(ESC1_Value > 100)
  {
    ESC1_Value = 100;
  }
  
  if(ESC1_Value < -100)
  {
    ESC1_Value = -100;
  }
  //esc 
  ESC1.writeMicroseconds(1270 + ESC1_Value);
  ESC2.writeMicroseconds(1270 - ESC1_Value);
  
 // Serial.print(ESC1_Value);
 // Serial.print("/");
  //Serial.println(x_gyro_final);
  //Serial.print("/");
  //Serial.print(x_acc);
  //Serial.println();
  delay(5); 
  
}
2 
    while (Serial.available()>0)
    {      
    serIn += byte( Serial.read());
    delay(5);		
    }
     delay(1);
     
     if(serIn.startsWith("<") && serIn.endsWith(">") )

What's with the delay()s? When sending delimited data, read it as fast as possible. The cast to a byte is unnecessary.

     x_gyro_final += x_cont_i / 1000.0; //problem when I change 10.0 to a larger number, I get strange results.

When variables of different types are involved, the results can be other than what you expect. Since x_cont_i is an integer, the result of the division will be an int. Since the value in x_cont_i is less than 1000 in your example, the result of the division will always be 0.

You can cast x_cont_i to a float before dividing by 1000.0, to get the results that you want:

     x_gyro_final += ((float)x_cont_i / 1000.0);
3

I just tried that, but it wont work. The results are still very strange.

4

Have you checked if analogRead(A4), Anglespeed, X and x_cont_i have the expected values and whether x_gyro_final is of the expected values with the inputs of the other values?

If not, the error might be occurring earlier on and you look in the wrong place.
If the you have the input values for the bothersome calculation, please post them so we get some idea of what order of magnitudes of values we talk about.

Korman

5

That's not the problem. because when I delete the "x_gyro_final += ((float)x_cont_i / 1000.0) ;" line
everyting is normal. (except 2 degreees gyro drift per minute). note that it works only when I divide x_cont_i by 10.
Thanks for the replys

6

You may get numerical instabilities if the order of magnitude of the result smaller in the result than in the input values. The easiest way to check for that is by looking at the input values - in this case the old value of x_gyro_final and x_cont_i.

Korman

7

Sorry but I dont think I understand you, english is not my native language.

But isnt there an other way to do what I want to accomplish? Maybee that wil help.

8

ozo123:
Maybee that wil help.

No, that didn't help at all because you didn't post the data I asked for.

ozo123:
But isnt there an other way to do what I want to accomplish?

Your problem might lie in the way how the algorithm is calculated: It's mathematically correct but may be numerical unstable. If this is the case, it's just a matter of transforming the calculation to make it stable in the right value range. Even if that sounds like gibberish to you, don't worry, just post the data.

Korman

9

ok I wil post he data as soon as possible.

10

I don't see any obvious errors. When x_cont_i is divided by the float constant 1000.0 it will be promoted to a float first. The explicit cast should not be needed.

I'd put some debug Serial.print()'s in to check the values of buf, x_cont_i, and the result of x_cont_i/1000.0 to see where things are going wrong.

It is possible that you are encountering a compiler error. If so, no amount of looking at the source code will help. The only way to know is to look at the variables before and after each operation to see where the compiler is getting the math wrong.

11

I already tried that. x_cont_i/ 1000 gives me right data(send data comes back divided by 1000). So the problem must be the += operation.

12

ozo123:
I already tried that. x_cont_i/ 1000 gives me right data(send data comes back divided by 1000). So the problem must be the += operation.

I assumed as much. So what was the value of x_cont_i? 345?
And what was the value of x_gyro_final before the operation? Perhaps -0.344992?

With the correct values it would be possible to check if you get truncation errors.

Korman

13

ozo123:
I already tried that. x_cont_i/ 1000 gives me right data(send data comes back divided by 1000). So the problem must be the += operation.

So now you just have to print out x_gyro_final before and after the +=. I'd suggest Serial.print( x_gyro_final, 5) to print it out to 5 decimal places.

14

Floats on Arduino have a resolution of 6-7 digits only, and that could be the source of your problem here.

You are accumulating your AngleSpeedX readings into your x_gyro_final value over time, and for whatever reason your AngleSpeedX values are getting multiplied by 1000. This could quickly skew the value of x_gyro_final such that small changes can no longer be made properly.

You should constrain your angles to values in the range of 0-360º. This will keep all your floats in the best range for optimal accuracy. Also, why are you multiplying all your analog readings by 1000?

15

As a test I've added x_gyro_final += (double) 0.2 ; to my code. So the arduino appends 0.2 per sycle. This works, but when I touch the joystick, x_gyro_final changes way faster, and in a different direction,then I told the arduino to do. I am very confused! I made a video on youtube, So every one can see my problem. Very strange Arduino problem - YouTube
How can the arduino possibly do things it is'nt supposed to do? Nowhere in the code is declared that the arduino should do something, whit x_gyro_final, based on the joystick input. Here is my new code.

#include <Servo.h>

Servo ESC1;
Servo ESC2;

double AngleSpeedX = 0; 
double hoekX = 0;
double X = 0;
double total = 0;
double zero = 0;
double x_gyro_final = 0.0;
int x_gyro_final_i = 0;
double x_acc = 0;
double curr_as = 0;
double prev_as = 0;
double ESC1_Value = 0;
int count = 0;
int count2 = 20;
int delay_value = 0;
int target_acc = 0;
int target_speed = 0;
unsigned long previousMillis, currMillis;

String serIn = "";
String x_cont_s = "";
int x_cont = 0;
int x_cont_i = 0;
double x_div = 1000.00;
int string_length = 0;
char buf[] = "";

int loop_count = 0;

int test = 0;

void setup()
{
  
 
  ESC1.attach(3);
  ESC2.attach(5);
  ESC1.writeMicroseconds(1000);
  ESC2.writeMicroseconds(1000);
                            
 //   while(count2 > 0) //20 seconden delay
 // {
  //  count2 -= 1;
  //  Serial.print(count2);
   // delay(1000);
  //}                     
  
  
  
  analogReference(EXTERNAL);
  Serial.begin(9600);
  while(count < 1000)
  {
    count ++;
    total += analogRead(A4) * ((3.3 / 1024)* 1000) ;
    zero = total / count;
    delay(10);
  }
}

void loop()
{
  // serial part
  x_cont = 0;
  if(Serial.available()) {  
    
    while (Serial.available()>0)
    {      
    serIn += byte( Serial.read());
    delay(5);		
    }
     delay(1);
     
     if(serIn.startsWith("<") && serIn.endsWith(">") )
     {
       string_length = serIn.length() - 1;
       x_cont_s = serIn.substring(1,string_length);
       x_cont_s.toCharArray(buf,9);
       
      
       x_cont_s = "";
     }
     
  serIn = "";
  }  
  
  
  
  //angle part
  AngleSpeedX = (analogRead(A4) *((3.3 / 1024) * 1000)) - zero;
  
  X = AngleSpeedX / 9.1;
  currMillis = millis();
  delay_value = currMillis - previousMillis;
  previousMillis = currMillis;
  x_cont_i = atoi(buf);
   
  x_gyro_final += ((X * ( 1000 / delay_value))/ 450) ;
     
   x_gyro_final += (double) 0.2 ;
   
  Serial.println(x_gyro_final);
  
  curr_as = AngleSpeedX;
  x_acc = curr_as - prev_as;
  prev_as = curr_as;
  
  if(x_gyro_final < 0)
  {
    target_speed = 2;
  }
  else if(x_gyro_final > 0)
  {
    target_speed = -2;
  }
  if(X < target_speed)
  {
    target_acc = 2;
  }
  if( X > target_speed )
  {
    target_acc = -2;
  }
  
  ESC1_Value += (target_acc - x_acc) / 5  ; // esc value to pwm 
  // safty
  if(ESC1_Value > 100)
  {
    ESC1_Value = 100;
  }
  
  if(ESC1_Value < -100)
  {
    ESC1_Value = -100;
  }
  //esc 
  ESC1.writeMicroseconds(1270 + ESC1_Value);
  ESC2.writeMicroseconds(1270 - ESC1_Value);
  
 // Serial.print(ESC1_Value);
 // Serial.print("/");
  //Serial.println(x_gyro_final);
  //Serial.print("/");
  //Serial.print(x_acc);
  //Serial.println();
  delay(5); 
  
}
16

Is your control program sending X-axis joystick values continuously or is it just sending an update when the joystick value changes. I would suspect that the large delay introduced when you receive serial data is causing the problem because the difference between currMillis and previousMillis will be much greater.

17

I am sending joystick data continusly with a speed of 10 hz

18

I just tested if the received data causes a delay, but it is'nt. Also the value is only send 10 times a second, so that cant be the problem.
It is very strange that the arduino is doing things, that I did'nt program it to do. Nowhere in the code is specified that the arduino should modify the x_gyro_final value based on the received joystick data.
HOW CAN THIS HAPPEN?? is this a bug or something?

19

ozo123:
is this a bug or something?

Yes. Your program is not doing what you want. It's a bug.

Perhaps it is time to start over.

20

ozo123:

char buf[] = "";

:
  :
x_cont_s.toCharArray(buf,9);

My bet is that this is your problem. 'buf' is declared only one byte long (the length of "", including null termination) and you are writing 9 or 10 bytes onto it.

Try doing this instead:

char buf[10];
   :
   :
x_cont_s.toCharArray(buf,sizeof(buf)-1);

I'm uncertain about toCharArray() and how it handles length and am too lazy to look it up. Maybe the -1 isn't needed.