beginner code needs a bit of work please

Hi, i’m working on a small project for something that’ll keep a platform level. I’ve come up with a code which will work a servo and an L12 linear actuator from an IMU, its so slow because of loads of float maths, and rife with tiny little errors. I’d be really grateful if anyone could tinker with it and offer some suggestions :slight_smile:

#include<Math.h>
#include<Servo.h>
#define PI

#define AX = A0            //Pin number for accelerometer in X - which is A0
#define AY = A1            //Pin number for accelerometer in Y - which is A1
#define AZ = A2            //Pin number for accelerometer in Z - which is A2
#define GX = A3            //Pin number for gyroscope in X - which is A3
#define GY = A4            //Pin number for gyroscope in Y - which is A4
#define L12 = PD3      //Pin number for linear actuator - pin 3

float Xd = 0;            //Variable for X orientation in degrees from upright
float Yd = 0;
float Z = 0;            //Variable to determine whether inverted
float TAx = 0;            //Variable for calculating mean orientation at beginning
float TAy = 0;
Servo hitec;            //Detailing name of servo as hitec
float Errx = 0;            //Variable for calculating error in gyro
float Erry = 0;
float RGX = 0;            //Variable used for raw gyro input
float RGY = 0;
float DX = 0;           //Variable used for change (in degrees)
float DY = 0;
int setupct = 0;        //Used in calculating the change in orientation

void setup ()
{
pinMode(AX, INPUT)
pinMode(AY, INPUT)
pinMode(AZ, INPUT)
pinMode(GX, INPUT)
pinMode(GY, INPUT)
pinMode(L12, OUTPUT)
hitec.attach(2)            //Servo output on pin 2

      while (Setupct <5){                                 //Repeat this five times
              AcclX = analogRead (AX)                        //Find reading for X from accelerometer
              Xd = asin(Acclx / * 6.0 - 3.0) * 180.0 / PI    //Convert to g-forces, radians, then degrees
              Accly = analogRead (AY)                           //Find reading for y from acclerometer
              Yd = asin(Accly / * 6.0 - 3.0) * 180.0 / PI    //Convert to g-forces, radians, then degrees
              If (Z < 511) {Xd = 180.0 - Xd}
              TAx = TAx + Xd
              TAy = TAy + Yd
              Setupct = Setupct + 1
              Delay (100)
                }

Xd = Xd / 5.0                        //Find mean of Xd in the previous 5 checks
hitec.write(Xd - 90.0 * -1.0 + 90.0)        //Send servo to opposite of orientation

Yd = Yd / 5.0                          //Find mean of Yd in the previous 5 checks
Yd = Yd + 36.0 / 64.0 * 1000.0 + 1000.0        //Turn orientation into output for L12
L12.writeMicroseconds (Yd)

ErrX = analogRead(GX) + 1.0 / 1024.0 - 0.5
ErrY = analogRead(GY) + 1.0 / 1024.0 - 0.5

Delay(2200)

}

void loop ()

{

RGX = analogRead(GX)                      //Get raw reading for rate of turn
DX = RGX + 1.0 / 1024 - 0.5 - ErrX                //Process to get degrees moved in 1 millisecond
RGY = analogRead(GY)
DY = RGY + 1.0 / 1024 - 0.5 - ErrY

hitec.write(hitec.read() - DX)              //Removes change in angle from servo angle

DY = DY + 36.0 / 64.0 * 1000.0 + 1000.0        //Turn orientation into output for L12
L12.writeMicroseconds (DY)
}

i can explain some of the maths if you can’t see where the numbers come from

I'd be grateful if you could format your code consistently.

its so slow because of loads of float maths, and rife with tiny little errors

And the tiny little errors exhibit themselves...how? (I'm puzzled as to why you've decided it is slow - you haven't even run it yet)

#define AX = A0

Macro substitution.

pinMode(= A0, INPUT)

That's not "an error", that's careless. (semicolons are useful too)

#define PI

last time I looked, it was three-and-a-bit. (355/113 is probably good enough for 'float's)

Is this assignment due to be handed in any time soon?

Before you go using values like A0, A1, etc., you ought to know what they mean.

A0 is a name assigned to an analog pin WHEN IT IS USED AS A DIGITAL PIN.

Since you are doing analogRead on the pins, it makes no sense to be digital pin name.

Since they are analog pins, it makes no sense to have set pinMode for the pin, either, since analog pins are input only.

A few parentheses are in order.

ErrX = analogRead(GX) + 1.0 / 1024.0 - 0.5

Do you know what order the operations are performed in? You are adding 1/1024 to the value read from the analog pin, then subtracting 0.5 from the result. Most likely, this is not what you mean to be doing.

Is this assignment due to be handed in any time soon?

Not until it at least compiles.

yh sorry, i didn't check the format, i copied it straight from the software where it seemed to be alligned.

all i know about arduino is what i've learned from the reference pages on the site and from this forum; i had gathered that float maths can take about 100microseconds each, and there's a lot of them. I'm not making factual claims but i'm just trying to give any kind person who replies an idea of what i think the problems are.

With the macro substitution, I've never seen it written how you just have, what difference will it make.

Thank you for the advice on pi :)

And finally, A-levels don't have enough of the good stuff in them so this is something i undertook myself, taking me on a bit from BASIC.

thank you paul, i understand it that the arduino has no concept of BODMAS, so i've written it as do one function then the next quite precisely.

and how would i therefore read a value for AX from the pin A0 without referring to it as a digital pin?

yh sorry, i didn’t check the format, i copied it straight from the software where it seemed to be alligned.

I’m sure that you know what that means.

I’m not making factual claims

its so slow

Quite.

but i’m just trying to give any kind person who replies an idea of what i think the problems are.

Waiting for you to start.

I think you’re in above your head.
Slow down, work through some examples.

There is so much wrong with that code - you need to start simple.

and how would i therefore read a value for AX from the pin A0 without referring to it as a digital pin?

Pick up your Arduino. Have a real close look at it. See that wire from the sensor going into some pin on the Arduino board? See that number next to the pin? Does it say A0? Or, does it simply say 0?

In WProgram.h, the Arduino team defined a bunch of constants to be used when using the analog pins as digital pins. Is that what you are doing? No. If it were, you would be using digitalRead, not analogRead.

Since that is not what you are doing, why are you using the wrong name? Just use 0.

Hey, mellis, when are you going to get the damned examples fixed?

Ahem - I think I may have introduced that misconception, but have since apologised. If you look carefully, A0 can be correctly in this sense.

If you look carefully, A0 can be correctly in this sense.

I'm not sure how 14 can be correctly used as the pin number in analogRead. If it can, I'd like to know how, and I'll quit complaining about it being mis-used.

Well, you could look carefully at the source code (from 0021, I think)

Well, you could look carefully at the source code (from 0021, I think)

Aww, c'mon. Whack me with a bigger clue-by-four than that.