// EyeBot Demo Program: Drive, T. Bräunl, Nov. 2017
// Allowing speed control via PWM
#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 motors(int a, int b)
{ softPwmWrite(Left1, a);
  softPwmWrite(Left2, 0);  // only needed for reversing
  softPwmWrite(Right1, b);
  softPwmWrite(Right2, 0); // only needed for reversing
}


int main (void)
{ LCDPrintf("init pins\n");
  pwm_init(26, 27, 28, 29);
  
  LCDPrintf("Motor Left and Right ramp up\n");
  for (int i=0; i<100; i++)
  { motors(i, i); delay(50);}

  LCDPrintf("Motor Left and Right ramp down\n");
  for (int i=100; i>0; i--)
  { motors(i, i); delay(50);}

  return 0 ;
}