// EyeBot Demo Program: Drive, T. Bräunl, Nov. 2017
// Allowing speed control via PWM as well as reversing
#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);
}


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

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

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


  return 0 ;
}