// ROBOTIC ARMS CONTROL CODE VIA KEYBOARD
// By Mauricio Duarte e Flavio Guimaraes

#include <Arduino.h>   //libraries
#include <EEPROM.h>
#include <VarSpeedServo.h>

#define espacoMemoria 199
#define tempoPausaEntreMovimentos 500  //sets the pause time between each movement
#define pinServoBase  13 //Servo pinout
#define pinServoBraco 12
#define pinServoAnte  11
#define pinServoGarra 10
#define pinServoPulso  9


VarSpeedServo servobase;  
VarSpeedServo servobraco;
VarSpeedServo servoante;
VarSpeedServo servopulso;
VarSpeedServo servogarra;

void setMemoria(byte posicao, byte servo, byte valor);
byte readMemoria(byte posicao, byte servo);
int ultMemoria;


int n = 0;                  
int AguloServoBase = 90;    //servo angles Variables
int AguloServoBraco = 100;
int AguloServoAnte = 90;
int AguloServoGarra = 140;
int AguloServoPulso = 130;
int velocidade = 50;
bool botaoGrava = false;
int modo = 1;

void setup() {
  
  Serial.begin(9600);  //Enabling the serial port
  servobase.attach(pinServoBase);
  servobraco.attach(pinServoBraco);
  servoante.attach(pinServoAnte);
  servogarra.attach(pinServoGarra);
  servopulso.attach(pinServoPulso);
  Serial.flush();  //Clearing serial memory
  
  
  ultMemoria = EEPROM.read(0);
}

void loop() {
static byte modoAnt;
static byte movimento = 0;
static byte posMemoria = 0;
static int ultMemoria = -1;
static unsigned long delayPausa;

  
  EntradaTeclado(); //reading keyboard
  
  //Play Mode
  if (modo == 0) {
        Serial.print("Play Mode- Velocidade = ");
        Serial.println(velocidade); 
        //executa um movimento  
        if (movimento == 0) {
            
          servobase.slowmove(readMemoria(posMemoria,0),velocidade);
          servobraco.slowmove(readMemoria(posMemoria,1),velocidade);
          servoante.slowmove(readMemoria(posMemoria,2),velocidade);
          servogarra.slowmove(readMemoria(posMemoria,3),velocidade);
          servopulso.slowmove(readMemoria(posMemoria,4),velocidade);
          
          movimento = 1;
        } 

        //waits for a move to finish to select the next move
        if (movimento == 1) {
          if ( (servobase.read() == readMemoria(posMemoria,0)) &&
               (servobraco.read() == readMemoria(posMemoria,1)) &&
               (servoante.read() == readMemoria(posMemoria,2)) &&
               (servogarra.read() == readMemoria(posMemoria,3)) &&
               (servopulso.read() == readMemoria(posMemoria,4)) ) {
  
             posMemoria++;
             if (posMemoria > ultMemoria) { posMemoria = 0; }
             
             delayPausa = millis();
             movimento = 2;
          } 
        }

        //pausa entre movimentos
        if (movimento == 2) {
           if ((millis() - delayPausa) > tempoPausaEntreMovimentos) {
              movimento = 0;
           }
        }
                  
    
  }

  //Rec Mode
  
  if (modo == 1) {
     
     Serial.println("Rec Mode");
     if (modoAnt == 0) {
        ultMemoria = -1;  
        EEPROM.write(0, ultMemoria);
     }

     servobase.slowmove( AguloServoBase,velocidade );
     servobraco.slowmove( AguloServoBraco,velocidade );
     servoante.slowmove( AguloServoAnte,velocidade );
     servogarra.slowmove( AguloServoGarra,velocidade );
     servopulso.slowmove( 220 - AguloServoAnte,velocidade ); 
    

     if (botaoGrava) {
        Serial.println("Recorded Position!");
        ultMemoria++;
        EEPROM.write(0, ultMemoria);
        setMemoria(ultMemoria, 0, AguloServoBase);
        setMemoria(ultMemoria, 1, AguloServoBraco);
        setMemoria(ultMemoria, 2, AguloServoAnte);
        setMemoria(ultMemoria, 3, AguloServoGarra);
        setMemoria(ultMemoria, 4, 220 - AguloServoAnte);
        
        if ( ultMemoria == (espacoMemoria - 1)) {
            modo = 0; 
        }

        botaoGrava = false;
     
     }
  }

  modoAnt = modo;
}




void EntradaTeclado(){
  if(Serial.available()>0){  //Test if serial port is receiving data
    n = Serial.read();  //key reading

    if(n==81){  //girar 1
    AguloServoBase = AguloServoBase + 1;
    delay(500);
    }

    if(n==65){  //girar -1
    AguloServoBase = AguloServoBase - 1;
    delay(500);
    }

    if(n==113){  //girar 10
    AguloServoBase = AguloServoBase + 10;
    delay(500);
    }
    
    if(n==97){  //girar -10
    AguloServoBase = AguloServoBase - 10;
    delay(500);
    }
    
    
    if(n==87){  //girar 1
    AguloServoBraco = AguloServoBraco + 1;
    delay(500);
    }

    if(n==83){  //girar -1
    AguloServoBraco = AguloServoBraco - 1;
    delay(500);
    }

    if(n==119){  //girar 10
    AguloServoBraco = AguloServoBraco + 10;
    delay(500);
    }
    
    if(n==115){  //girar -10
    AguloServoBraco = AguloServoBraco - 10;
    delay(500);
    }
    
    
    if(n==69){  //girar 1
    AguloServoAnte = AguloServoAnte + 1;
    delay(500);
    }

    if(n==68){  //girar -1
    AguloServoAnte = AguloServoAnte - 1;
    delay(500);
    }

    if(n==101){  //girar 10
    AguloServoAnte = AguloServoAnte + 10;
    delay(500);
    }
    
    if(n==100){  //girar -10
    AguloServoAnte = AguloServoAnte - 10;
    delay(500);
    }
    
    
    if(n==82){  //girar 1
    AguloServoGarra = AguloServoGarra + 1;
    delay(500);
    }

    if(n==70){  //girar -1
    AguloServoGarra = AguloServoGarra - 1;
    delay(500);
    }

    if(n==114){  //girar 10
    AguloServoGarra = AguloServoGarra + 10;
    delay(500);
    }
    
    if(n==102){  //girar -10
    AguloServoGarra = AguloServoGarra - 10;
    delay(500);
    }
    
    
    if(n==78){  //girar 1
    AguloServoPulso = AguloServoPulso + 1;
    delay(500);
    }

    if(n==77){  //girar -1
    AguloServoPulso = AguloServoPulso - 1;
    delay(500);
    }

    if(n==110){  //girar 10
    AguloServoPulso = AguloServoPulso + 10;
    delay(500);
    }
    
    if(n==109){  //girar -10
    AguloServoPulso = AguloServoPulso - 10;
    delay(500);
    }
    
    
    if(n==80 or n==112){  //Play
    modo = 0;
    delay(500);
    }

    if(n==71 or n==103){  //Gravacao
    modo = 1;
    delay(500);
    }

    if(n==88 or n==120 or n==32){  //Grava Posicao
    botaoGrava = true;
    delay(500);
    }
    
    if(n==61){  //velocidade + 5
    velocidade = velocidade + 5;
    delay(500);
    }
    
    if(n==45){  //velocidade - 5
    velocidade = velocidade - 5;
    delay(500);
    }
    
  }
  Serial.flush();
 }
  
void setMemoria(byte posicao, byte servo, byte valor) {
 int posMem;

    posMem = ((posicao * 5) + servo) + 1;
    EEPROM.write(posMem, valor);
}

byte readMemoria(byte posicao, byte servo) {
 int posMem;

    posMem = ((posicao * 5) + servo) + 1;
    return EEPROM.read(posMem);
}