// EyeBot Demo Program: Drive, T. Bräunl, Nov. 2017
// Only turning motors on and off
#include "eyebot.h"

void motors(int a, int b, int c, int d)
{ digitalWrite(26, a);
  digitalWrite(27, b);
  digitalWrite(28, c);
  digitalWrite(29, d);
}


int main (void)
{ LCDPrintf("init pins\n");
  wiringPiSetup();
  pinMode(26, OUTPUT);
  pinMode(27, OUTPUT);
  pinMode(28, OUTPUT);
  pinMode(29, OUTPUT);

  LCDPrintf("Left Motor\n");
  motors(1,0, 0,0); delay(1000); // forward
  motors(0,1, 0,0); delay(1000); // reverse
  motors(0,0, 0,0); delay(1000); // stop

  LCDPrintf("Right Motor\n");
  motors(0,0, 1,0); delay(1000); // forward
  motors(0,0, 0,1); delay(1000); // reverse
  motors(0,0, 0,0); delay(1000); // stop

  return 0 ;
}