C3PO - Interactive humanoid robot

#include <AccelStepper.h>

#include <LiquidCrystal_I2C.h>
#include <Adafruit_NeoPixel.h>
#ifdef __AVR__
#include <avr/power.h>  // Required for 16 MHz Adafruit Trinket
#endif

#define LF          0x0A
LiquidCrystal_I2C lcd(0x27,20,4);

// Which pin on the Arduino is connected to the NeoPixels?
#define PIN 7  // On Trinket or Gemma, suggest changing this to 1

// How many NeoPixels are attached to the Arduino?
#define NUMPIXELS 6  // Popular NeoPixel ring size
Adafruit_NeoPixel pixels(NUMPIXELS, PIN, NEO_GRB + NEO_KHZ800);

#define DELAYVAL 111

char val[5];
int index = 0;
int directionMultiplier = 1;
bool newData = false;
bool runallowed = false;
String readString;
String motor_com;
String pos_com;
String speed_com;
String acceleration_com;
String side_comm;
int RightLeft = 1;
#define enArmR 6
#define enArmL 5
#define enHips_comm 4

AccelStepper stepperArmL(1, 38, 40); //PULL 8, DIR 9 46,48 - gacie, 44, 42 ArmL, 
AccelStepper stepperArmR(1, 42, 44); //PULL 8, DIR 9 46,48 - gacie, 44, 42 ArmL, 
AccelStepper stepperhips_comm(1, 46, 48);
//HardwareSerial Serial1 Serial1;

void setup() 
{
  Serial2.begin(115200);
  Serial2.println("C3PO running...");

  Serial.begin(115200);
  Serial.println("C3PO systems running...");

  pinMode(enArmR, OUTPUT); //enable motor pin
  pinMode(enArmL, OUTPUT); //enable motor pin
  pinMode(enHips_comm, OUTPUT); //enable motor pin
  
  stepperArmR.setMaxSpeed(400); //SPEED = Steps / second
  stepperArmR.setAcceleration(100); //ACCELERATION = Steps /(second)^2
  stepperArmL.setMaxSpeed(400); //SPEED = Steps / second
  stepperArmL.setAcceleration(100); //ACCELERATION = Steps /(second)^2
  stepperhips_comm.setMaxSpeed(400); //SPEED = Steps / second
  stepperhips_comm.setAcceleration(100); //ACCELERATION = Steps /(second)^2
    
  lcd.init();                      // initialize the lcd 
  lcd.init();
  lcd.backlight();
  lcd.setCursor(0,0);
  lcd.print("Hello from C3PO");
  runallowed = false;
  RunStopMotors("STOP");

    pixels.begin();  // INITIALIZE NeoPixel
    fBlink(10, 1);
    fClear();
    fColor(2);
    
}

void fClear()
{
    for (int i = 0; i < NUMPIXELS; i++) 
    {  
      pixels.setPixelColor(i, pixels.Color(0, 0, 0));
      pixels.show();  // Send the updated pixel colors to the hardware.
    }
}
void fBlink(int ile, int kolor)
{
  uint32_t colorUI[] = {pixels.Color(0, 0, 0), pixels.Color(255, 0, 0), pixels.Color(255, 255, 0)};
  for (int i = 0; i < ile; i++)
  {
    for (int i = 0; i < NUMPIXELS; i++) 
    {  
      pixels.setPixelColor(i, colorUI[0]);
      pixels.show(); 
    }
    pixels.show();
    delay(DELAYVAL);
    for (int i = 0; i < NUMPIXELS; i++) 
    {  
      pixels.setPixelColor(i, colorUI[kolor]);
    }
    pixels.show();  
    delay(DELAYVAL);
  }
}

void fColor(int kolor)
{
  uint32_t colorUI[] = {pixels.Color(0, 0, 0), pixels.Color(255, 0, 0), pixels.Color(255, 255, 0)};

    for (int i = 0; i < NUMPIXELS; i++) 
    {  
      pixels.setPixelColor(i, colorUI[kolor]);
      pixels.show();
    }
    pixels.show();
}

void loop() 
{
  checkSerial();
  RunTheMotor();
}

String getValue(String data, char separator, int index)
{
    int found = 0;
    int strIndex[] = { 0, -1 };
    int maxIndex = data.length() - 1;

    for (int i = 0; i <= maxIndex && found <= index; i++) {
        if (data.charAt(i) == separator || i == maxIndex) {
            found++;
            strIndex[0] = strIndex[1] + 1;
            strIndex[1] = (i == maxIndex) ? i+1 : i;
        }
    }
    return found > index ? data.substring(strIndex[0], strIndex[1]) : "";
}

void RunStopMotors(String STAN)
{
  if (STAN == "RUN")
  {
    digitalWrite(enArmR, HIGH);
    digitalWrite(enArmL, HIGH);
    digitalWrite(enHips_comm, HIGH);
  }
  else
  {
    runallowed = false;
    digitalWrite(enArmR, LOW);
    digitalWrite(enArmL, LOW);
    digitalWrite(enHips_comm, LOW);
  }
}


void checkSerial()
{
  if (Serial.available())
  {
     readString = Serial.readStringUntil('\n');

      if (readString.length() >0) 
      {
         Serial.println("Command: " + readString);
         motor_com = getValue(readString, ':', 0);
         pos_com = getValue(readString, ':', 1);
         speed_com = getValue(readString, ':', 2);
         acceleration_com = getValue(readString, ':', 3);
         
         lcd.setCursor(0,0);
         lcd.clear();
         lcd.print(motor_com);
         lcd.print(" ");
         lcd.print(pos_com);
         lcd.print(" ");
         lcd.print(speed_com);
         lcd.print(" ");
         lcd.print(acceleration_com);
         readString = "";

        if(motor_com == "ArmR")
        {
          directionMultiplier = 1;
          Serial.println("Right arm move to: " + String(directionMultiplier * pos_com.toInt()) );
          RunStopMotors("RUN");
          runallowed = true; //allow running - this allows entering the RunTheMotor() function.
          stepperArmR.setMaxSpeed(speed_com.toInt()); //set speed
          stepperArmR.setAcceleration(acceleration_com.toInt());
          stepperArmR.moveTo(directionMultiplier * pos_com.toInt()); //set relative distance  
          Serial.println("CURRENT POSR: " + String(stepperArmR.currentPosition()) );
        }
        else if(motor_com == "ArmL")
        {
          fClear();
          fColor(2);
          directionMultiplier = -1;
          Serial.println("Left arm move to: " + String(directionMultiplier * pos_com.toInt()) );
          RunStopMotors("RUN");
          runallowed = true;
          stepperArmL.setMaxSpeed(speed_com.toInt()); //set speed
          stepperArmL.setAcceleration(acceleration_com.toInt());
          stepperArmL.moveTo(directionMultiplier * pos_com.toInt()); //set relative distance  
          Serial.println("CURRENT POSL: " + String(stepperArmL.currentPosition()) );
        }
        else if(motor_com == "hips_comm") //hips_comm:-25000:50000:5000
        {
          directionMultiplier = -1;
          Serial.println("Hips move to: " + String(directionMultiplier * speed_com.toInt()) );
          RunStopMotors("RUN");
          runallowed = true; //allow running - this allows entering the RunTheMotor() function.
          stepperhips_comm.setMaxSpeed(speed_com.toInt()); //set speed
          stepperhips_comm.setAcceleration(acceleration_com.toInt());
          stepperhips_comm.moveTo(directionMultiplier * pos_com.toInt()); //set relative distance  
        }
        else if (motor_com == "A")
        {
          Serial.println("Left arm");
          RotateAbsolute();
        }
        else if (motor_com == "HOME")
        {
          Serial.println("Home");
          GoHome();
        }
        else if (motor_com == "O")
        {
          Serial.println(runallowed);
        }
        else if (motor_com == "r")
        {
          runallowed = true;
          Serial.println(runallowed);
        }
        else if (motor_com == "Z")
        {
          stepperArmR.setMaxSpeed(500);
          stepperArmR.setSpeed(500);
          stepperArmR.move(500);
        }
        else if (motor_com == "STOP")
        {
          Serial.println("DISABLED MOTORS");
          runallowed = false;
          RunStopMotors("STOP");
        }
        else if (motor_com == "LED")
        {
              fBlink(10, 2);
              fClear();
              fBlink(10, 1);
              fClear();
              fColor(2);
        }
        else if (motor_com == "LEDOFF")
        {
              fClear();
        }
      }
}
}

void RunTheMotor() //function for the motor
{
    if (runallowed == true)
    {
        RunStopMotors("RUN");
        stepperArmR.run();
        stepperArmL.run();
        stepperhips_comm.run();
    }
    else 
    {
        runallowed = false;
        RunStopMotors("STOP");
        return;
    }
}


void GoHome()
{  
    if (stepperArmR.currentPosition() == 0)
    {
        RunStopMotors("STOP");
    }
    else
    {
        runallowed = true; 
        RunStopMotors("RUN");
        stepperArmR.setMaxSpeed(200);
        stepperArmR.moveTo(0);
    }

    if (stepperArmR.currentPosition() == 0)
    {
        RunStopMotors("STOP");
    }
    else
    {
        runallowed = true; 
        RunStopMotors("RUN");
        stepperArmL.setMaxSpeed(200);
        stepperArmL.moveTo(0);
    }

} //end loop

 
void RotateRelative()
{
    //We move X steps from the current position of the stepperArmR motor in a given direction.
    //The direction is determined by the multiplier (+1 or -1)
    RunStopMotors("RUN");
    runallowed = true;
    stepperArmR.setMaxSpeed(acceleration_com.toInt()); //set speed
    stepperArmR.move(directionMultiplier * speed_com.toInt()); //set relative distance and direction
    stepperArmL.setMaxSpeed(acceleration_com.toInt()); //set speed
    stepperArmL.move(directionMultiplier * speed_com.toInt()); //set relative distance and direction
}
 
 
 
void RotateAbsolute()
{
    //We move to an absolute position.
    //The AccelstepperArmR library keeps track of the position.
    //The direction is determined by the multiplier (+1 or -1)
    acceleration_com = "400";
    speed_com = "1000";
    
    RunStopMotors("RUN");
    runallowed = true; //allow running - this allows entering the RunTheMotor() function.
    stepperArmR.setMaxSpeed(acceleration_com.toInt()); //set speed
    stepperArmR.moveTo(speed_com.toInt()); //set relative distance  
    stepperArmL.setMaxSpeed(acceleration_com.toInt()); //set speed
    stepperArmL.moveTo(speed_com.toInt()); //set relative distance
    //stepperArmR.disableOutputs(); 
    Serial.println("POSR: " + String(stepperArmR.currentPosition()) );
    Serial.println("CURRENT POSL: " + String(stepperArmL.currentPosition()) );
}