Vnh2sp Monster motorshield & Joystick Control

       


int xPin = A4; // A0-A5 analog pinlerinden herhangi birine bağlanabilir.
int yPin = A5; // A0-A5 analog pinlerinden herhangi birine bağlanabilir.
int butonPin = 10; // Joystick buton pini arduino bağlantısı (Joystick SW çıkışı)

int xPozisyonu = 0;
int yPozisyonu = 0;
int butonDurum = 0;
int motorLimit = 255;
int halfMotor = int(motorLimit/2.0);
int joystickLimit = 1024;
int halfJoystick = int(joystickLimit / 2.0);
int xDiff =0;
int yDiff =0;
int speed1 = 0;
int speed2 = 0;
int tolerans = 5;

#define BRAKEVCC 0
#define CW   1
#define CCW  2
#define BRAKEGND 3
#define CS_THRESHOLD 100
#define MOTOR_1 0
#define MOTOR_2 1

int usSpeed = 1023;  //default motor speed
unsigned short usMotor_Status = BRAKEVCC;

/*  VNH2SP30 pin definitions
 xxx[0] controls '1' outputs
 xxx[1] controls '2' outputs */
int inApin[2] = {7, 4};  // INA: Clockwise input
int inBpin[2] = {8, 9}; // INB: Counter-clockwise input
int pwmpin[2] = {5, 6}; // PWM input
int cspin[2] = {2, 3}; // CS: Current sense ANALOG input
int enpin[2] = {0, 1}; // EN: Status of switches output (Analog pin)

int statpin = 13;

void setup()
{
  Serial.begin(9600);
  
  pinMode(statpin, OUTPUT);

  // Initialize digital pins as outputs
  for (int i=0; i<2; i++)
  {
    pinMode(inApin[i], OUTPUT);
    pinMode(inBpin[i], OUTPUT);
    pinMode(pwmpin[i], OUTPUT);
  }
  // Initialize braked
  for (int i=0; i<2; i++)
  {
    digitalWrite(inApin[i], LOW);
    digitalWrite(inBpin[i], LOW);
  }
 
}

void loop()
{
  xPozisyonu = analogRead(xPin);
  yPozisyonu = analogRead(yPin);
  butonDurum = digitalRead(butonPin);
  /*
  if(butonDurum == 1) {
    xPozisyonu = halfJoystick;
    yPozisyonu = halfJoystick;
  }
  */
  xDiff = xPozisyonu - halfJoystick;
  yDiff = yPozisyonu - halfJoystick;
  xDiff = map(xDiff, 0, halfJoystick, 0, halfMotor);
  yDiff = -map(yDiff, 0, halfJoystick, 0, halfMotor);
  speed1 = abs(yDiff);
  speed2 = abs(abs(yDiff)- abs(xDiff));
  Serial.print("---X: ");
  Serial.print(xPozisyonu);
  Serial.print("|Y: ");
  Serial.print(yPozisyonu);
  Serial.print("|B: ");
  Serial.print(butonDurum);
  Serial.print("|xDiff: ");
  Serial.print(xDiff);
  Serial.print("|yDiff: ");
  Serial.print(yDiff);
  
  Serial.print("|S1: ");
  Serial.print(speed1);
  Serial.print("|S2: ");
  Serial.print(speed2);
  Serial.print("----\r\n\r");
  

  if(abs(xDiff) < tolerans && abs(yDiff) < tolerans){
    Serial.print("sag dur ");
    Serial.print("sol dur\r\n\r");
    usMotor_Status = BRAKEVCC;
    motorGo(MOTOR_1, usMotor_Status, 0);
    motorGo(MOTOR_2, usMotor_Status, 0);
  }
  else if(xDiff == 0 && yDiff > 0) {
    Serial.print("sol one ");
    Serial.print("sag one\n\r");
    usMotor_Status = CW;
    motorGo(MOTOR_1, usMotor_Status, speed1);
    motorGo(MOTOR_2, usMotor_Status, speed1);
  }
  else if(xDiff == 0 && yDiff <= 0) {
    Serial.print("sol arka ");
    Serial.print("sag arka\n\r");
    usMotor_Status = CCW;
    motorGo(MOTOR_1, usMotor_Status, speed1);
    motorGo(MOTOR_2, usMotor_Status, speed1);
  }
  else if(xDiff > 0 && yDiff == 0) {
    Serial.print("sol one ");
    Serial.print("sag arka\n\r");
    usMotor_Status = CW;
    motorGo(MOTOR_1, usMotor_Status, speed1);
    usMotor_Status = CCW;
    motorGo(MOTOR_2, usMotor_Status, speed2);
  }
  else if(xDiff <= 0 && yDiff == 0) {
    Serial.print("sol arka ");
    Serial.print("sag one\n\r");
    usMotor_Status = CCW;
    motorGo(MOTOR_1, usMotor_Status, speed1);
    usMotor_Status = CW;
    motorGo(MOTOR_2, usMotor_Status, speed2);
  }
  else if(xDiff > 0 && yDiff > 0) {
    Serial.print("yon : Front right");
    Serial.print("sol one hizli ");
    Serial.print("sag one yavas\n\r");
    usMotor_Status = CW;
    motorGo(MOTOR_1, usMotor_Status, speed1);
    motorGo(MOTOR_2, usMotor_Status, speed2);
  }
  else if(xDiff < 0 && yDiff < 0) {
    Serial.print("yon : back left ");
    Serial.print("sol arka yavas ");
    Serial.print("sag arka hizli\n\r");
    usMotor_Status = CCW;
    motorGo(MOTOR_1, usMotor_Status, speed2);
    motorGo(MOTOR_2, usMotor_Status, speed1);
  }
  else if(xDiff > 0 && yDiff < 0) {
    Serial.print("yon : back right ");
    Serial.print("sol arka hizli ");
    Serial.print("sag arka yavas\n\r");
    usMotor_Status = CCW;
    motorGo(MOTOR_1, usMotor_Status, speed1);
    motorGo(MOTOR_2, usMotor_Status, speed2);
  }
  else if(xDiff < 0 && yDiff > 0) {
    Serial.print("yon : Front left ");
    Serial.print("sol one yavas");
    Serial.print("sag one hizli\n\r");
    usMotor_Status = CW;
    motorGo(MOTOR_1, usMotor_Status, speed2);
    motorGo(MOTOR_2, usMotor_Status, speed1);
  }
  else {
    Serial.println("Anlamsiz");
  }
  
  
  if ((analogRead(cspin[0]) < CS_THRESHOLD) && (analogRead(cspin[1]) < CS_THRESHOLD)) {
    Serial.println("Stat pin activ");
    digitalWrite(statpin, HIGH);
  }
}

void motorOff(int motor)
{
  // Initialize braked
  for (int i=0; i<2; i++)
  {
    digitalWrite(inApin[i], LOW);
    digitalWrite(inBpin[i], LOW);
  }
  analogWrite(pwmpin[motor], 0);
}


void motorGo(uint8_t motor, uint8_t direct, uint8_t pwm)
{
  if (motor <= 1)
  {
    if (direct <=4)
    {
      // Set inA[motor]
      if (direct <=1)
        digitalWrite(inApin[motor], HIGH);
      else
        digitalWrite(inApin[motor], LOW);

      // Set inB[motor]
      if ((direct==0)||(direct==2))
        digitalWrite(inBpin[motor], HIGH);
      else
        digitalWrite(inBpin[motor], LOW);

      analogWrite(pwmpin[motor], pwm);
    }
  }
}