// Omnidirectional Drive - Mecanum Wheels
// "Avoid" behavior - keep equal distance to all 4 sides
// Thomas Braunl 2018
#include "eyebot.h"
#define S 50  // spee [0..100]d
#define T 50  //distance threshold [mm] 

// Global speed variables
int Fleft, Fright, Bleft, Bright;

void Mdrive()
{ MOTORDrive(1, Fleft);
  MOTORDrive(2, Fright);
  MOTORDrive(3, Bleft);
  MOTORDrive(4, Bright);
}

void setspeed(int a, int b, int c, int d)
{ Fleft = a; Fright = b;
  Bleft = c; Bright = d;
}

void addspeed(int a, int b, int c, int d)
{ Fleft += a; Fright += b;
  Bleft += c; Bright += d;
}


int main ()
{ int left, front, right, back, Xdiff, Ydiff;

  while(1)
  { left  = PSDGet(1);
    front = PSDGet(2);
    right = PSDGet(3);
    back  = PSDGet(4);
    LCDPrintf("L%d F%d  R%d  B%d\n", left, front, right, back);

    Xdiff = front - back;
    Ydiff = left  - right;
    setspeed(0, 0, 0, 0);
    if (Xdiff >  T) addspeed( S,  S,  S,  S);
    if (Xdiff < -T) addspeed(-S, -S, -S, -S);
    if (Ydiff >  T) addspeed(-S,  S,  S, -S);
    if (Xdiff < -T) addspeed( S, -S, -S,  S);
    Mdrive();
  }
  return 0;
}

