# Servo commands based on serial port input

Straight to the point: Can you give me pointers on how to make this code more maintainable? In the future, I want to add more more things to this, but first it should be easy to maintain/read. Should making a library be considered?

What it does:

Takes input from the serial line and converts it to servo commands + all the bells and whistles added.

#include <NewPing.h>
#include <Servo.h>

#define errorLED 13
#define ThrottlePin 2
#define RollPin 3
#define PitchPin 5
#define YawPin 4
#define Aux1Pin 6
#define Trigger 18
#define Echo 19

NewPing sonar(Trigger, Echo);
Servo Throttle;
Servo Roll;
Servo Pitch;
Servo Yaw;
Servo Aux1;
unsigned int throttle = DEFAULT_THROTTLE;
unsigned int roll = DEFAULT_ROLL;
unsigned int pitch = DEFAULT_PITCH;
unsigned int yaw = DEFAULT_YAW;
unsigned int aux1 = DEFAULT_AUX1;
unsigned int temp = 0;
unsigned int range = 7;

unsigned long lastSerialData = 0;
unsigned long lastThrottleUpdate = 0;
unsigned long ledPreviousMillis = 0;

const int DEFAULT_THROTTLE = 45;
const int DEFAULT_ROLL = 70;
const int DEFAULT_PITCH = 70;
const int DEFAULT_YAW = 70;
const int DEFAULT_AUX1 = 45;

boolean isLedOn = false;
//Pins 7-12 power
//A0-A3 pin 13-16
boolean index = 7;
boolean circleIndex = 14;
void setup()
{
Throttle.attach(ThrottlePin);
Roll.attach(RollPin);
Pitch.attach(PitchPin);
Yaw.attach(YawPin);
Aux1.attach(Aux1Pin);
Serial.begin(115200);
for (byte x = 7; x <= 12; x++)
pinMode(x, OUTPUT);
for (byte xy = 14; xy <= 17; xy++)
pinMode(xy, OUTPUT);

setPowerPinsOn(false);
setGroundPinsOn(true);

pinMode(errorLED, OUTPUT);

}

int GetFromSerial()
{
// wait until we have some serial data
while (Serial.available() == 0) {
if (millis() - lastSerialData > 1000) {
digitalWrite(errorLED, HIGH);
autoLand();
}
}

lastSerialData = millis();
digitalWrite(errorLED, LOW);

}

void loop()
{
switch (GetFromSerial()) {
case 't':
temp = 0;
temp = GetFromSerial() + 45;
if (temp >= 45 && temp <= 141)
throttle = temp;    //45 to 141
Throttle.write(throttle);
break;
case 'r':
temp = 0;
temp = GetFromSerial() + 45;
if (temp >= 45 && temp <= 141)
roll = map(temp, 45, 141, 69, 117); //45 to 141
if (roll < (93 + range) && roll > (93 - range))
roll = 93;

Roll.write(roll);
break;
case 'p':
temp = 0;
temp = GetFromSerial() + 45;
if (temp >= 45 && temp <= 141)
pitch = map(temp, 45, 141, 69, 117);    //45 to 141
if (pitch < (93 + range) && pitch > (93 - range))
pitch = 93;
Pitch.write(pitch);
break;
case 'y':
temp = 0;
temp = GetFromSerial() + 45;
if (temp >= 45 && temp <= 141)
yaw = map(temp, 45, 141, 68, 117);  //45 to 141
Yaw.write(yaw);
break;
case 'a':
temp = 0;
temp = GetFromSerial() + 45;
if (temp >= 45 && temp <= 141)
aux1 = temp;    //45 to 141
Aux1.write(aux1);
break;
}               // end switch
if (throttle <= 45)     //Connected but not flying
circleLed();
else if (throttle >= 45 && aux1 > 45)
}

void autoLand()
{
if (throttle <= 60 && aux1 >= 50) {
throttle = 45;
aux1 = 45;
} else if (throttle >= 45)
if (millis() - lastThrottleUpdate > 400) {
throttle = throttle * .95;
aux1 = 45;
lastThrottleUpdate = millis();
}
writeAllValues();
}

void writeAllValues()
{
Throttle.write(throttle);
Roll.write(roll);
Pitch.write(pitch);
Yaw.write(yaw);
Aux1.write(aux1);
}

void setPowerPinsOn(boolean on)
{
if (on) {
for (byte x = 7; x <= 12; x++)
digitalWrite(x, HIGH);
} else
for (byte x = 7; x <= 12; x++)
digitalWrite(x, LOW);
}

void setGroundPinsOn(boolean on)
{
if (on) {
for (byte x = 14; x <= 17; x++)
digitalWrite(x, LOW);
} else
for (byte x = 14; x <= 17; x++)
digitalWrite(x, HIGH);
}

void circleLed()
{
if (millis() - ledPreviousMillis > 400) {
if (circleIndex == 18)
circleIndex = 14;

ledPreviousMillis = millis();
setPowerPinsOn(true);
setGroundPinsOn(false);
digitalWrite(circleIndex, LOW);
circleIndex++;
}
}

{
if (millis() - ledPreviousMillis > 1000) {
ledPreviousMillis = millis();
isLedOn = !isLedOn;
setPowerPinsOn(isLedOn);
setGroundPinsOn(true);
}
}

{
if (millis() - ledPreviousMillis > 300 - (throttle - 45) * 2) {
ledPreviousMillis = millis();
if (index == 13)
index = 7;

setPowerPinsOn(false);
setGroundPinsOn(false);
digitalWrite(index, HIGH);
index++;
}
}

{
int uS = sonar.ping_median();
Serial.println(uS / US_ROUNDTRIP_CM);
}


Summary: How can this code be more maintainable? Should making a library be considered?

I'll start by breaking the code down into different files. There are a many #define and const int here. If I am correct that will definitely grow with your project. Having all the constants in a different file is always a good idea if you are considering making it even moderately big. So to your question

Should making a library be considered?

I would say you should definitely make a library.

After that I'll go and indent this code properly. I couldn't understand where the loop() function started and where it ended in the first go. If you want readability that is a something that you must seriously consider. All of your functions need proper indentation but for the loop() function it is a must.

There is a lot of redundancy in your loop() function.

• The first 2 lines of each of your case is same. That is really bad programming. It made your code so big that you had to write comments to note down the end of your braces.
• You are writing same comments over and over again with no actual use. If you needed to change these you'll be getting a headache.

I'll write your loop() function something like this

#define TEMP_RANGE(x,y) (temp >= x && temp <= y)
#define CHECK_RANGE(x, y, z) ((y + z) > x && x > (y - z))

void loop()
{
temp = 0;
temp = GetFromSerial() + 45;
switch (GetFromSerial())
{
case 't':
if (TEMP_RANGE(45,141))
throttle = temp;
Throttle.write(throttle);
break;
case 'r':
if (TEMP_RANGE(45,141))
roll = map(temp, 45, 141, 69, 117);
if (CHECK_RANGE(roll, 93, range))
roll = 93;
Roll.write(roll);
break;
case 'p':
if (TEMP_RANGE(45,141))
pitch = map(temp, 45, 141, 69, 117);
if (CHECK_RANGE(pitch, 93, range))
pitch = 93;
Pitch.write(pitch);
break;
case 'y':
if (TEMP_RANGE(45,141))
yaw = map(temp, 45, 141, 68, 117);
Yaw.write(yaw);
break;
case 'a':
if (TEMP_RANGE(45,141))
aux1 = temp;
Aux1.write(aux1);
break;
}

if (throttle <= 45) //Connected but not flying
circleLed();
else if (aux1 > 45)
}


Note these

• I placed the range checks which were being done more than once into their own define. This made them much easier to read and change. You can make better names based on the context. I didn't use functions but you can use them for even better abstraction and readability.
• Due to proper indentation people will be able to read this much faster. That is very important if you want maintainability.
• Took out redundancy from your code. You were checking whether a number was <= 45 something and in the else if you were checking whether it was >=. Obviously it is > 45 otherwise the fist condition would have executed.
• It can be improve more if you use functions or a bit more little complex macros. Functions might be preferable.

I think I covered the biggest issues.

• Use a proper indentation in your whole code. That is essential.
• Maybe add function prototypes at the top to make it easier to find what is where.
• If you are thinking of making it big try to break it down into logical parts first.
• Use braces consistently. Sometimes you are using braces for containing single statements but sometimes you are not. That is a source of many bugs.

Hope this helped. If you want more feedback I suggest you to fix these problems and ask another question with the updated code for more feedback.

• Just what I was looking for :) – user2587674 Jul 18 '13 at 12:11