// rudimentary pigeon control
// using Adafruit's 2009 motor shield (library)
int joystickpins[] = { A1, A2, A3, A4 }; // for Arcade joystick
#define IDLE 15    // joystick states (8 + IDLE)
#define FW 14
#define BA 13
#define LL 7
#define RR 11
#define FL 6
#define FR 10
#define BL 5
#define BR 9
// define 4 motors
#include <AFMotor.h>

AF_DCMotor FLmotor(4);
AF_DCMotor FRmotor(3);
AF_DCMotor BLmotor(1);
AF_DCMotor BRmotor(2);

void setup() {
  Serial.begin(9600);  // set up Serial library at 9600 bps
  Serial.println("Real Urben Pigeon Eye View Project Main Control Software (RUPEVPMCSS)");
  pinMode(A0, OUTPUT);  // gnd for the joystick
  for (int n = 0; n < 4; n++) {
    pinMode(joystickpins[n], INPUT_PULLUP);
  }
  FRmotor.setSpeed(240);
  FLmotor.setSpeed(240);
  BRmotor.setSpeed(240);
  BLmotor.setSpeed(240);
}
void loop() {
  static unsigned long looptime;
  int joyinput = 0;
  if (millis() > looptime + 49) {  // 20Hz (20 times a sec)
    looptime = millis();
    joyinput = 0;
    for (int n = 0; n < 4; n++) {
      joyinput += digitalRead(joystickpins[n]) << n;
    }
    // now the ugly and bulky switch
    switch (joyinput) {
      case FW:
        Serial.println("forward");
        FRmotor.setSpeed(240);
        FLmotor.setSpeed(240);
        BRmotor.setSpeed(240);
        BLmotor.setSpeed(240);
        FRmotor.run(FORWARD);
        FLmotor.run(FORWARD);
        BLmotor.run(FORWARD);
        BRmotor.run(FORWARD);
        break;
      case BA:
        Serial.println("backward");
        FRmotor.setSpeed(240);
        FLmotor.setSpeed(240);
        BRmotor.setSpeed(240);
        BLmotor.setSpeed(240);
        FRmotor.run(BACKWARD);
        FLmotor.run(BACKWARD);
        BLmotor.run(BACKWARD);
        BRmotor.run(BACKWARD);
        break;
      case LL:
        Serial.println("spin left");
        FRmotor.setSpeed(240);
        FLmotor.setSpeed(240);
        BRmotor.setSpeed(240);
        BLmotor.setSpeed(240);
        FRmotor.run(FORWARD);
        FLmotor.run(BACKWARD);
        BLmotor.run(BACKWARD);
        BRmotor.run(FORWARD);
        break;
      case RR:
        Serial.println("spin right");
        FRmotor.setSpeed(240);
        FLmotor.setSpeed(240);
        BRmotor.setSpeed(240);
        BLmotor.setSpeed(240);
        FRmotor.run(BACKWARD);
        FLmotor.run(FORWARD);
        BLmotor.run(FORWARD);
        BRmotor.run(BACKWARD);
        break;
      case FR:
        Serial.println("turn right");
        FRmotor.setSpeed(100);
        BRmotor.setSpeed(100);
        FLmotor.setSpeed(240);
        BLmotor.setSpeed(240);
        FLmotor.run(FORWARD);
        BLmotor.run(FORWARD);
        FRmotor.run(FORWARD);
        BRmotor.run(FORWARD);
        break;
      case FL:
        Serial.println("turn left");
        FRmotor.setSpeed(240);
        BRmotor.setSpeed(240);
        FLmotor.setSpeed(100);
        BLmotor.setSpeed(100);
        FRmotor.run(FORWARD);
        BRmotor.run(FORWARD);
        FLmotor.run(FORWARD);
        BLmotor.run(FORWARD);
        break;
      default:
        FRmotor.run(RELEASE);
        FLmotor.run(RELEASE);
        BLmotor.run(RELEASE);
        BRmotor.run(RELEASE);
        break;
    }
  }
}