#include <TB6612FNG.h>
#include <PS2Mouse.h>
 
Motor *left_track;
Motor *right_track;
PS2Mouse *mouse;
 
void get_mouse_data(int& mstat, int& left_encoder, int& right_encoder);
void smooth_move(int l_tr, int r_tr);
 
int freeRAM()
{
 extern int __heap_start, *__brkval; 
 int v; 
 return (int) &v - (__brkval == 0 ? (int) &__heap_start : (int) __brkval); 
}
 
void setup()
{
 Serial.begin(9600);
 left_track = new Motor(6, 5, 7);
 right_track = new Motor(3, 4, 2);
 //delay(1000);  //wait for the mouse to self-check
 Serial.println("Start-----------------------------");
 mouse = new PS2Mouse(9, 8, REMOTE);
 pinMode(13, OUTPUT);
 digitalWrite(13, HIGH);
 mouse->initialize();
 
 if (mouse->is_error())
 {
  while (true);
 }
 Serial.println("Initialized");
 digitalWrite(13, LOW);
}
 
void loop()
{
 const int IRsensitivity = 360;
 const byte IR_pin = A3;
 
 boolean goon = true;
 int total_dist_travelled_l = 0;
 int total_dist_travelled_r = 0;
 int spd_reg_factor = 1;
 
 left_track->ChangeDirection(FORWARD);
 right_track->ChangeDirection(FORWARD);
 left_track->ChangeSpeed(255);
 right_track->ChangeSpeed(255);
 
 left_track->Rotate();
 right_track->Rotate();
 
 /*Serial.print("Speed regulations: ");
 Serial.print(left_track->GetSpeedRegulation());
 Serial.print(",");
 Serial.println(right_track->GetSpeedRegulation());*/
 
 while (goon)
 {
  Serial.println("Start of loop");
  int m_stat = 0;
  int l_enc = 0;
  int r_enc = 0;
 
  get_mouse_data(m_stat, l_enc, r_enc);
 
  Serial.print("LENC: ");
  Serial.print(l_enc);
  Serial.print(",RENC: ");
  Serial.println(r_enc);
 
 
  total_dist_travelled_l += l_enc;
  total_dist_travelled_r += r_enc;
 
  if (r_enc != l_enc)
  {
   byte difference = 0;
   Motor *faster_track;
   Motor *slower_track;
   if (r_enc > l_enc)
   {
    Serial.println("Zpomaluji PRAVY pas");
    difference = r_enc - l_enc;
    faster_track = right_track;
    slower_track = left_track;
   } else if (l_enc > r_enc)
   {
    Serial.println("Zpomaluji LEVY pas");
    difference = l_enc - r_enc;
    faster_track = left_track;
    slower_track = right_track;
   }
   if (slower_track->GetSpeedRegulation() < 255)
   {
    slower_track->ChangeSpeedRegulation(slower_track->GetSpeedRegulation() + spd_reg_factor);
   } else
   {
    faster_track->ChangeSpeedRegulation(faster_track->GetSpeedRegulation() - spd_reg_factor);
   }
  }
 
  if (analogRead(IR_pin) < IRsensitivity) goon = false;  //obstacle detected
  Serial.print("End of loop.\n Free RAM: ");Serial.println(freeRAM());
 }
 
 left_track->Stop();
 right_track->Stop();
 
 delay(5000);
 /*
 left_track->ChangeDirection(REVERSE);
 right_track->ChangeDirection(REVERSE);
 
 goon = true;
 while (goon)
 {
  int m_stat = 0;
  int l_enc = 0;
  int r_enc = 0;
  
  get_mouse_data(m_stat, l_enc, r_enc);
  //Serial.println("Za gmd");
  total_dist_travelled_l += l_enc;
  total_dist_travelled_r += r_enc;
  
  
  if (r_enc != l_enc)
  {
   Motor *faster_track;
   Motor *slower_track;
   if (r_enc < l_enc)
   {
    faster_track = right_track;
    slower_track = left_track;
   } else if (l_enc < r_enc)
   {
    faster_track = left_track;
    slower_track = right_track;
   }
    if (slower_track->GetSpeedRegulation() < 255)
    {
     slower_track->ChangeSpeedRegulation(slower_track->GetSpeedRegulation() + spd_reg_factor);
    } else
    {
     faster_track->ChangeSpeedRegulation(faster_track->GetSpeedRegulation() - spd_reg_factor);
    }
  }
  
  if (abs(total_dist_travelled_l) < 255 && abs(total_dist_travelled_r) < 255)
  {
   if (abs(total_dist_travelled_l) < 5 && abs(total_dist_travelled_r) < 5)
   {
    goon = false;
   } else
   {
    smooth_move(total_dist_travelled_l, total_dist_travelled_r);
   }
  }
 }*/
}
 
void get_mouse_data(int& mstat, int& left_encoder, int& right_encoder)
{
 int data[2] = {0};
 mouse->report(data);
 mstat = data[0];
 right_encoder = -data[1];
 left_encoder = -data[2];
}
 
 
void smooth_move(int l_tr, int r_tr)
{
 const int min_spd = 50;
 const int tolerance = 5;
 if (abs(l_tr) < tolerance) l_tr = 0;
 if (abs(r_tr) < tolerance) r_tr = 0;
 
 if (abs(l_tr) > 0)
 {
  if (l_tr > 0)
  {
   left_track->ChangeDirection(FORWARD);
  } else
  {
   left_track->ChangeDirection(REVERSE);
  }
  if (abs(l_tr) < 255)
  {
   int spd = abs(l_tr);
   if (spd < min_spd) spd = min_spd;
   left_track->ChangeSpeed(spd);
  } else
  {
   left_track->ChangeSpeed(255);
  }
  left_track->Rotate();
 } else
 {
  left_track->Stop();
 }
 
 if (abs(r_tr) > 0)
 {
  if (r_tr > 0)
  {
   right_track->ChangeDirection(FORWARD);
  } else
  {
   right_track->ChangeDirection(REVERSE);
  }
  if (abs(r_tr) < 255)
  {
   int spd = abs(r_tr);
   if (spd < min_spd) spd = min_spd;
   right_track->ChangeSpeed(spd);
  } else
  {
   right_track->ChangeSpeed(255);
  }
  right_track->Rotate();
 } else
 {
  right_track->Stop();
 }
 
}
 
Parsed using GeSHi 1.0.8.11 at 0.019