MoveThisWay Heavy Payload Assistant

#include <Encoder.h>
#include <math.h>
#include <HX711.h>
#include <Adafruit_NeoPixel.h>
#include <EEPROM.h>
#include <PID_v1.h>
#include <SD.h>
#include <SPI.h>
#include <RH_RF69.h>

#define RFM69_RST     9
#define RFM69_CS      10
#define RFM69_INT     digitalPinToInterrupt(8)
#define RF69_FREQ     915.0

// Singleton instance of the radio driver
RH_RF69 rf69(RFM69_CS, RFM69_INT);

typedef enum {
  NORMAL, PROGRAM, PATH1, PATH2, PATH3, PATH4
} OpMode;

OpMode mode = NORMAL;
OpMode prevMode = NORMAL;
uint8_t databuff[9];
uint8_t datalen = sizeof(databuff);
unsigned int ud_remote, lr_remote;
bool prog, pb1, pb2, pb3, pb4;
bool progp1 = false;
bool progp2 = false;
bool progp3 = false;
bool progp4 = false;
bool ow_file = true;
bool follow = false;
bool reverse = false;
bool prevFollow = false;
int path_num = 0;

Sd2Card card;
SdVolume volume;
SdFile root;

const int chipSelect =  BUILTIN_SDCARD;

int arraysize = 5000;
double leftData[5000];
double rightData[5000];
int len = 0;

int indexData = 0;
int indexWrite = 0;

File myFile;
File revFile;

double Setpoint_R, Input_R, Output_R;
double Setpoint_L, Input_L, Output_L;

//Specify the links and initial tuning parameters
PID leftPID(&Input_L, &Output_L, &Setpoint_L,  1.55,   10,  0.05, DIRECT);
PID rightPID(&Input_R, &Output_R, &Setpoint_R,  1.55,   10,  0.05, DIRECT);

int address = 0;
byte value;

HX711 scale;
#define calibration_factor -8000  //This value is obtained using the SparkFun_HX711_Calibration sketch
#define DOUT  2
#define CLK  3

#define savebutton 12

#define LED_PIN   1
#define LED_COUNT 24
Adafruit_NeoPixel strip(LED_COUNT, LED_PIN, NEO_GRB + NEO_KHZ800);

#define ud_pin A9
#define lr_pin A8

#define obstacle 0

#define forward_m1 4      // motor 1 forward
#define reverse_m1 5      // motor 1 reverse
#define forward_m2 6      // motor 2 forward
#define reverse_m2 7      // motor 2 reverse

#define e1_a 18            // encoder output pins
#define e1_b 19
#define e2_a 20
#define e2_b 21
Encoder knobLeft(e1_a, e1_b);
Encoder knobRight(e2_a, e2_b);

double positionLeft  = -999;
double positionRight = -999;
double left_rpm = 0;
double left_rpm_old = 0;
double right_rpm = 0;
double storeLeft = 0;
double storeRight = 0;

float time_new = 0;
float time_new_saveSD = 0;
float time_new_readSD = 0;
float time_delta = 25;

int ud = 0;
int lr = 0;
double ud_cartesian = 0;
double lr_cartesian = 0;
double r = 0;
double theta = 0;
double left_coord = 0;
double right_coord = 0;

int load = 0;

int indicator_brightness = 0;
int noload_brightness = 0;
int standby_brightness = 0;
int moving_brightness = 0;
int obstacle_brightness = 0;
int leftright_input = 0;        // INDICATOR COMMAND; off = 0, left = 1, right = 2
int ledaddress = 0;

void setup() {
  Serial.begin(2000000);
  pinMode(13, OUTPUT);

  pinMode(forward_m1, OUTPUT);        // set motor PWM pins as digital outputs
  pinMode(reverse_m1, OUTPUT);
  pinMode(forward_m2, OUTPUT);
  pinMode(reverse_m2, OUTPUT);

  pinMode(3, INPUT);

  analogWrite(forward_m1, 0);         // initialize PWM outputs as zero
  analogWrite(reverse_m1, 0);
  analogWrite(forward_m2, 0);
  analogWrite(reverse_m2, 0);

  pinMode(e1_a, INPUT);
  pinMode(e1_b, INPUT);
  pinMode(e2_a, INPUT);
  pinMode(e2_b, INPUT);
  pinMode(RFM69_RST, OUTPUT);

  digitalWrite(RFM69_RST, LOW);

  Serial.println("Welcome to MoveThisWay Central Processing!");
  Serial.println();

  // Manual RFM69 Reset
  digitalWrite(RFM69_RST, HIGH);
  delay(10);
  digitalWrite(RFM69_RST, LOW);
  delay(10);

  if (!rf69.init())
    Serial.println("init failed");
  // Defaults after init are 434.0MHz, modulation GFSK_Rb250Fd250, +13dbM, No encryption
  if (!rf69.setFrequency(RF69_FREQ))
    Serial.println("setFrequency failed");

  // If you are using a high power RF69, you *must* set a Tx power in the range 14 to 20 like this:
  rf69.setTxPower(14);

  Serial.print("RFM69 radio @");  Serial.print((int)RF69_FREQ);  Serial.println(" MHz");

  // SD CARD INITIALIZATION
  if (!SD.begin(chipSelect)) {
    Serial.println("initialization failed!");
    while (1);
  }
  Serial.println("initialization done.");

  scale.begin(DOUT, CLK);
  scale.set_scale(calibration_factor); //This value is obtained by using the SparkFun_HX711_Calibration sketch
  scale.tare(); //Assuming there is no weight on the scale at start up, reset the scale to 0

  strip.begin();
  strip.show(); // Initialize all pixels to 'off'

  //initialize the variables we're linked to
  Input_L = left_rpm;
  Setpoint_L = 0;
  Input_R = right_rpm;
  Setpoint_R = 0;

  //turn the PID on
  leftPID.SetSampleTime(20);
  leftPID.SetMode(AUTOMATIC);
  leftPID.SetOutputLimits(-255, 255);

  rightPID.SetSampleTime(20);
  rightPID.SetMode(AUTOMATIC);
  rightPID.SetOutputLimits(-255, 255);
}

void loop() {

  if (millis() < 1000) {        // Prevent motors from moving
    analogWrite(forward_m1, 0); // while establishing
    analogWrite(reverse_m1, 0); // connection to the motors.
    analogWrite(forward_m2, 0); //
    analogWrite(reverse_m2, 0); //
  }

  load = scale.get_units();

  bool obstacle_exist = digitalRead(obstacle);

  if (obstacle_exist == HIGH) {
    analogWrite(forward_m1, 0);
    analogWrite(reverse_m1, 0);
    analogWrite(forward_m2, 0);
    analogWrite(reverse_m2, 0);
    leftright_input = 0;
    ledaddress = ledcount(leftright_input, ledaddress);
    obstacle_brightness = stopped(ledaddress);
  }

  else {
    /////////////////////////////////////
    ///////// TANK CONTROL CODE /////////
    /////////////////////////////////////

    ud = analogRead(ud_pin);
    lr = analogRead(lr_pin);

    // >>>> MAP EITHER UD/LR OR UD/LR REMOTE HERE <<<<
    //ud_cartesian = map(ud, 7, 1011, -500, 500) * 1; //remap the pot range
    //lr_cartesian = map(lr, 7, 1011, -500, 500) * 1; // ^^^^^^^
    ud_cartesian = map(ud_remote, 7, 1011, -500, 500) * -1; //remap the pot range
    lr_cartesian = map(lr_remote, 7, 1011, -500, 500) * 1; // ^^^^^^^

    r = sqrt(pow(ud_cartesian , 2) + pow(lr_cartesian, 2)); // polar coord conversion for R

    if (ud_cartesian >= 0) {                                 //polar coord conversion for theta
      theta = atan2(ud_cartesian, lr_cartesian);
    }
    else if (ud_cartesian < 0) {
      theta = atan2(ud_cartesian, lr_cartesian) + 2 * M_PI ;
    }

    theta = theta - M_PI / 4;             //shift the polar coordinates by 45deg (pi/4) to make a diamond
    left_coord =  r * cos(theta);         //convert back to cartesian
    right_coord = r * sin(theta);
    left_coord =  left_coord * sqrt(2) * 255 / 500 ;
    right_coord = right_coord * sqrt(2) * 255  / 500 ;
    left_coord = constrain(left_coord, -255, 255);         //limit the output range
    right_coord = constrain(right_coord, -255, 255);

    if (left_coord <= 20 && left_coord >= -20) {
      left_coord = 0;
    }
    if (right_coord <= 20 && right_coord >= -20) {
      right_coord = 0;
    }

    //////////////////////////////////
    ///////// READ PATH CODE /////////
    //////////////////////////////////

    if (follow) {
      if (millis() > time_new_saveSD + 100) {
        if (myFile) {
          if (myFile.available()) {
            char buffer[50]; // May need to be a bit bigger if you have long names
            byte index = 0;

            while (myFile.available()) {
              char c = myFile.read();
              if (c == '\n' || c == '\r') { // Test for newline
                parseAndSave(buffer);
                index = 0;
                buffer[index] = '\0'; // NULL terminate buffer
                break;
              }
              else {
                buffer[index++] = c;
                buffer[index] = '\0'; // NULL terminate buffer
              }
            }
          } else {
            myFile.close();
            reverseAndWrite();
            revFile.close();
            follow = false;
            prevFollow = true;
            indexData = 0;
          }
        } else {
          Serial.println("Error opening file!");
        }

        // Write to reverse file
        if (revFile) {
          // Reverse the direction of wheels
          double rSetpoint_L = -1 * Setpoint_L;
          double rSetpoint_R = -1 * Setpoint_R;

          leftData[len] = rSetpoint_L;
          rightData[len++] = rSetpoint_R;

        } else {
          Serial.println("Error writing to reverse file!");
        }

        Serial.print("L: ");
        Serial.print(Setpoint_L);
        Serial.print("  ");
        Serial.print(left_rpm);
        Serial.print("    R: ");
        Serial.print(Setpoint_R);
        Serial.print("  ");
        Serial.println(right_rpm);

        if (Setpoint_L <= -20 ) {
          analogWrite(forward_m1, 0);
          analogWrite(reverse_m1, abs(Setpoint_L));
        }
        else if (Setpoint_L >= 20) {
          analogWrite(reverse_m1, 0);
          analogWrite(forward_m1, abs(Setpoint_L));
        }
        else {
          analogWrite(reverse_m1, 0);
          analogWrite(forward_m1, 0);
        }
        if (Setpoint_R <= -20) {
          analogWrite(forward_m2, 0);
          analogWrite(reverse_m2, abs(Setpoint_R));
        }
        else if (Setpoint_R >= 20) {
          analogWrite(reverse_m2, 0);
          analogWrite(forward_m2, abs(Setpoint_R));
        }
        else {
          analogWrite(reverse_m2, 0);
          analogWrite(forward_m2, 0);
        }

        time_new_saveSD = millis();
      }
    }

    if (reverse) {
      if (prevFollow) {
        if (path_num == 1) {
          revFile = SD.open("rev_1.txt");
          Serial.println("Opened rev_1.txt");
        } else if (path_num == 2) {
          revFile = SD.open("rev_2.txt");
          Serial.println("Opened rev_2.txt");
        } else if (path_num == 3) {
          revFile = SD.open("rev_3.txt");
          Serial.println("Opened rev_3.txt");
        } else if (path_num == 4) {
          revFile = SD.open("rev_4.txt");
          Serial.println("Opened rev_4.txt");
        }
        prevFollow = false;
      }
      if (millis() > time_new_saveSD + 100) {
        if (revFile) {
          if (revFile.available()) {
            char buffer[50]; // May need to be a bit bigger if you have long names
            byte index = 0;

            while (revFile.available()) {
              char c = revFile.read();
              if (c == '\n' || c == '\r') { // Test for newline
                parseAndSave(buffer);
                index = 0;
                buffer[index] = '\0'; // NULL terminate buffer
                break;
              }
              else {
                buffer[index++] = c;
                buffer[index] = '\0'; // NULL terminate buffer
              }
            }
          } else {
            revFile.close();
            Serial.println("Closed Reverse File");
            SD.remove("rev_1.txt");
            SD.remove("rev_2.txt");
            SD.remove("rev_3.txt");
            SD.remove("rev_4.txt");
            //Serial.println("Removed Reverse Files");
            if (SD.exists("rev_1.txt")) { //If the file still exist display message exist
              Serial.println("The rev_1.txt exists.");
            } else { //If the file was successfully deleted display message that the file was already deleted.
              Serial.println("The rev_1.txt doesn't exist.");
            }
            reverse = false;
            indexData = 0;
          }
        } else {
          Serial.println("Error opening reverse file for reading!");
        }

        Serial.print("Rev L: ");
        Serial.print(Setpoint_L);
        Serial.print("  ");
        Serial.print(left_rpm);
        Serial.print("    Rev R: ");
        Serial.print(Setpoint_R);
        Serial.print("  ");
        Serial.println(right_rpm);

        if (Setpoint_L <= -20 ) {
          analogWrite(forward_m1, 0);
          analogWrite(reverse_m1, abs(Setpoint_L));
        }
        else if (Setpoint_L >= 20) {
          analogWrite(reverse_m1, 0);
          analogWrite(forward_m1, abs(Setpoint_L));
        }
        else {
          analogWrite(reverse_m1, 0);
          analogWrite(forward_m1, 0);
        }
        if (Setpoint_R <= -20) {
          analogWrite(forward_m2, 0);
          analogWrite(reverse_m2, abs(Setpoint_R));
        }
        else if (Setpoint_R >= 20) {
          analogWrite(reverse_m2, 0);
          analogWrite(forward_m2, abs(Setpoint_R));
        }
        else {
          analogWrite(reverse_m2, 0);
          analogWrite(forward_m2, 0);
        }

        time_new_saveSD = millis();
      }
    }

    ////////////////////////////////
    ///////// ENCODER CODE /////////
    ////////////////////////////////

    long newLeft, newRight;
    newLeft = knobLeft.read();
    newRight = knobRight.read();

    if (newLeft != positionLeft || newRight != positionRight) {
      positionLeft = newLeft;
      positionRight = newRight;
    }

    if (millis() > time_new + time_delta) {
      float left_delta = positionLeft - storeLeft;
      left_rpm = ((left_delta / 4800) / (time_delta / 1000)) * 60;
      float right_delta = positionRight - storeRight;
      right_rpm = ((right_delta / 4800) / (time_delta / 1000)) * 60 ;
      storeLeft = positionLeft;
      storeRight = positionRight;
      time_new = millis();

      float wheelspeed_delta = right_rpm - left_rpm;

      if (left_rpm == 0 && right_rpm == 0) {
        leftright_input = 0;
        ledaddress = ledcount(leftright_input, ledaddress);
      }
      else if (left_rpm >= right_rpm - 10 && left_rpm <= right_rpm + 10) {
        leftright_input = 0;
        ledaddress = ledcount(leftright_input, ledaddress);
      }
      else if (wheelspeed_delta <= -10) {
        leftright_input = 1;
        ledaddress = ledcount(leftright_input, ledaddress);
      }
      else if (wheelspeed_delta >= 10) {
        leftright_input = 2;
        ledaddress = ledcount(leftright_input, ledaddress);
      }

      if (left_rpm == 0 && right_rpm == 0) {
        if (digitalRead(obstacle) == LOW) {
          if (load <= 2) {
            noload_brightness = noload(ledaddress);
            if (prevFollow) { // Trigger return upon object removal
              reverse = true;
            }
          }
          else if (load > 2) {
            standby_brightness = standby(ledaddress);
          }
        }
        else if (digitalRead(obstacle) == HIGH) {
          obstacle_brightness = stopped(ledaddress);
        }
      }
      else if (left_rpm >= right_rpm - 10 && left_rpm <= right_rpm + 10) {
        leftright_input = 0;
        moving_brightness = moving(ledaddress);
      }
      else if (wheelspeed_delta <= -10) {
        leftright_input = 2;
        indicator_brightness = indicator(ledaddress);
      }
      else if (wheelspeed_delta >= 10) {
        leftright_input = 1;
        indicator_brightness = indicator(ledaddress);
      }
    }

    ////////////////////////////////
    ///////// RECEIVE CODE /////////
    ////////////////////////////////

    if (rf69.available()) {
      if (rf69.recv(databuff, &datalen)) {
        // Parse data buffer
        prog = (databuff[0] == 255) ? true : false;
        pb1 = (databuff[1] == 255) ? true : false;
        pb2 = (databuff[2] == 255) ? true : false;
        pb3 = (databuff[3] == 255) ? true : false;
        pb4 = (databuff[4] == 255) ? true : false;

        ud_remote = databuff[5] * 256 + databuff[6];
        lr_remote = databuff[7] * 256 + databuff[8];
      }

      // Set mode
      if (prog) {
        mode = PROGRAM;
      } else if (pb1) {
        mode = PATH1;
      } else if (pb2) {
        mode = PATH2;
      } else if (pb3) {
        mode = PATH3;
      } else if (pb4) {
        mode = PATH4;
      } else {
        mode = NORMAL;
      }
    }

    if (mode == NORMAL) {
      if (prevMode == PROGRAM) {
        progp1 = false;
        progp2 = false;
        progp3 = false;
        progp4 = false;
        ow_file = true;
      }

      // Drive Normally
      if (!follow && !reverse) {
        if (left_coord <= -20) {
          analogWrite(forward_m1, 0);
          analogWrite(reverse_m1, abs(left_coord));
        }
        else if (left_coord >= 20) {
          analogWrite(reverse_m1, 0);
          analogWrite(forward_m1, abs(left_coord));
        }
        else {
          analogWrite(reverse_m1, 0);
          analogWrite(forward_m1, 0);
        }
        if (right_coord <= -20) {
          analogWrite(forward_m2, 0);
          analogWrite(reverse_m2, abs(right_coord));
        }
        else if (right_coord >= 20) {
          analogWrite(reverse_m2, 0);
          analogWrite(forward_m2, abs(right_coord));
        }
        else {
          analogWrite(reverse_m2, 0);
          analogWrite(forward_m2, 0);
        }
      }
      prevMode = NORMAL;
    } else if (mode == PROGRAM) {
      // Wait for a path button to be selected
      if (pb1 && !progp2 && !progp3 && !progp4) {
        progp1 = true;
      } else if (pb2 && !progp1 && !progp3 && !progp4) {
        progp2 = true;
      } else if (pb3 && !progp1 && !progp2 && !progp4) {
        progp3 = true;
      } else if (pb4 && !progp1 && !progp2 && !progp3) {
        progp4 = true;
      }

      Serial.print("Program Mode:  ");
      if (progp1) {
        Serial.println("1");
      } else if (progp2) {
        Serial.println("2");
      } else if (progp3) {
        Serial.println("3");
      } else if (progp4) {
        Serial.println("4");
      } else {
        Serial.println("0");
      }

      // drive normally
      if (left_coord <= -20 ) {
        analogWrite(forward_m1, 0);
        analogWrite(reverse_m1, abs(left_coord));
      }
      else if (left_coord >= 20) {
        analogWrite(reverse_m1, 0);
        analogWrite(forward_m1, abs(left_coord));
      }
      else {
        analogWrite(reverse_m1, 0);
        analogWrite(forward_m1, 0);
      }
      if (right_coord <= -20) {
        analogWrite(forward_m2, 0);
        analogWrite(reverse_m2, abs(right_coord));
      }
      else if (right_coord >= 20) {
        analogWrite(reverse_m2, 0);
        analogWrite(forward_m2, abs(right_coord));
      }
      else {
        analogWrite(reverse_m2, 0);
        analogWrite(forward_m2, 0);
      }

      //WRITE TO CARD HERE
      if (millis() > time_new_saveSD + 100) {
        if (progp1) {
          if (ow_file) {
            SD.remove("path_1.txt");
            SD.remove("rev_1.txt");
            ow_file = false;
          }
          // Save encoder values to P1
          myFile = SD.open("path_1.txt", FILE_WRITE);

          // if the file opened okay, write to it:
          if (myFile) {
            myFile.print(left_coord);
            myFile.print(" ");
            myFile.print(right_coord);
            myFile.print('\n');

            // close the file:
            myFile.close();
          }
          else {
            Serial.println("error opening path_1.txt");
          }

        } else if (progp2) {
          // Save encoder values to P2
          if (ow_file) {
            SD.remove("path_2.txt");
            SD.remove("rev_2.txt");
            ow_file = false;
          }
          // Save encoder values to P1
          myFile = SD.open("path_2.txt", FILE_WRITE);

          // if the file opened okay, write to it:
          if (myFile) {
            myFile.print(left_coord);
            myFile.print(" ");
            myFile.print(right_coord);
            myFile.print('\n');

            // close the file:
            myFile.close();
          }
          else {
            Serial.println("error opening path_2.txt");
          }
        } else if (progp3) {
          // Save encoder values to P3
          if (ow_file) {
            SD.remove("path_3.txt");
            SD.remove("rev_3.txt");
            ow_file = false;
          }
          // Save encoder values to P1
          myFile = SD.open("path_3.txt", FILE_WRITE);

          // if the file opened okay, write to it:
          if (myFile) {
            myFile.print(left_coord);
            myFile.print(" ");
            myFile.print(right_coord);
            myFile.print('\n');

            // close the file:
            myFile.close();
          }
          else {
            Serial.println("error opening path_3.txt");
          }
        } else if (progp4) {
          // Save encoder values to P4
          if (ow_file) {
            SD.remove("path_4.txt");
            SD.remove("rev_4.txt");
            ow_file = false;
          }
          // Save encoder values to P1
          myFile = SD.open("path_4.txt", FILE_WRITE);

          // if the file opened okay, write to it:
          if (myFile) {
            myFile.print(left_coord);
            myFile.print(" ");
            myFile.print(right_coord);
            myFile.print('\n');

            // close the file:
            myFile.close();
          }
          else {
            Serial.println("error opening path_4.txt");
          }
        }

        time_new_saveSD = millis();
      }

      prevMode = PROGRAM;
      //READ FROM CARD HERE
    } else if (mode == PATH1 && !follow && !reverse) {
      revFile.close();
      follow = true;
      path_num = 1;
      SD.remove("rev_1.txt");
      Serial.println("Reverse 1 file removed!");
      myFile = SD.open("path_1.txt");
      revFile = SD.open("rev_1.txt", FILE_WRITE);
      revFile.seek(0);

    } else if (mode == PATH2 && !follow && !reverse) {
      revFile.close();
      path_num = 2;
      follow = true;
      SD.remove("rev_2.txt");
      Serial.println("Reverse 2 file removed!");
      myFile = SD.open("path_2.txt");
      revFile = SD.open("rev_2.txt", FILE_WRITE);
      revFile.seek(0);

    } else if (mode == PATH3 && !follow && !reverse) {
      revFile.close();
      path_num = 3;
      follow = true;
      SD.remove("rev_3.txt");
      Serial.println("Reverse 3 file removed!");
      myFile = SD.open("path_3.txt");
      revFile = SD.open("rev_3.txt", FILE_WRITE);
      revFile.seek(0);

    } else if (mode == PATH4 && !follow && !reverse) {
      revFile.close();
      path_num = 4;
      follow = true;
      SD.remove("rev_4.txt");
      Serial.println("Reverse 4 file removed!");
      myFile = SD.open("path_4.txt");
      revFile = SD.open("rev_4.txt", FILE_WRITE);
      revFile.seek(0);
    }
  }
}

int ledcount(int leftright_input, int ledaddress) {
  if (leftright_input == 1) {
    if (ledaddress < 8) {
      ledaddress++;
    }
    else {
      ledaddress = 0;
    }
  }
  else if (leftright_input == 2) {
    if (ledaddress < 11) {
      ledaddress = 11;
    }
    else if (ledaddress >= 11 && ledaddress <= 20) {
      ledaddress++;
    }
    else {
      ledaddress = 12;
    }
  }
  else if (leftright_input == 0) {
    if (ledaddress <= 23) {
      ledaddress++;
    }
    else {
      ledaddress = 0;
    }
  }
  return (ledaddress);
}

int indicator(int ledaddress) {
  strip.setPixelColor(ledaddress + 0, 255, 255, 0);
  strip.setPixelColor(ledaddress + 1, 255, 255, 0);
  strip.setPixelColor(ledaddress + 2, 255, 255, 0);
  strip.setPixelColor(ledaddress + 3, 255, 255, 0);
  strip.show();
  strip.setPixelColor(ledaddress + 0, 0, 0, 0);
  strip.setPixelColor(ledaddress + 1, 0, 0, 0);
  strip.setPixelColor(ledaddress + 2, 0, 0, 0);
  strip.setPixelColor(ledaddress + 3, 0, 0, 0);
  return (0);
}

int noload(int ledaddress) {
  strip.setPixelColor(ledaddress + 0, 255, 51, 153);
  strip.setPixelColor(ledaddress + 1, 255, 51, 153);
  strip.setPixelColor(ledaddress + 2, 255, 51, 153);
  strip.setPixelColor(ledaddress + 3, 255, 51, 153);
  strip.show();
  strip.setPixelColor(ledaddress + 0, 0, 0, 0);
  strip.setPixelColor(ledaddress + 1, 0, 0, 0);
  strip.setPixelColor(ledaddress + 2, 0, 0, 0);
  strip.setPixelColor(ledaddress + 3, 0, 0, 0);
  return (0);
}

int standby(int ledaddress) {
  strip.setPixelColor(ledaddress + 0, 0, 255, 255);
  strip.setPixelColor(ledaddress + 1, 0, 255, 255);
  strip.setPixelColor(ledaddress + 2, 0, 255, 255);
  strip.setPixelColor(ledaddress + 3, 0, 255, 255);
  strip.show();
  strip.setPixelColor(ledaddress + 0, 0, 0, 0);
  strip.setPixelColor(ledaddress + 1, 0, 0, 0);
  strip.setPixelColor(ledaddress + 2, 0, 0, 0);
  strip.setPixelColor(ledaddress + 3, 0, 0, 0);
  return (0);
}


int moving(int ledaddress) {
  strip.setPixelColor(ledaddress + 0, 0, 255, 0);
  strip.setPixelColor(ledaddress + 1, 0, 255, 0);
  strip.setPixelColor(ledaddress + 2, 0, 255, 0);
  strip.setPixelColor(ledaddress + 3, 0, 255, 0);
  strip.show();
  strip.setPixelColor(ledaddress + 0, 0, 0, 0);
  strip.setPixelColor(ledaddress + 1, 0, 0, 0);
  strip.setPixelColor(ledaddress + 2, 0, 0, 0);
  strip.setPixelColor(ledaddress + 3, 0, 0, 0);
  return (0);
}

int stopped(int ledaddress) {
  strip.setPixelColor(ledaddress + 0, 255, 0, 0);
  strip.setPixelColor(ledaddress + 1, 255, 0, 0);
  strip.setPixelColor(ledaddress + 2, 255, 0, 0);
  strip.setPixelColor(ledaddress + 3, 255, 0, 0);
  strip.setPixelColor(ledaddress + 4, 255, 0, 0);
  strip.setPixelColor(ledaddress + 5, 255, 0, 0);
  strip.setPixelColor(ledaddress + 6, 255, 0, 0);
  strip.setPixelColor(ledaddress + 7, 255, 0, 0);
  strip.setPixelColor(ledaddress + 8, 255, 0, 0);
  strip.setPixelColor(ledaddress + 9, 255, 0, 0);
  strip.show();
  strip.setPixelColor(ledaddress + 0, 0, 0, 0);
  strip.setPixelColor(ledaddress + 1, 0, 0, 0);
  strip.setPixelColor(ledaddress + 2, 0, 0, 0);
  strip.setPixelColor(ledaddress + 3, 0, 0, 0);
  strip.setPixelColor(ledaddress + 4, 0, 0, 0);
  strip.setPixelColor(ledaddress + 5, 0, 0, 0);
  strip.setPixelColor(ledaddress + 6, 0, 0, 0);
  strip.setPixelColor(ledaddress + 7, 0, 0, 0);
  strip.setPixelColor(ledaddress + 8, 0, 0, 0);
  strip.setPixelColor(ledaddress + 9, 0, 0, 0);
  strip.setPixelColor(ledaddress + 10, 0, 0, 0);
}

void onLED() {
  digitalWrite(13, LOW);
  if (millis() > time_new + time_delta) {
    digitalWrite(13, HIGH);
  }
  digitalWrite(13, LOW);
}

int parseAndSave(char *buff) {
  Serial.print(indexData);
  Serial.print(" ");
  char *l_string = strtok(buff, " ");
  char *r_string = strtok(NULL, "\0");

  if (l_string && r_string) {
    Setpoint_L = atof(l_string);
    Setpoint_R = atof(r_string);
    indexData++;
  }
  return (indexData);
}

void reverseAndWrite() {
  for (int i = 0, j = len - 1; i < len / 2; i++, j--) {
    double temp1 = leftData[i];
    leftData[i] = leftData[j];
    leftData[j] = temp1;

    double temp2 = rightData[i];
    rightData[i] = rightData[j];
    rightData[j] = temp2;
  }

  for (int i = 0; i < len; i++) {
    revFile.print(leftData[i]);
    revFile.print(" ");
    revFile.print(rightData[i]);
    revFile.print('\n');
  }
 

  for (int i = 0; i < len; i++) {
    leftData[i] = 0;
    rightData[i] = 0;
  }
}