#include <Wire.h>
#include <EEPROM.h>
 
const int motorA1 = 8;  // IN1
const int motorA2 = 9;  // IN2
const int motorAp = 10;  //
const int motorB1 = 7;  // IN1
const int motorB2 = 6;  // IN2
const int motorBp = 5;  //
const int STBY = 11;


int addr = 0;
int readaddr = 0;
byte values;

String mode = "m";

void setup() {
  // put your setup code here, to run once:
  pinMode( motorA1, OUTPUT );
  pinMode( motorA2, OUTPUT );
  pinMode( motorAp, OUTPUT );
  pinMode( motorB1, OUTPUT );
  pinMode( motorB2, OUTPUT );
  pinMode( motorBp, OUTPUT );
  pinMode( STBY, OUTPUT );
   
  digitalWrite( motorA1, LOW );
  digitalWrite( motorA2, LOW );
  digitalWrite( motorAp, LOW );
  digitalWrite( motorB1, LOW );
  digitalWrite( motorB2, LOW );
  digitalWrite( motorBp, LOW );
  digitalWrite( STBY, HIGH );
   
  Serial.begin(9600);
  delay( 1000 );
}
 
void loop() {
  // put your main code here, to run repeatedly:
  //Serial.println("motorA forward");

  if(Serial.available() > -1){
    switch(Serial.read()){

      case 'm':
        mode = "m";
        break;
      case 'r':
        mode = "r";
        break;
    }
  }

  if(mode == "m"){
     analogWrite( motorAp, 100 );
     digitalWrite( motorA1, HIGH );
     digitalWrite( motorA2, LOW );
     delay( 200 );
     //writeMEMS();
   
     analogWrite( motorAp, 0 );
     delay( 200 );
     //writeMEMS();
 
     analogWrite( motorBp, 120 );
     digitalWrite( motorB1, HIGH );
     digitalWrite( motorB2, LOW );
     delay( 200 );
     //writeMEMS();
     analogWrite( motorBp, 0 );
     delay(200);
     //writeMEMS();

    digitalWrite( motorA1, LOW );
    digitalWrite( motorA2, LOW );
    digitalWrite( motorAp, LOW );
    digitalWrite( motorB1, LOW );
    digitalWrite( motorB2, LOW );
    digitalWrite( motorBp, LOW );
    
  }else if(mode == "r"){

    values = EEPROM.read(readaddr);
    Serial.println(values,DEC);

    readaddr = readaddr + 1;
    if(readaddr == EEPROM.length()){
      addr = 0;
    }

    delay(500);

    
  }
  
  
 
  
}



void writeMEMS(){
 int MEMSval = analogRead(1);
  int valtoEEPROM = MEMSval / 4;
  EEPROM.write(addr,valtoEEPROM);
  
  addr = addr + 1;
  if(addr == EEPROM.length()){
    addr = 0;
  }

}