mc4.ino

mc4.ino

#include <Wire.h>
#include "PS2X_lib.h"
#include "QGPMaker_MotorShield.h"

QGPMaker_MotorShield AFMS = QGPMaker_MotorShield();
PS2X ps2x;

QGPMaker_DCMotor *DCMotor_2 = AFMS.getMotor(2);
QGPMaker_DCMotor *DCMotor_4 = AFMS.getMotor(4);
QGPMaker_DCMotor *DCMotor_1 = AFMS.getMotor(1);
QGPMaker_DCMotor *DCMotor_3 = AFMS.getMotor(3);

void setup(){
  
  AFMS.begin(50);

  int error = 0;
  do {
    error = ps2x.config_gamepad(13, 11, 10, 12, true, true);
    if (error == 0){
      break;
    } else {
      delay(100);
    }
  } while(1);

}

int speed = 0;

void loop(){
  ps2x.read_gamepad(false, 0);
  delay(30);

  if (ps2x.Button(PSB_PAD_UP)) {
    speed += 1;
  }

  if (ps2x.Button(PSB_PAD_DOWN)) {
    speed -= 1;
  }

  if (speed < 0) {
    speed = 0;
  }

  if (speed > 255) {
    speed = 255;
  }

  if (speed) {
    DCMotor_1->setSpeed(speed);
    DCMotor_2->setSpeed(speed);
    DCMotor_3->setSpeed(speed);
    DCMotor_4->setSpeed(speed);
    DCMotor_1->run(FORWARD);
    DCMotor_2->run(FORWARD);
    DCMotor_3->run(FORWARD);
    DCMotor_4->run(FORWARD);
  } else {
    DCMotor_1->run(RELEASE);
    DCMotor_2->run(RELEASE);
    DCMotor_3->run(RELEASE);
    DCMotor_4->run(RELEASE);
  }

  delay(2);

}