Tunirobots 2018 (robotics competition) Autonomous robot code. The robot is based on a PID controller + optic encoders to control distance and orientation, Serial to read instructions, EEPROM to store and load instructions and servo motors + stepper motor to do actions.

main.ino

#include <EEPROM.h>
#include <Servo.h>
Servo servo1, servo2, servo3, servo4, servo5, servo6, servo7;

///////////////////////
//EEPROM variables
#define mode 1

#define test_mode 0
#define EEPROM_mode 1

#define strategy  0 
#define strategyA 0 
#define strategyB 1 
int EEPROM_line = 0;
///////////////////////
//encoder variables
#define encoder0PinA 19
#define encoder0PinB 18
#define encoder1PinA 20
#define encoder1PinB 21
long encoder0Pos = 0;
long encoder1Pos = 0;
///////////////////////
//motor variables
#define motor_g_0 6
#define motor_g_1 7
#define motor_d_0 8
#define motor_d_1 9
///////////////////////
////control variables
//teta variables
float kp_teta_g=0.021,kp_teta_d=0.021,kd_teta_g=0.015,kd_teta_d=0.015,ki_teta_g=0.000,ki_teta_d=0.0;
//float kp_teta_g=0.017,kp_teta_d=0.017,kd_teta_g=0.01,kd_teta_d=0.01,ki_teta_g=0.000004,ki_teta_d=0.000004;
float consigne_teta = 0,cmd_teta_g,cmd_teta_d;
float teta = 0,e_teta,somme_erreur_teta,last_e_teta;
float epsilon_teta = 15;
float k_teta = 0.0005;

//l variables
float kp_l_g=0.03,kp_l_d=0.03,kd_l_g=0.02,kd_l_d=0.02,ki_l_g=0,ki_l_d=0;//0.000003

//float kp_l_g=0.00,kp_l_d=0.00,kd_l_g=0,kd_l_d=0,ki_l_g=0,ki_l_d=0;
float consigne_l=0,cmd_l_g,cmd_l_d;
float l = 0,e_l ,somme_erreur_l, last_e_l;
float output_d,output_g ;
float epsilon_l=50;
//float k_l=0.001278 ;
float k_l=0.0010 ;
long max_time = 10000000;
///////////////////////
//serial variables
String serial_input = "0";
int print_counter = 0 ;
///////////////////////
//stepper variables
int stepTime=50;
int en = 22 ;
int steps=24;
int dir =26;
///////////////////////
//timer
unsigned long loop_timer ;
///////////////////////
//relays variables
#define servo_en_0 30
#define servo_en_1 42
#define stepper_en 40
///////////////////////
#define start_button 36
void setup() {
  init_stepper();

  pinMode(encoder0PinA, INPUT_PULLUP);
  pinMode(encoder0PinB, INPUT_PULLUP);
  pinMode(encoder1PinA, INPUT_PULLUP);
  pinMode(encoder1PinB, INPUT_PULLUP);

  pinMode(servo_en_0, OUTPUT);
  pinMode(servo_en_1, OUTPUT);
  pinMode(stepper_en,OUTPUT);
  relays_init();
      
  pinMode(motor_g_0,OUTPUT);
  pinMode(motor_g_1,OUTPUT);
  pinMode(motor_d_0,OUTPUT);
  pinMode(motor_d_1,OUTPUT);

  void doEncoder0A();
  void doEncoder0B();
  void doEncoder1A();
  void doEncoder1B();

  attachInterrupt(digitalPinToInterrupt(encoder0PinA), doEncoder0A, CHANGE);
  attachInterrupt(digitalPinToInterrupt(encoder0PinB), doEncoder0B, CHANGE); 
  attachInterrupt(digitalPinToInterrupt(encoder1PinA), doEncoder1A, CHANGE);
  attachInterrupt(digitalPinToInterrupt(encoder1PinB), doEncoder1B, CHANGE); 

  Serial.begin (9600);

  loop_timer =micros();
  init_servos();
  print_EEPROM();
  delay(1000);
  //if (mode == test_mode)clear_EEPROM();
  pinMode (start_button,INPUT_PULLUP);
 // while (digitalRead(start_button)!=LOW);
  //delay(500);
  encoder0Pos = 0;
  encoder1Pos = 0;
}


void loop(){
  if(mode == test_mode){
    read_from_serial();
    execute_commands(serial_input);
    pid();  
  }
  else if (EEPROM_line<EEPROM_size()) {
    Serial.println(EEPROM_read_instruction(EEPROM_line));
    execute_commands(EEPROM_read_instruction(EEPROM_line));
    EEPROM_line ++;
  }
}

void doEncoder0A(){

  // look for a low-to-high on channel A

  if (digitalRead(encoder0PinA) == HIGH) {
    // check channel B to see which way encoder is turning
    if (digitalRead(encoder0PinB) == LOW) { 
      encoder0Pos = encoder0Pos + 1;         // CW
    }else{
      encoder0Pos = encoder0Pos - 1;         // CCW
    }
  }

  else   // must be a high-to-low edge on channel A                                      
  {

    // check channel B to see which way encoder is turning 
    if (digitalRead(encoder0PinB) == HIGH) {  
      encoder0Pos = encoder0Pos + 1;          // CW
    }
    else {
      encoder0Pos = encoder0Pos - 1;          // CCW
    }
  }
}


void doEncoder0B(){

  // look for a low-to-high on channel B
  if (digitalRead(encoder0PinB) == HIGH) {  
   // check channel A to see which way encoder is turning
    if (digitalRead(encoder0PinA) == HIGH) { 
      encoder0Pos = encoder0Pos + 1;         // CW
    }
    else {
      encoder0Pos = encoder0Pos - 1;         // CCW
    }
  }

  // Look for a high-to-low on channel B
  else {
    // check channel B to see which way encoder is turning 
    if (digitalRead(encoder0PinA) == LOW) {  
      encoder0Pos = encoder0Pos + 1;          // CW
    }
    else {
      encoder0Pos = encoder0Pos - 1;          // CCW
    }
  }
}

void doEncoder1A(){

  // look for a low-to-high on channel A

  if (digitalRead(encoder1PinA) == HIGH) {
    // check channel B to see which way encoder is turning
    if (digitalRead(encoder1PinB) == LOW) { 
      encoder1Pos = encoder1Pos + 1;         // CW
    }

    else {
      encoder1Pos = encoder1Pos - 1;         // CCW

    }
  }

  else   // must be a high-to-low edge on channel A                                      
  {

    // check channel B to see which way encoder is turning 
    if (digitalRead(encoder1PinB) == HIGH) {  
      encoder1Pos = encoder1Pos + 1;          // CW
    }
    else {
      encoder1Pos = encoder1Pos - 1;          // CCW
    }
  }
}


void doEncoder1B(){

  // look for a low-to-high on channel B
  if (digitalRead(encoder1PinB) == HIGH) {  
   // check channel A to see which way encoder is turning
    if (digitalRead(encoder1PinA) == HIGH) { 
      encoder1Pos = encoder1Pos + 1;         // CW
    }
    else {
      encoder1Pos = encoder1Pos - 1;         // CCW
    }
  }

  // Look for a high-to-low on channel B
  else {
    // check channel B to see which way encoder is turning 
    if (digitalRead(encoder1PinA) == LOW) {  
      encoder1Pos = encoder1Pos + 1;          // CW
    }
    else {
      encoder1Pos = encoder1Pos - 1;          // CCW
    }
  }
}

void apply_output(){
//  output_d = 0;
//  output_g = 0;
  if(output_d >0){
    digitalWrite(motor_d_0,LOW);
    analogWrite(motor_d_1,output_d*255/5); //constrain(output_d*255/5,255,30);
  }
  else {
    digitalWrite(motor_d_1,LOW);
    analogWrite(motor_d_0,(-1)*output_d*255/5);
  }
 if(output_g >0){
    digitalWrite(motor_g_0,LOW);
    analogWrite(motor_g_1,output_g*255/5);
  }
  else {
    digitalWrite(motor_g_1,LOW);
    analogWrite(motor_g_0,(-1)*output_g*255/5);
  }
}
void print_data(){

  if(print_counter==100){
   // Serial.print(0);
  // Serial.print(",");
  // Serial.println(e_l); 
//  Serial.print("encoder_0:");
//  Serial.println(encoder0Pos);
//  Serial.print("encoder_1:");
//  Serial.println(encoder1Pos);
//  Serial.print(encoder0Pos);
  //Serial.print(",");
  //Serial.println(encoder1Pos);
//    Serial.print("consigne_l:");
//    Serial.println(consigne_l);
//    Serial.print("l:");
//    Serial.println(l);
//    Serial.print("consigne_teta:");
//    Serial.println(consigne_teta);
//    Serial.print("teta:");
//    Serial.println(teta);
    print_counter= 0;
  }
  print_counter ++;
}


void read_from_serial() {
  
  if (Serial.available()) {
    serial_input="";
    serial_input = Serial.readString();
    if(!is_edit_command(serial_input))EEPROM_add_instruction(serial_input);
  }
}
void execute_commands(String input_command){
  if (input_command.substring(0,1)=="k")k_l = input_command.substring(2).toFloat();
  else if (input_command =="detach")detach_servos();
  else if (input_command =="stop")stop_stepper();
  else if (input_command =="ccw")moveCCW();
  else if (input_command =="cw")moveCW();
  else if (input_command=="act")activate_stepper();
  else if (input_command=="hold")hold_stepper();
  else if(input_command=="stop")stop_stepper();
  ///to debug
  else if(input_command=="clr")clear_EEPROM();
  else if(input_command=="print")print_EEPROM();
  else if(input_command.substring(0,6)=="insert")EEPROM_insert_instruction(input_command.substring(7,input_command.lastIndexOf(' ')),input_command.substring(input_command.lastIndexOf(' ')+1).toInt());
  else if(input_command.substring(0,6)=="delete")EEPROM_delete_instruction(input_command.substring(7).toInt());
  else if(input_command.substring(0,4)=="edit")EEPROM_write_instruction(input_command.substring(5,input_command.lastIndexOf(' ')),input_command.substring(input_command.lastIndexOf(' ')+1).toInt());
 /// end of debug
  else if (input_command.substring(0,2)=="up")up(input_command.substring(3).toInt());
  else if (input_command.substring(0,4)=="down")down(input_command.substring(5).toInt());
  else if (input_command.substring(0,5)=="delay")delay_pid(input_command.substring(6).toInt());
  else if (input_command.substring(0,8)=="stepTime") stepTime = input_command.substring(9).toInt();
  else if (input_command.substring(0,1)=="v") virage(input_command.substring(2,input_command.substring(2).indexOf(' ')+2).toFloat()*6394/1000,input_command.substring(input_command.substring(2).indexOf(' ')+3).toFloat()*1250/90);
  else if (input_command.substring(0,1)=="o") orientation(input_command.substring(2).toFloat()*1250/90);
  else if (input_command.substring(0,1)=="d") distance(input_command.substring(2).toFloat()*6394/1000);
  else if (input_command.substring(0,2)=="ms") move_servo_sweep(input_command.substring(3,input_command.substring(3).indexOf(' ')+3).toInt(),input_command.substring(input_command.substring(3).indexOf(' ')+4,input_command.lastIndexOf(' ')).toInt(),input_command.substring(input_command.lastIndexOf(' ')).toInt());
  else if(input_command.substring(0,3)=="pld")kp_l_d = input_command.substring(4).toFloat();
  else if(input_command.substring(0,3)=="ild")ki_l_d = input_command.substring(4).toFloat();
  else if(input_command.substring(0,3)=="dld")kd_l_d = input_command.substring(4).toFloat();
  else if(input_command.substring(0,3)=="plg")kp_l_g = input_command.substring(4).toFloat();
  else if(input_command.substring(0,3)=="ilg")ki_l_g = input_command.substring(4).toFloat();
  else if(input_command.substring(0,3)=="dlg")kd_l_g = input_command.substring(4).toFloat();
  else if(input_command.substring(0,3)=="ptd")kp_teta_d = input_command.substring(4).toFloat();
  else if(input_command.substring(0,3)=="itd")ki_teta_d = input_command.substring(4).toFloat();
  else if(input_command.substring(0,3)=="dtd")kd_teta_d = input_command.substring(4).toFloat();
  else if(input_command.substring(0,3)=="ptg")kp_teta_g = input_command.substring(4).toFloat();
  else if(input_command.substring(0,3)=="itg")ki_teta_g = input_command.substring(4).toFloat();
  else if(input_command.substring(0,3)=="dtg")kp_teta_g = input_command.substring(4).toFloat();
  else if(input_command.substring(0,4)=="cmdl")consigne_l = (input_command.substring(5).toFloat())*6394/1000;
  else if(input_command.substring(0,4)=="cmdt")consigne_teta = input_command.substring(5).toFloat();
  else if(input_command.substring(0,5)=="pltet")Serial.println(e_teta);
  else if(input_command.substring(0,5)=="pltel")Serial.println(e_l);
  else move_servo(input_command.substring(0,input_command.indexOf(' ')).toInt(),input_command.substring(input_command.indexOf(' ')+1).toInt());
  serial_input = "0";
}



void distance (float setpoint){
 delay(500);
 long offset = micros();
 long loop_timer1 = micros();
  float constant ;
  if(setpoint>0)constant = k_l;
  else constant = -k_l ;
  float last = consigne_l ;
  while((abs(setpoint-(l-last))>epsilon_l)&&(micros()-offset<max_time)){
    
    l = 0.5*(encoder0Pos+encoder1Pos);
    teta = encoder0Pos-encoder1Pos;
    consigne_l = last+constant * (micros()-offset);
    if(abs(consigne_l-last)>abs(setpoint))consigne_l=setpoint+last;
  
       // distance control
    e_l=consigne_l-l;
    somme_erreur_l+=e_l ;
    cmd_l_g =kp_l_g * e_l + ki_l_g * somme_erreur_l + kd_l_g * (e_l-last_e_l);
    cmd_l_d =kp_l_d * e_l + ki_l_d * somme_erreur_l + kd_l_d * (e_l-last_e_l);
    // orientation control
    e_teta= consigne_teta - teta ;
    somme_erreur_teta += e_teta ;
    cmd_teta_g = kp_teta_g * e_teta + ki_teta_g * somme_erreur_teta+kd_teta_g*(e_teta-last_e_teta);
    cmd_teta_d = kp_teta_d * e_teta + ki_teta_d * somme_erreur_teta+kd_teta_d*(e_teta-last_e_teta); 
    output_d =cmd_l_d *0.8+ cmd_teta_d*3;
    output_g =cmd_l_g*0.8 - cmd_teta_g*3;
  
  // saturation
    saturation ();
    
    apply_output();
  
  // updating last errors
    last_e_teta=e_teta;
    last_e_l = e_l;
  
  
    while(micros()-loop_timer1 <5000)  ;   
    
    loop_timer1 = micros();
  }
  consigne_l=setpoint+last;
  serial_input = "0";
}


void orientation (float setpoint){
  delay(500);
 long offset = micros();
 long loop_timer1 = micros();
 float constant ;
 if(setpoint>0)constant = k_teta;
 else constant = -k_teta;
 float last = consigne_teta;
  while((abs(setpoint-(teta-last))>epsilon_teta)&&(micros()-offset<max_time)){
    l = 0.5*(encoder0Pos+encoder1Pos);
    teta = encoder0Pos-encoder1Pos;
    
    consigne_teta =last +constant * (micros()-offset);
    if(abs(consigne_teta-last)>abs(setpoint))consigne_teta=setpoint+last;
  
       // distance control
    e_l=consigne_l-l;
    somme_erreur_l+=e_l ;
    cmd_l_g =kp_l_g * e_l + ki_l_g * somme_erreur_l + kd_l_g * (e_l-last_e_l);
    cmd_l_d =kp_l_d * e_l + ki_l_d * somme_erreur_l + kd_l_d * (e_l-last_e_l);
    // orientation control
    e_teta= consigne_teta - teta ;
    somme_erreur_teta += e_teta ;
    cmd_teta_g = kp_teta_g * e_teta + ki_teta_g * somme_erreur_teta+kd_teta_g*(e_teta-last_e_teta);
    cmd_teta_d = kp_teta_d * e_teta + ki_teta_d * somme_erreur_teta+kd_teta_d*(e_teta-last_e_teta); 
    output_d =cmd_l_d *0.8+ cmd_teta_d;
    output_g =cmd_l_g*0.8 - cmd_teta_g;
  
  // saturation
    saturation ();
    apply_output();
  
  // updating last errors
    last_e_teta=e_teta;
    last_e_l = e_l;
  
  
    while(micros()-loop_timer1 <5000)    ;   
    loop_timer1 = micros();
  }
  consigne_teta=setpoint+last;
  serial_input="0";
}

void saturation () {
    if (output_d>5)output_d=5;
    if(output_d<-5)output_d=-5;
    if (output_g>5)output_g=5;
    if(output_g<-5)output_g=-5;
}


void virage (float setpoint_l , float setpoint_teta){
  float k_teta1 = 0.0006;
 long offset = micros();
 long loop_timer1 = micros();
 
 float constant_teta ;
 if(setpoint_teta>0)constant_teta = k_teta1;
 else constant_teta = -k_teta1;
 float last_teta = consigne_teta;
 
 float constant_l ;
 if(setpoint_l>0)constant_l = k_l;
 else constant_l = -k_l;
 float last_l = consigne_l;
 
  while((abs(setpoint_teta-(teta-last_teta))>epsilon_teta)||(abs(setpoint_l-(l-last_l))>epsilon_l)){
    l = 0.5*(encoder0Pos+encoder1Pos);
    teta = encoder0Pos-encoder1Pos;
    
    consigne_teta =last_teta +constant_teta * (micros()-offset);
    if(abs(consigne_teta-last_teta)>abs(setpoint_teta))consigne_teta=setpoint_teta+last_teta;

    consigne_l =last_l +constant_l * (micros()-offset);
    if(abs(consigne_l-last_l)>abs(setpoint_l))consigne_l=setpoint_l+last_l;
 
       // distance control
    e_l=consigne_l-l;
    somme_erreur_l+=e_l ;
    cmd_l_g =kp_l_g * e_l + ki_l_g * somme_erreur_l + kd_l_g * (e_l-last_e_l);
    cmd_l_d =kp_l_d * e_l + ki_l_d * somme_erreur_l + kd_l_d * (e_l-last_e_l);
    // orientation control
    e_teta= consigne_teta - teta ;
    somme_erreur_teta += e_teta ;
    cmd_teta_g = kp_teta_g * e_teta + ki_teta_g * somme_erreur_teta+kd_teta_g*(e_teta-last_e_teta);
    cmd_teta_d = kp_teta_d * e_teta + ki_teta_d * somme_erreur_teta+kd_teta_d*(e_teta-last_e_teta); 
    output_d =cmd_l_d *0.8+ cmd_teta_d;
    output_g =cmd_l_g*0.8 - cmd_teta_g;
  
  // saturation
    saturation ();
    apply_output();
  
  // updating last errors
    last_e_teta=e_teta;
    last_e_l = e_l;
  
  
    while(micros()-loop_timer1 <5000)    ;   
    
    loop_timer1 = micros();
  }
  serial_input="0";
}

void init_servos(){
  digitalWrite(servo_en_0,LOW);
  digitalWrite(servo_en_1,HIGH);
  //servo1.attach(6);servo1.write (140);delay(1000);servo1.detach();   //servo syst 3atef 1
  servo2.attach(7);servo2.write(100);delay(1000); servo2.detach();    //fitouri d100  a150
  servo3.attach(9);servo3.write(90);delay(1000);servo3.detach();    //pince cylindre 70 ouvert 30 fermé 
  servo4.attach(8);servo4.write(70);delay(1000);servo4.detach();    //syst fitouri
 // servo5.attach(6);servo5.write(50);delay(1000);servo5.detach(); 
  //servo6.attach(10);servo6.write(160);delay(1000); servo6.detach();    //pince cube 160 ouvert 100 fermé
  //servo7.attach(11);servo7.write(152);delay(1000);servo7.detach();     //port 152 fermée
  
  digitalWrite(servo_en_0,HIGH);
  digitalWrite(servo_en_1,LOW);
}

void move_servo(int pin_servo, int angle){
  if(pin_servo ==6 ){
    servo1.attach(6);
    servo1.write(angle);
    delay(2000);
   // servo1.detach();
    }
  else if(pin_servo ==7 ){
    servo2.attach(7);  
    servo2.write(angle);    
    delay(2000);
    servo2.detach();
    }
  else if(pin_servo ==9) 
  {
    servo3.attach(9); 
    servo3.write(angle);
    delay(500);
    //servo3.detach();
    }
  else if(pin_servo ==8 ){
    servo4.attach(8); 
    servo4.write(angle);
    delay(2000);
    //servo4.detach();
    }
  else if(pin_servo ==11 ){
    servo7.attach(11); 
    servo7.write(angle);
    delay(1000);
    //servo7.detach();
    }
  else if(pin_servo ==10){
    servo6.attach(10); 
    servo6.write(angle);
    delay(500);
    //servo6.detach();
    }
    serial_input = "0";
}

void move_servo_sweep(int pin_servo ,int pos0,int posf){

  int inc ;
  int delay_time = 20;
  if(pos0<=posf)inc = 1;
  else inc = -1;
  for(int pos = pos0; pos*inc<=posf*inc; pos+=inc){
    if(pin_servo ==6 ){
      servo1.attach(6);
      servo1.write(pos);
      delay(delay_time);
      //servo1.detach();
    }
    else if(pin_servo ==7 ){
      servo2.attach(7);  
      servo2.write(pos);    
      delay(delay_time);
      //servo2.detach();
    }
    else if(pin_servo ==9) 
    {
      servo3.attach(9); 
      servo3.write(pos);
      delay(delay_time);
      //servo3.detach();
    }
    else if(pin_servo ==8 ){
      servo4.attach(8); 
      servo4.write(pos);
      delay(delay_time);
      //servo4.detach();
    }
    else if(pin_servo ==10){
      servo6.attach(10); 
      servo6.write(pos);
      delay(delay_time);
      //servo6.detach();
    }
    else if(pin_servo ==11){
      servo7.attach(11); 
      servo6.write(pos);
      delay(delay_time);
      //servo6.detach();
    }
  }
  serial_input = "0";
}
void detach_servos(){
  servo1.detach();
  servo2.detach();
  servo3.detach();
  servo4.detach(); 
  //servo5.detach();
  servo6.detach();
  servo7.detach();
}

void init_stepper(){
  pinMode(steps, OUTPUT);
  pinMode(dir, OUTPUT);
  pinMode(en,OUTPUT);
  stop_stepper();
}
void moveCCW(){
  digitalWrite(dir, HIGH);
  digitalWrite(steps, HIGH);   
  delay(stepTime);                      
  digitalWrite(steps, LOW);    
  delay(stepTime);     
}
void moveCW(){
  digitalWrite(dir, LOW);
  digitalWrite(steps, HIGH);   
  delay(stepTime);                      
  digitalWrite(steps, LOW);    
  delay(stepTime);     
}
void stop_stepper(){
  digitalWrite(en,HIGH);
  digitalWrite(dir, LOW);
  digitalWrite(stepper_en,HIGH);
}
void hold_stepper(){
  //Serial.println("activating stepper");
  digitalWrite(dir, LOW);
  digitalWrite(stepper_en,LOW);
}
void activate_stepper(){
  //Serial.println("activating stepper");
  digitalWrite(stepper_en,LOW);
}

void up(int up_distance){

  stepTime = 1;
  for (int i =1; i<up_distance; i++){
    moveCW();
  }
  hold_stepper();
  serial_input="0";
}
void down (int down_distance){
  stepTime = 1;
  for (int i=1; i<down_distance; i++){
    moveCCW();
  }
  hold_stepper();
    serial_input="0";
}

void relays_init(){
  pinMode(33,OUTPUT);
  pinMode(35,OUTPUT);
  pinMode(37,OUTPUT);
  digitalWrite(33,LOW);
  digitalWrite(35,LOW);
  digitalWrite(37,LOW);
}
void pid(){
  
  // distance control
  l = 0.5*(encoder0Pos+encoder1Pos);
  e_l=consigne_l-l;
  somme_erreur_l+=e_l ;
  cmd_l_g =kp_l_g * e_l + ki_l_g * somme_erreur_l + kd_l_g * (e_l-last_e_l);
  cmd_l_d =kp_l_d * e_l + ki_l_d * somme_erreur_l + kd_l_d * (e_l-last_e_l);
  // orientation control
  teta = encoder0Pos-encoder1Pos;
  e_teta= consigne_teta - teta ;
  somme_erreur_teta += e_teta ;
  cmd_teta_g = kp_teta_g * e_teta + ki_teta_g * somme_erreur_teta+kd_teta_g*(e_teta-last_e_teta);
  cmd_teta_d = kp_teta_d * e_teta + ki_teta_d * somme_erreur_teta+kd_teta_d*(e_teta-last_e_teta); 
     
//  cmd_teta_g=0;
//  cmd_teta_d=0;
  output_d =cmd_l_d *0.8+ cmd_teta_d*2;
  output_g =cmd_l_g*0.8 - cmd_teta_g*2;
// saturation
  if (output_d>5)output_d=5;
  if(output_d<-5)output_d=-5;
  if (output_g>5)output_g=5;
  if(output_g<-5)output_g=-5;

  apply_output();

// updating last errors
  last_e_teta=e_teta;
  last_e_l = e_l;
  print_data();
  while(micros()-loop_timer <5000)    ;   
  loop_timer = micros();
}
void delay_pid(int duration_ms){
  long delay_timer = millis();
  while(millis()-delay_timer<duration_ms){
    pid();
  }
}  
int address_offset(){
  if(strategy == strategyA)return 0;
  else return 2000;
}
void update_EEPROM_size(int EEPROM_size){
  EEPROM.write(0+address_offset(),EEPROM_size>>8);
  EEPROM.write(1+address_offset(),EEPROM_size);
}
int EEPROM_size(){
  return (EEPROM.read(0+address_offset())<<8|EEPROM.read(1+address_offset()));
}
void inc_EEPROM_size(){
  update_EEPROM_size(EEPROM_size()+1);
}
void dec_EEPROM_size(){
  update_EEPROM_size(EEPROM_size()-1);
}
void clear_EEPROM(){
  update_EEPROM_size(0);
}
int EEPROM_line_to_address(int line){
  return 2 + line * 20+address_offset();
}
void EEPROM_write_instruction(String instruction, int line ){
  for (int i =0 ; i<instruction.length();i++){
    EEPROM.write(EEPROM_line_to_address(line)+i,instruction.charAt(i));
  }
  EEPROM.write(EEPROM_line_to_address(line)+instruction.length(),'\0');
}
String EEPROM_read_instruction(int line){
  String temp = "";
  int i = 0;
  while((char)EEPROM.read(EEPROM_line_to_address(line)+i)!='\0'){
    temp+=(char)EEPROM.read(EEPROM_line_to_address(line)+i);
    i++;
  }
  return temp;
}
void EEPROM_add_instruction(String instruction){
  Serial.println("added");
  Serial.println(EEPROM_size());
  EEPROM_write_instruction(instruction,EEPROM_size());
  inc_EEPROM_size();
}
void print_EEPROM (){
  Serial.print("program");
  if(strategy==strategyA)Serial.println("A:");
  else Serial.println("B:");
  for (int i =0; i<EEPROM_size();i++){
    Serial.print("L");Serial.print(i);Serial.print(":");Serial.print(EEPROM_read_instruction(i));Serial.println("*");
  }
  serial_input ="0";
  Serial.println("end of program");
}

bool is_edit_command(String command){
  return (command.substring(0,3) =="clr")||(command.substring(0,5) =="print")||(command.substring(0,6) =="insert")||(command.substring(0,6) =="delete")||(command.substring(0,4) =="edit")||(command =="0");
}

void EEPROM_insert_instruction(String instruction,int line){
  String temp = EEPROM_read_instruction(line);
  EEPROM_write_instruction(instruction,line);
  for(int line_i = line + 1; line_i<EEPROM_size();line_i++){
    String temp1 = EEPROM_read_instruction(line_i);
    EEPROM_write_instruction(temp,line_i);
    temp = temp1;    
  }
  EEPROM_add_instruction(temp);
}

void  EEPROM_delete_instruction(int line){
  for (int line_i = line ;line_i<EEPROM_size()-1;line_i++){
    EEPROM_write_instruction(EEPROM_read_instruction(line_i+1),line_i);
  }
  dec_EEPROM_size();
  serial_input="0";
}


//////////// instructions : ////////////
/*
 * d 930
 * o -90
 * d -60
 * 6 60
 * 8 50
 * 8 40
 * 8 30
 * 8 70
 * 6 140
 * detach
 * d 100
 * o 90
 * d 610
 * d -300
 * o -174  // to verify
 * d 350
 * o 90
 * d 400
 * o -90
 */