// EyeBot Demo Program: Drive, T. Bräunl, Nov. 2017
// VW Interface (v-omega) linear and angular velocity
#include "eyebot.h"
int Left1, Left2, Right1, Right2;

void pwm_init(int a, int b, int c, int d)
{ wiringPiSetup();
  Left1 = a; Left2 = b; Right1 = c; Right2 = d;  // global variables
  softPwmCreate (a, 0, 100);  // setup pin for PWM [0..100]
  softPwmCreate (b, 0, 100);  // setup pin for PWM [0..100]
  softPwmCreate (c, 0, 100);  // setup pin for PWM [0..100]
  softPwmCreate (d, 0, 100);  // setup pin for PWM [0..100]
}


void drive(int Mot1, int Mot2, int speed)
{ if (speed > +100) speed =  100; // limit range
  if (speed < -100) speed = -100; // limit range
  
  if(speed>0)
  { softPwmWrite(Mot1, speed);  // Drive motor forward
    softPwmWrite(Mot2, 0);
  }
  else
  { softPwmWrite(Mot1, 0);      // Drive motor backward
    softPwmWrite(Mot2, -speed);
  }  

}

void motors(int a, int b)
{ drive(Left1,Left2, a);
  drive(Right1,Right2, b);
}

void vw(int speed, int angular)
{ motors(speed-angular, speed+angular);
} 


int main (void)
{ LCDPrintf("init pins\n");
  pwm_init(26, 27, 28, 29);
  
  LCDPrintf("Drive straight\n");
  vw(50, 0); delay(2000);
  vw(0, 0); delay(1000);

  LCDPrintf("Drive curve left\n");
  vw(70, 30); delay(2000);
  vw(0, 0); delay(1000);

  LCDPrintf("Drive curve right\n");
  vw(70, -30); delay(2000);
  vw(0, 0); delay(1000);

  LCDPrintf("Turn left\n");
  vw(0, 25); delay(2000);
  vw(0, 0); delay(1000);

  LCDPrintf("Turn right\n");
  vw(0, -25); delay(2000);
  vw(0, 0); delay(1000);

  LCDPrintf("Stop\n");
  vw(0, 0); delay(1000);

  return 0 ;
}