Is there a better way to write a sketch.

AntonyG · March 1, 2021, 8:34am

Hi.
I have done my sketch and it looks messy compared to others I have seen.

What can I do to improve the sketch is my question.

The project description:- Balancing robot controller using a 5D Rocker Joystick Independent Keyboard and a MPU6050. So I use the "Set" and the "Reset" buttons to change between control methods.

The sketch is working. But I am open to a better method of doing that as well.

Sketch below.
The Print to Serial monitor statement's are for debugging only and will be removed before completion of project.
Regards Antony.

#include <Wire.h>
#include <MPU6050.h>
MPU6050 mpu;
const int up = 6;
const int dwn = 7;
const int let = 8;
const int rht = 9;
const int mid = 10;
const int set = 11;
const int rst = 12;
int buttonState = 0;
int tilt_angle = 15;
int Accelerometer = 0;
byte send_byte;
void setup()
{
  while (!mpu.begin(MPU6050_SCALE_2000DPS, MPU6050_RANGE_2G))
  {
    Serial.println("Could not find a valid MPU6050 sensor, check wiring!");
    delay(500);
  }
  Serial.begin(38400);// 57600
  Serial.print(" Starting up");

  pinMode(up, INPUT_PULLUP);//               on D6
  pinMode(dwn, INPUT_PULLUP);//              on D7
  pinMode(let, INPUT_PULLUP);//              on D8
  pinMode(rht, INPUT_PULLUP);//              on D9
  pinMode(mid, INPUT_PULLUP);//              on 10
  pinMode(set, INPUT_PULLUP);//              on 11
  pinMode(rst, INPUT_PULLUP);//              on 12
}
void loop()
{
  send_byte = B00000000;
  if (Accelerometer == 1) {
    tilt_messure();
  }
  buttonState = digitalRead(let);                         //    white wire to D9
  if (buttonState == LOW) {
    send_byte |= B00000001; Serial.print(" left");        // Left Turn
  }
  buttonState = digitalRead(rht);                        //    yellow wire  to D8
  if (buttonState == LOW) {
    send_byte |= B00000010; Serial.print(" right");      // Right Turn
  }
  buttonState = digitalRead(dwn);                       //   Blue wire   to  D7
  if (buttonState == LOW) {
    send_byte |= B00000100; Serial.print(" back");     //   backwards
  }
  buttonState = digitalRead(up);                       //  red wire   to  D6
  if (buttonState == LOW) {
    send_byte |= B00001000;  Serial.print(" forward"); // forward
  }
  buttonState = digitalRead(set);                      //  yellow wire    to  D11
  if (buttonState == LOW) {
    Serial.print(" Jumping to Acc");
    Accelerometer = 1;
    tilt_messure();
  }
  /*            Add thease buttons as wanted
    buttonState = digitalRead(mid);                //  green wire   to D10
    if (buttonState == LOW) {
      send_byte |= B00001000;   // middle button
    }
  */
  Serial.print("          ..     "); Serial.println(send_byte);
  if (send_byte)Serial.print((char)send_byte);
  delay(40);

}
void tilt_messure() {
  Vector normAccel = mpu.readNormalizeAccel();
  int pitch = -(atan2(normAccel.XAxis, sqrt(normAccel.YAxis * normAccel.YAxis + normAccel.ZAxis * normAccel.ZAxis)) * 180.0) / M_PI;
  int roll = (atan2(normAccel.YAxis, normAccel.ZAxis) * 180.0) / M_PI;
  // Serial.print(" Pitch = ");
  // Serial.print(pitch);
  //  Serial.print(" Roll = ");
  //Serial.print(roll);
  Serial.println();

  if (pitch <= -tilt_angle) {
    send_byte |= B00000001;
    Serial.print(" left");// Left Turn
  }
  if (pitch >= tilt_angle) {
    send_byte |= B00000010;
    Serial.print(" right"); // Right Turn
  }
  if (roll <= -tilt_angle) {
    send_byte |= B00000100;
    Serial.print(" back");  //   backwards
  }
  if (roll >= tilt_angle) {
    send_byte |= B00001000;
    Serial.print(" forward"); // forward
  }
  Serial.print(send_byte);
  delay(10);
  buttonState = digitalRead(rst);                  // Green wire   to  D12
  if (buttonState == LOW) {
    Accelerometer = 0;
  }

}

TomGeorge · March 1, 2021, 8:46am

Hi,
To start;

const int up = 6;
const int dwn = 7;
const int let = 8;
const int rht = 9;
const int mid = 10;
const int set = 11;
const int rst = 12;

Make.

const int upPin = 6;
const int dwnPin = 7;
const int letPin = 8;
const int rhtPin = 9;
const int midPin = 10;
const int setPin = 11;
const int rstPin = 12;

Have you pressed CTRL-T in the IDE to Auto Format your code?

Tom...

UKHeliBob · March 1, 2021, 9:02am

Or even better

const byte upPin = 6;
const byte dwnPin = 7;
const byte letPin = 8;
const byte rhtPin = 9;
const byte midPin = 10;
const byte setPin = 11;
const byte rstPin = 12;

AntonyG · March 1, 2021, 10:38am

Thanks for the reply's.
Yes I can see that it is better to label things that is more meaning full. Changes Made. Will do this in the future.

UKHeliBob:
Or even better
const byte upPin = 6;

const byte dwnPin = 7;
const byte letPin = 8;
const byte rhtPin = 9;
const byte midPin = 10;
const byte setPin = 11;
const byte rstPin = 12;

Yes I did some research into this byte v int
:- Better practice and it saves on memory. Also changes made..

Thanks for your input:)
Regards Antony.

gcjr · March 1, 2021, 11:55am

AntonyG:
What can I do to improve the sketch is my question.

consider (avoid redundant code)

#if 0   // my hardware
enum { MPU6050_SCALE_2000DPS, MPU6050_RANGE_2G };

struct  Vector {
    int     XAxis;
    int     YAxis;
    int     ZAxis;
};

struct MPU6050 {
    bool begin (int a, int b) { return true; };
    Vector readNormalizeAccel (void) { return {}; };
};

const byte up  = A1;
const byte dwn = 3;
const byte let = A2;
const byte rht = 4;
const byte mid = 5;
const byte set = A3;
const byte rst = 6;

#else
#include <Wire.h>
#include <MPU6050.h>

const byte up = 6;
const byte dwn = 7;
const byte let = 8;
const byte rht = 9;
const byte mid = 10;
const byte set = 11;
const byte rst = 12;
#endif

MPU6050 mpu;

byte pins [] = { up, dwn, let, rht, mid, set, rst };

struct Cmd_s {
    byte    pin;
    byte    val;
    byte    acc;
    const char *str;
    byte    state;
};

Cmd_s cmds [] = {
    {  up,   B00001000, 0, " forward" },
    {  dwn,  B00000100, 0, " back" },
    {  let,  B00000001, 0, " left" },
    {  rht,  B00000010, 0, " right" },
    {  mid,  0,         0, " mid" },        // placeholder

    {  set,  0,         1, "" },
    {  rst,  0,         0, "" },            // unused
};

#define N_CMDS  (sizeof(cmds)/sizeof(Cmd_s))

int   buttonState = 0;
int   tilt_angle = 15;
int   Accelerometer = 0;
byte  send_byte;
const char *cmdStr;

char s [80];

// -----------------------------------------------------------------------------
void setup()
{
    while (!mpu.begin(MPU6050_SCALE_2000DPS, MPU6050_RANGE_2G))
    {
        Serial.println("Could not find a valid MPU6050 sensor, check wiring!");
        delay(500);
    }

    Serial.begin(38400);// 57600
    Serial.println (" Starting up");

    for (unsigned n = 0; n < sizeof(pins); n++)
        pinMode (pins [n], INPUT_PULLUP);
}

// -----------------------------------------------------------------------------
void loop()
{
    send_byte = B00000000;

    if (Accelerometer == 1) {
        tilt_messure();
    }

    Cmd_s  *p = cmds;
    for (unsigned n = 0; n < N_CMDS; n++, p++)  {
        buttonState = digitalRead (cmds [n].pin);
        if (p->state != buttonState)  {
            p->state = buttonState;
            delay(10);                  // debounce

            if (HIGH == p->state)
                continue;

            if (p->val)  {
                send_byte = p->val;
                cmdStr    = p->str;
            }
            else if (p->acc)
                Accelerometer = ! Accelerometer;
        }
    }

    if (send_byte)  {
        sprintf (s, "   0x%02x  %s", send_byte, cmdStr);
        Serial.println (s);
    }

    delay(40);
}

// -----------------------------------------------------------------------------
void
tilt_messure()
{
    Vector normAccel = mpu.readNormalizeAccel();
    int    pitch     = -(atan2(normAccel.XAxis, sqrt(normAccel.YAxis * normAccel.YAxis + normAccel.ZAxis * normAccel.ZAxis)) * 180.0) / M_PI;
    int    roll      =  (atan2(normAccel.YAxis, normAccel.ZAxis) * 180.0) / M_PI;

    // Serial.print(" Pitch = ");
    // Serial.print(pitch);
    //  Serial.print(" Roll = ");
    //Serial.print(roll);
    if (0)  {
        sprintf (s, "  pitch %4d, roll %4d", pitch, roll);
        Serial.println (s);
    }

    if (pitch <= -tilt_angle) {
        send_byte |= B00000001;
        cmdStr = " left";
    }

    else if (pitch >= tilt_angle) {
        send_byte |= B00000010;
        cmdStr = " right";
    }

    else if (roll <= -tilt_angle) {
        send_byte |= B00000100;
        cmdStr = " back";
    }

    else if (roll >= tilt_angle) {
        send_byte |= B00001000;
        cmdStr = " forward";
    }
    else
        Serial.println ("tile_measure:");

    // values get printed in loop()
}

6v6gt · March 1, 2021, 12:29pm

gcjr:
consider (avoid redundant code)
#if 0   // my hardware

enum { MPU6050_SCALE_2000DPS, MPU6050_RANGE_2G };

struct Vector {
int XAxis;
int YAxis;
int ZAxis;
};

struct MPU6050 {
bool begin (int a, int b) { return true; };
Vector readNormalizeAccel (void) { return {}; };
};

const byte up = A1;
const byte dwn = 3;
const byte let = A2;
const byte rht = 4;
const byte mid = 5;
const byte set = A3;
const byte rst = 6;

#else
#include <Wire.h>
#include <MPU6050.h>

const byte up = 6;
const byte dwn = 7;
const byte let = 8;
const byte rht = 9;
const byte mid = 10;
const byte set = 11;
const byte rst = 12;
#endif

MPU6050 mpu;

byte pins [] = { up, dwn, let, rht, mid, set, rst };

struct Cmd_s {
byte pin;
byte val;
byte acc;
const char *str;
byte state;
};

Cmd_s cmds [] = {
{ up, B00001000, 0, " forward" },
{ dwn, B00000100, 0, " back" },
{ let, B00000001, 0, " left" },
{ rht, B00000010, 0, " right" },
{ mid, 0, 0, " mid" }, // placeholder

{ set, 0, 1, "" },
{ rst, 0, 0, "" }, // unused
};

#define N_CMDS (sizeof(cmds)/sizeof(Cmd_s))

int buttonState = 0;
int tilt_angle = 15;
int Accelerometer = 0;
byte send_byte;
const char *cmdStr;

char s [80];

// -----------------------------------------------------------------------------
void setup()
{
while (!mpu.begin(MPU6050_SCALE_2000DPS, MPU6050_RANGE_2G))
{
Serial.println("Could not find a valid MPU6050 sensor, check wiring!");
delay(500);
}

Serial.begin(38400);// 57600
Serial.println (" Starting up");

for (unsigned n = 0; n < sizeof(pins); n++)
pinMode (pins [n], INPUT_PULLUP);
}

// -----------------------------------------------------------------------------
void loop()
{
send_byte = B00000000;

if (Accelerometer == 1) {
tilt_messure();
}

Cmd_s *p = cmds;
for (unsigned n = 0; n < N_CMDS; n++, p++) {
buttonState = digitalRead (cmds [n].pin);
if (p->state != buttonState) {
p->state = buttonState;
delay(10); // debounce

if (HIGH == p->state)
continue;

if (p->val) {
send_byte = p->val;
cmdStr = p->str;
}
else if (p->acc)
Accelerometer = ! Accelerometer;
}
}

if (send_byte) {
sprintf (s, " 0x%02x %s", send_byte, cmdStr);
Serial.println (s);
}

delay(40);
}

// -----------------------------------------------------------------------------
void
tilt_messure()
{
Vector normAccel = mpu.readNormalizeAccel();
int pitch = -(atan2(normAccel.XAxis, sqrt(normAccel.YAxis * normAccel.YAxis + normAccel.ZAxis * normAccel.ZAxis)) * 180.0) / M_PI;
int roll = (atan2(normAccel.YAxis, normAccel.ZAxis) * 180.0) / M_PI;

// Serial.print(" Pitch = ");
// Serial.print(pitch);
// Serial.print(" Roll = ");
//Serial.print(roll);
if (0) {
sprintf (s, " pitch %4d, roll %4d", pitch, roll);
Serial.println (s);
}

if (pitch <= -tilt_angle) {
send_byte |= B00000001;
cmdStr = " left";
}

else if (pitch >= tilt_angle) {
send_byte |= B00000010;
cmdStr = " right";
}

else if (roll <= -tilt_angle) {
send_byte |= B00000100;
cmdStr = " back";
}

else if (roll >= tilt_angle) {
send_byte |= B00001000;
cmdStr = " forward";
}
else
Serial.println ("tile_measure:");

// values get printed in loop()
}

You may have changed the logic from that in the OP because pitch and roll do not appear to be directly coupled there allowing, in a single iteration, say : Left Turn, Backwards. Your if / then / else if / else precludes this.

gcjr · March 1, 2021, 1:00pm

6v6gt:
You may have changed the logic from that in the OP because pitch and roll do not appear to be directly coupled there allowing, in a single iteration, say : Left Turn, Backwards. Your if / then / else if / else precludes this.

there are a lot of issues. but consider that send_byte is only sent once, so last set is the value sent.

johnwasser · March 1, 2021, 1:16pm

Warning: You have a Serial.println() in setup() before the Serial.begin().

You have Pitch (nose up, nose down) associated with left/right and Roll (tilt left, tilt right) associated with forward/backward. This will be confusing to anyone who knows what the words mean. I think your accelerometer is in sideways.

AntonyG · March 1, 2021, 2:15pm

Hi.
Gcjr .. That is a lot of strange code to me. I will study and play with it.
The only issue would be that in the button input stage it does not keep repeating the send.
I have to keep pressing the button to achieve this.
Its way interesting to compare the code.. Will play with it for a while. Thanks.

6v6gt..
At this point in time I have no idea on what to look for..

gcjr..
Thanks . Yes I spotted that after I loaded the code onto the forum.
That I found because it did not print the Start up message.

Thanks for all the reply's.. Its given me something to do.

Regards Antony.

tomparkin · March 1, 2021, 3:25pm

Things I would consider changing:

Definitions for your pins are variables which are only ever referred to in setup(). I'd personally make them preprocessor macros (e.g. #define UP_PIN 6). I think this probably saves some RAM, but from the perspective of a reader of the sketch helps them understand that these are not variables and hence there's no need to check for when/where they are written to.
Make send_byte local to loop(). It gets reset every time around loop(), so there's no intrinsic value to it having a global scope. I appreciate that tilt_measure() accesses the variable, but this could be resolved by having tilt_measure() return a value, or passing a pointer to send_byte to tilt_measure(). By making send_byte local to loop() it's easier to review how and where the variable is used.
Look into debouncing the various buttons, e.g. https://www.arduino.cc/en/pmwiki.php?n=Tutorial/Debounce
Separate concerns. tilt_measure() currently reads the accelerometer, does some serial logging, modifies send_byte, delays a bit, and reads the rst pin. In other words, it does a lot more than its name suggests. Personally I would try to implement tilt_measure() such that it just reads the accelerometer sensor data and performs the maths to resolve pitch and roll values (I would pass these results out from the function). By doing so, the sketch is easier to follow since tilt_measure() is just doing what it says on the tin, and the reader doesn't need to worry about all the "other" things the function does.
loop()'s handling of the Accelerometer variable looks a bit odd to me. If Accelerometer is 1, then tilt_measure() is called. However, the code then goes on to read the states of the direction buttons. My reading of your description is that the control mechanism should be either accelerometer or buttons, but not both at the same time.

Btw -- I have mentioned "the reader" a couple of times. Sometimes that reader is someone else. More often that reader is yourself six months down the line when you can't remember how the code was supposed to work in the first place

dougp · March 1, 2021, 6:19pm

buttonState seems redundant. Consider:

  buttonState = digitalRead(let);                         //    white wire to D9
  if (buttonState == LOW) {
    send_byte |= B00000001; Serial.print(" left");        // Left Turn
  }

  // vs. 
                          
  if (digitalRead(let) == LOW) {                         //    white wire to D9
    send_byte |= B00000001; Serial.print(" left");        // Left Turn
  }

YMMV

AntonyG · March 2, 2021, 4:13pm

Hi.

tomparkin..and gcjr..
Thank you for your input.
I have made thebyte send_byte = 0; a local.

Sooo much to learn.

Thanks Again.
Antony.

system · June 30, 2021, 4:14pm

This topic was automatically closed 120 days after the last reply. New replies are no longer allowed.

Topic		Replies	Views
Help with making a sketch Programming	10	1425	May 5, 2021
Sketch optimization? Programming	7	1052	May 5, 2021
Questions about the sketch of my self balancing robot Programming	3	782	May 5, 2021
Programming techniques? Programming	26	9406	May 5, 2021
can someone help me mix two sketch Programming	2	520	May 5, 2021

Is there a better way to write a sketch.

Related topics