// Arduino joystick DC motor speed and rotation direction control
#define joystick1 A0     //frá joystic
#define pwm1 9           //frá joystic
#define pwm2 10          //frá joystic
#define pwm3 5           //frá joystic
#define pwm4 6           //frá joystic

#define joystick2 A1     //frá joystic
int limitSwitch1 =  A2;   //endapinni A2 á Ardo
int limitSwitch2 =  A3;   //endapinni A3 á Ardo
int limitSwitch3 =  A4;   //endapinni A4 á Ardo
int limitSwitch4 =  A5;   //endapinni A5 á Ardo
int motor_control;

void setup() {

  pinMode(limitSwitch1,  INPUT_PULLUP);
  pinMode(limitSwitch2,  INPUT_PULLUP);
  pinMode(limitSwitch3,  INPUT_PULLUP);
  pinMode(limitSwitch4,  INPUT_PULLUP);

  pinMode(pwm1, OUTPUT);
  pinMode(pwm2, OUTPUT);
  pinMode(pwm3, OUTPUT);
  pinMode(pwm4, OUTPUT);
}

void loop() {

  //Get the state of each limit switch
  //then if OKx is false then force the matching PWM signal to 0
  // OK1 will be false if limitSwitch1 is pressed etc
  bool OK1 = digitalRead(limitSwitch1); //pwm3 control
  bool OK2 = digitalRead(limitSwitch2); //pwm4 control
  bool OK3 = digitalRead(limitSwitch3); //pwm1 control
  bool OK4 = digitalRead(limitSwitch4); //pwm2 control

  motor_control = analogRead(joystick1);
  motor_control >>= 1; //Halves the analog reading - now in range 0 - 511

  if (motor_control > 255) {
    digitalWrite(pwm2, 0);
    if (OK3) {
      analogWrite(pwm1, (motor_control - 256));
    }  else {
      analogWrite(pwm1, 0) ;
    }
  } else if (motor_control < 255) {
    digitalWrite(pwm1, 0);
    if (OK4) {
      analogWrite(pwm2, (255 - motor_control));
    }  else {
      analogWrite(pwm2, 0) ;
    }

  } else {  //motor_control is exactly 255 - very small dead range
    digitalWrite(pwm1, 0);
    digitalWrite(pwm2, 0);
  }

  motor_control = analogRead(joystick2);
  motor_control >>= 1; //Halves the analog reading - now in range 0 - 511

  if (motor_control > 255) {
    digitalWrite(pwm4, 0);
    if (OK1) {
      analogWrite(pwm3, (motor_control - 256))  ;
    }  else {
      analogWrite(pwm3, 0) ;
    }
  } else if (motor_control < 255) {
    digitalWrite(pwm3, 0);
    if (OK2) {
      analogWrite(pwm4, (255 - motor_control)) ;
    }  else {
      analogWrite(pwm4, 0) ;
    }
  } else {
    digitalWrite(pwm3, 0);
    digitalWrite(pwm4, 0);
  }
}