# ardiuno & trigonometry.

Hello!
I’m having some problems getting the arduino to calculate how to tip in order to move a certain direction with an N-blade rotor (N being variable).
here is my current algorithm:

``````//VARIABLES:

float roll,pitch,throttle;
float m1 = (7/6)*PI, m2 = (PI/6), m3 = (5/6)*PI, m4 = (11/6)*PI, m5= (1.5*PI), m6 = (0.5*PI);  // the theta of all the motors, with theta=0 being the front. I have made sure that this is correct.
double adj = 0;//unused, used to be the rotational offset of the motors
double mm1,mm2,mm3,mm4,mm5,mm6;
double maxMultiplier = .06;
Servo s1,s2,s3,s4,s5,s6;

//CODE:

roll = ((float)ds2-1490)/500;//returns a value from -1.0 to 1.0 depending on the position of the stick
pitch = ((float)ds3-1490)/500;//does the above with the elevator stick
throttle = ds1;// throttle is throttle.
double rot = PI;// Pi is a default value
double rSq = 1;// 1 is a default value
if(pitch != 0 && roll !=0){//if the stick is not centered.
rot = atan2(roll,pitch);//all rotations shall be in radians
if(roll< 0){//unnessecary, but it makes the values plleasing to the eye
rot += 2*PI;
}
rSq = pitch * pitch + roll * roll;//gives the value of r (radius) squared. the square root function is CPU intensive
}else{
rot = 0;
rSq = 0;
}
//makes a sine function that zeroes at rot, and PI+rot. It also maxes out at (PI)/2+rot and minimums at rot - (PI/2)
mm1 = rSq*maxMultiplier*sin(rot-m1);//mm stands for motor multiplier.
mm2 = rSq*maxMultiplier*sin(rot-m2);
mm3 = rSq*maxMultiplier*sin(rot-m3);
mm4 = rSq*maxMultiplier*sin(rot-m4);
mm5 = rSq*maxMultiplier*sin(rot-m5);
mm6 = rSq*maxMultiplier*sin(rot-m6);

s1.write(throttle+mm1*throttle);//'s' objects are servo objects, these are used to control the speed controllers, and are all initiated at this point
s2.write(throttle+mm2*throttle);
s3.write(throttle+mm3*throttle);
s4.write(throttle+mm4*throttle);
s5.write(throttle+mm5*throttle);
s6.write(throttle+mm6*throttle);
``````

if I forgot something, then my full code canbe found here: http://pastebin.com/TQN2k8q8

here is my problem:
The drone thinks that the motors are not in the position that they are supposed to be. When I move the stick one way, some motrs will speed up, and an equal amount will slow down (good), but the motors that speed up and slow down when the stick is PI/2 from the original position wouldn’t turn the drone PI/2 degrees, but it seems to be random which ones turn on and off. anyone able to shed light on this?

tuskiomi:
anyone able to shed light on this?

When it comes to debugging

``````Serial.print()
``````

is your friend. lg, couka

I am using serial.print(). You can see it a lot in my full code, linked in OP.

Ok then... what does the Serial output look like, when the problem appears?

tuskiomi:
it seems to be random which ones turn on and off

Sounds like the trigonometric functions are not used within their first period.

lg, couka

Hi tuskiome,

I know you said the operation appears random, but could you try to characterize which motors behave which way for each control command? You might have a simple sign reversal or off by PI or so that can explain the behavior.

Create a table that shows the control function and what you expect each motor to do. Maybe they aren't installed in the right location? Are they reversed?

Sorry, but I probably won't scroll through the code looking for the math problem.

Pat.

patduino:
Sorry, but I probably won't scroll through the code looking for the math problem.

Pat.

... and almost nobody is willing to look in wastebin.com

Just a little FYI.

Arduino float and double are the same, 32-bit IEEE floating point.

Arduino has no FPU, floating point is 100x slower than 32 bit long, 50x slower than 64-bit long long. You can use integers as fixed-point or with small units to achieve your ends.

Whether you use float or long variables, if you table sines and roots in PROGMEM the even interpolated lookup will be more than a magnitude faster than using the standard functions.
Table-driven is how flight sims worked as well as they did on pre-pentium computers, even 1 MHz 6502 powered toy computers. They didn't just table trig functions, they boiled whole routines down to one step.

Is there any chance that the time spent float-calculating what you have now makes a lag that will always be a problem?

Serial.print takes time. Maybe the print statements are throwing off the timing of your motors.

Or maybe you are just plain wrong about the physics of how to make your drone move in a particular direction - gyroscopic effects and drag on the rotors come into play.

Or maybe:

``````float m1 = (7/6)*PI, m2 = (PI/6), m3 = (5/6)*PI, m4 = (11/6)*PI, m5= (1.5*PI), m6 = (0.5*PI);  // the theta of all the motors, with theta=0 being the front. I have made sure that this is correct.
``````

This won't work. The compiler calculates (7/6) using integer math because these are both integer constants, and gets 1. Change this line to

``````float m1 = (7.0/6.0)*PI, m2 = (PI/6), m3 = (5.0/6.0)*PI, m4 = (11.0/6.0)*PI, m5= (1.5*PI), m6 = (0.5*PI);  // the theta of all the motors, with theta=0 being the front. I have made sure that this is correct.
``````

and see how it goes.

Better still:

``````const double TWELVFS = PI/6; // = 2PI / 12

const float[] m = {
0, // we don't use motor zero
7 * TWELVFS,
1 * TWELVFS,
5 * TWELVFS,
11 * TWELVFS,
9 * TWELVFS,
3 * TWELVFS
};
``````

does the Due handle Flops better than the uno?
@pat I solved the problem it was in my PEMDAS.
@aarg this IS a programming board, no?

tuskiomi:
@aarg this IS a programming board, no?

Yes it is. Which is why it has a provision for adding attachments to messages. Also has detailed instructions on how to post code.

floating point is 100x slower than 32 bit long, 50x slower than 64-bit long long

This isn’t true, if you’re talking about multiply and (especially) divide. a floating point divide is actually faster than a 32bit divide, usually (because it’s only 24 bits.) The 64bit integer support for AVR is supposed to be particularly poor, too (generic C code, not optimized for the 8bit AVR.)

tuskiomi:
does the Due handle Flops better than the uno?

The processing power of an Atmega328P is more than enough to fly an hexacopter. If the program is executed too slow, it's inefficient. We are talking about seconds and milliseconds here, that's a lot of time to do stuff for a 16MHz-µC.

lg, couka

westfw:
This isn't true, if you're talking about multiply and (especially) divide. a floating point divide is actually faster than a 32bit divide, usually (because it's only 24 bits.) The 64bit integer support for AVR is supposed to be particularly poor, too (generic C code, not optimized for the 8bit AVR.)

Karma to you!

I'll still put table lookup as miles ahead of using trig functions or any long equations that don't reduce to few or one compiled constant.

Moore was using table-lookup to steer big telescopes with hardware available in 1959. That's where the Forth core came from.

so, I should be using RAM-loaded table lookups instead of functions like ATAN2() and SIN(), or would using a taylor series be faster?

tuskiomi:
so, I should be using RAM-loaded table lookups instead of functions like ATAN2() and SIN(), or would using a taylor series be faster?

How long does your current process take to work out the next set of rotor commands?
How fast does it need to?
My point is about the lag vs the need but it's your device, you decide.

................

The Arduino/GCC trig functions likely use some kind of optimized finite series already.

Table not in RAM, but flash/PROGMEM. And if you can, table whole final equations rather than just trig functions. Like a table organized by angle and radius that has coordinates in the array elements or a reverse table to give Cartesian to Polar. The lookup should be way, way faster than calculating.

As for table space, an ATmega328P has 32K flash. A big sketch with many libs may use half of that. There are AVR's with 64K to 256K flash.

As for RAM, there are external parallel RAM cards for Mega2560 boards. For about \$25 you can have 8 banks of 56K each that access direct same as internal RAM but 1 cycle slower. The internal 8K RAM goes to dedicated stack use.

Slower but still amazingly fast would be tables on SD. With gigs of space you needn't interpolate at all.
You can get or make an SD adapter cheap.

One member here sells a shield compatible Pro-duino board with an ATmega1284P overclocked to 24MHz. That one has 16K RAM and 128K flash.

And then there's the Due and other ARM-duinos that run 48MHz to 96MHz 32-bit ops.....

You have loads of options. Keep them in mind.

I see, the average loop() function time is only 33us, but the lag is more like 1/4 of a second.
any thoughts on why that may be?

Main_Project_file.ino (8.11 KB)

What does "lag" mean in your case?

lg, couka

tuskiomi:
I see, the average loop() function time is only 33us, but the lag is more like 1/4 of a second.
any thoughts on why that may be?

It only performs the calculations once in many loops.

Make a test sketch that does the calculations in a function called 10,000 times in a for-next or while loop and time that. Make it so the compiler doesn't optimize the calculating out.