L298P motor driver direction control

Hello,

I am using a pixy2 to track objects and have attached it to a large pan tilt. I am able to control the pantilt in all directions in a very basic program by just uncommenting the needed movements.

void setup() {
  pinMode(M1, OUTPUT);
  pinMode(M2, OUTPUT);
  pinMode(4, OUTPUT);
  delay(200);
  digitalWrite(4, HIGH);
  delay(1000);
  digitalWrite(4, LOW);
  delay(1000);
  timer = millis();
}
void loop() {
  
      digitalWrite(M1, HIGH);
      digitalWrite(M2, LOW);
      analogWrite(E1, HIGH);
      analogWrite(E2, HIGH);
      //delay(2000);
      //analogWrite(E1, 0);
      //delay(2000);
      /*digitalWrite(M1, LOW);
      //digitalWrite(M2, HIGH);
      analogWrite(E1, 255);
      //analogWrite(E2, value);
      delay(2000);
      */
      
}

But once I implement the code that works with the pixy2, it only moves the pan tilt in a clockwise direction and will not move the vertical motion motor. On the L298P motor driver, I can see the light for the clockwise direction lighting up and I can also see the light for the counterclockwise direction flashing occasionally. Tracking.ino is the main code that I am having trouble with. I have to where it only moves if the pixy sees an object and will stop moving if the object is out of sight or if the object is centered, just can't move it in the correct direction. If anyone can take a look at my code and tell me where I went wrong, it would be much appreciated!

#include <Pixy2.h>

#define S1  10
#define S2  11
#define VM  12
#define HM  13
//#define POT A0  // for motor speed control (input)
#define BUZZ  4

Pixy2 pixy;
int valx = 255;    // speed calculated from change in distances
int valy = 255;
int x = 0;
int y = 0;
int n = 0;

void setup() {

  Serial.begin(115200);
  Serial.println("Starting...");
  pinMode(HM, OUTPUT);
  pinMode(VM, OUTPUT);
  pinMode(S1, OUTPUT);
  pinMode(S2, OUTPUT);

  pixy.init();

  digitalWrite(S1, 0);
  digitalWrite(S2, 0);

  pixy.ccc.getBlocks();

}

void loop() {

  for (int i = 0; i < pixy.ccc.numBlocks; i++) {
    /*Serial.print(" block ");
    Serial.print(i);
    Serial.print(": ");
    pixy.ccc.blocks[i].print();
    */
    x += pixy.ccc.blocks[i].m_x;
    y += pixy.ccc.blocks[i].m_y;
    n++;
  }

  x = x / n;
  y = y / n;

  if(pixy.ccc.numBlocks){
    Serial.print("X: ");
    Serial.println(x);
    Serial.print("Y: ");
    Serial.println(y);
  }

  followAverage(x, y);

  digitalWrite(S1, LOW);
  digitalWrite(S2, LOW);

  pixy.ccc.getBlocks();
  x = 0;
  y = 0;
  n = 0;

}

// -------------- Fuctions --------------

void followAverage(int x, int y) {
  if (x < 135 && y < 90 && pixy.ccc.numBlocks) {
    digitalWrite(HM, HIGH);
    digitalWrite(VM, HIGH);
    digitalWrite(S1, HIGH);
    digitalWrite(S2, HIGH);
    Serial.println("UL");
    delay(250);
  }
  if (x < 135 && y > 110 && pixy.ccc.numBlocks) {
    digitalWrite(HM, HIGH);
    digitalWrite(VM, LOW);
    digitalWrite(S1, HIGH);
    digitalWrite(S2, HIGH);
    Serial.println("BL");
    delay(250);
  }
  if (x > 165 && y < 90 && pixy.ccc.numBlocks) {
    digitalWrite(HM, LOW);
    digitalWrite(VM, HIGH);
    digitalWrite(S1, HIGH);
    digitalWrite(S2, HIGH);
    Serial.println("UR");
    delay(250);
  }
  if (x > 165 && y > 110 && pixy.ccc.numBlocks) {
    digitalWrite(HM, LOW);
    digitalWrite(VM, LOW);
    digitalWrite(S1, HIGH);
    digitalWrite(S2, HIGH);
    Serial.println("BR");
    delay(250);
  }
  if (x < 135 && y > 90 && y < 110 && pixy.ccc.numBlocks) {
    digitalWrite(HM, HIGH);
    digitalWrite(S1, HIGH);
    Serial.println("L");
    delay(250);
  }
  if (x > 165 && y > 90 && y < 110 && pixy.ccc.numBlocks) {
    digitalWrite(HM, LOW);
    digitalWrite(S1, HIGH);
    Serial.println("R");
    delay(250);
  }
  if (x < 165 && x > 135 && y < 90 && pixy.ccc.numBlocks) {
    digitalWrite(VM, HIGH);
    digitalWrite(S2, HIGH);
    Serial.println("U");
    delay(250);
  }
  if (x < 165 && x > 135 && y > 110 && pixy.ccc.numBlocks) {
    digitalWrite(VM, LOW);
    digitalWrite(S2, HIGH);
    Serial.println("B");
    delay(250);
  }
  if (x < 165 && x > 135 && y < 110 && y > 90) {
    digitalWrite(HM, LOW);
    digitalWrite(VM, LOW);
    digitalWrite(S1, LOW);
    digitalWrite(S2, LOW);
    Serial.println("on target");
    delay(250);
  }
  if (pixy.ccc.numBlocks == 0){
    digitalWrite(HM, LOW);
    digitalWrite(VM, LOW);
    digitalWrite(S1, LOW);
    digitalWrite(S2, LOW);
    delay(250);
  }
}

Tracking.ino (2.92 KB)

move.ino (635 Bytes)