Hey all, I'm having a problem with this code. When I turn the robot on, the right motor (motor2) spins backwards for about 2 seconds and then it all runs normal. Why in the world is that happening? The code is a work in progress so its a mess. Don't try too hard to understand it all. Here's the code:
const int encoder1A = 2; // Encoder 1
const int motor1_1Pin = 4; // H-bridge 1 leg 1 (1Y, 2Y)
const int motor1_2Pin = 8; // H-bridge 1 leg 2 (3Y, 4Y)
const int enablePin1 = 6; // H-bridge 1 enable
//const int motor2power = 11;
const int motor2_1Pin = 12; // H-bridge 2 leg 1 (1Y, 1Y)
const int motor2_2Pin = 13; // H-bridge 2 leg 2 (3Y, 4Y)
const int enablePin2 = 9; // H-bridge 2 enable
volatile unsigned int encoderCount = 0;
void setup() {
pinMode(encoder1A, INPUT);
pinMode(motor1_1Pin, OUTPUT);
pinMode(motor1_2Pin, OUTPUT);
pinMode(enablePin1, OUTPUT);
digitalWrite(enablePin1, LOW);
pinMode(motor2_1Pin, OUTPUT);
pinMode(motor2_2Pin, OUTPUT);
pinMode(enablePin2, OUTPUT);
digitalWrite(enablePin2, LOW);
attachInterrupt(0, readEncoder, RISING);
// Serial.begin (9600);
// Serial.println("Begin Code");
}
void loop() {
digitalWrite(enablePin1, HIGH);
digitalWrite(enablePin2, HIGH);
digitalWrite(motor1_1Pin, HIGH);
digitalWrite(motor1_2Pin, LOW);
digitalWrite(motor2_1Pin, LOW);
digitalWrite(motor2_2Pin, HIGH);
if((encoderCount == 312) || (encoderCount == 624) || (encoderCount == 936) || (encoderCount == 1248) || (encoderCount == 1560) || (encoderCount == 1872) || (encoderCount == 2184) || (encoderCount == 2496) || (encoderCount == 2808) || (encoderCount == 3120) || (encoderCount == 3432) || (encoderCount == 3744) || (encoderCount == 4056) || (encoderCount == 4368) || (encoderCount == 4680) || (encoderCount == 4992)){
digitalWrite(motor2_2Pin, LOW);
digitalWrite(motor2_2Pin, LOW);
delay(225);
}
if (encoderCount == 1400) {
digitalWrite(motor1_1Pin, LOW);
digitalWrite(motor1_2Pin, LOW);
delay(2730);
interrupts();
}
if (encoderCount == 2250) {
digitalWrite(motor1_1Pin, LOW);
digitalWrite(motor1_2Pin, LOW);
delay(2550);
interrupts();
}
if (encoderCount == 5000) {
digitalWrite(motor1_1Pin, LOW);
digitalWrite(motor1_2Pin, LOW);
delay(2500);
interrupts();
}
if (encoderCount == 5720) {
digitalWrite(motor1_1Pin, HIGH);
digitalWrite(motor1_2Pin, LOW);
digitalWrite(motor2_1Pin, LOW);
digitalWrite(motor2_2Pin, LOW);
delay(2000);
digitalWrite(motor1_1Pin, LOW);
digitalWrite(motor1_2Pin, LOW);
digitalWrite(motor2_1Pin, LOW);
digitalWrite(motor2_2Pin, LOW);
delay(1000);
digitalWrite(motor1_1Pin, LOW);
digitalWrite(motor1_2Pin, HIGH);
digitalWrite(motor2_1Pin, HIGH);
digitalWrite(motor2_2Pin, LOW);
delay(2000);
digitalWrite(motor1_1Pin, LOW);
digitalWrite(motor1_2Pin, LOW);
digitalWrite(motor2_1Pin, LOW);
digitalWrite(motor2_2Pin, LOW);
delay(60000);
}
}
void readEncoder() {
if (digitalRead(encoder1A) == HIGH){
encoderCount++;
if ((encoderCount == 1360) || (encoderCount == 2266) || (encoderCount == 5000) || (encoderCount == 5720)) {
noInterrupts();
}
// Serial.println(encoderCount);
}
}