Lập trình Arduino điều khiển mạch động cơ VNH2SP30 bằng bluetooth.

[ntdanh1997]

Dear ace
Hiện tại em đang làm đồ án tốt nghiệp với đề tài là máy cắt cỏ tự động. Tới phần lập trình Arduino để điểu khiển mạch động cơ VNH2SP30 thì gặp tình trạng sau:
- 2 động cơ tiến lên, quẹo trái bình thường.
- Động cơ 2 không chạy khi lùi và quẹo phải.
Đây là code của tụi em mọi người xem qua coi có sai sót chỗ nào cần sửa không ạ. Cám ơn mọi người.

#define BRAKE 0
#define CW 1
#define CCW 2
#define CS_THRESHOLD 15 // Definition of safety current (Check: "1.3 Monster Shield Example").

//MOTOR 1
#define MOTOR_A1_PIN 7
#define MOTOR_B1_PIN 8

//MOTOR 2
#define MOTOR_A2_PIN 4
#define MOTOR_B2_PIN 9

#define PWM_MOTOR_1 5
#define PWM_MOTOR_2 6

#define CURRENT_SEN_1 A2
#define CURRENT_SEN_2 A3

#define EN_PIN_1 A0
#define EN_PIN_2 A1

#define MOTOR_1 0
#define MOTOR_2 1

short usSpeed = 255; //default motor speed

void setup()
{
pinMode(MOTOR_A1_PIN, OUTPUT);
pinMode(MOTOR_B1_PIN, OUTPUT);

pinMode(MOTOR_A2_PIN, OUTPUT);
pinMode(MOTOR_B2_PIN, OUTPUT);

pinMode(PWM_MOTOR_1, OUTPUT);
pinMode(PWM_MOTOR_2, OUTPUT);

pinMode(CURRENT_SEN_1, OUTPUT);
pinMode(CURRENT_SEN_2, OUTPUT);

pinMode(EN_PIN_1, OUTPUT);
pinMode(EN_PIN_2, OUTPUT);

Serial.begin(9600); // Initiates the serial to do the monitoring
Serial.println(); //Print function list for user selection
Serial.println("Enter number for control option:");
Serial.println("1. FORWARD");
Serial.println("2. BACKWARD");
Serial.println("3. RIGHT");
Serial.println("4. LEFT");
Serial.println("5. STOP");
Serial.println();
}

void loop()
{
char user_input;
while(Serial.available())
{
user_input = Serial.read(); //Read user input and trigger appropriate function
digitalWrite(EN_PIN_1, HIGH);
digitalWrite(EN_PIN_2, HIGH);

if (user_input =='1')
{
Forward();
}
else if(user_input =='2')
{
Backward();
}
else if(user_input =='3')
{
Right();
}
else if(user_input =='4')
{
Left();
}
else if(user_input =='5')
{
Stop();
}

}
}

void Stop()
{
Serial.println("STOP");
motorGo(MOTOR_1, BRAKE, 0);
motorGo(MOTOR_2, BRAKE, 0);
}

void Forward()
{
Serial.println("FORWARD");
motorGo(MOTOR_1, CW, usSpeed);
motorGo(MOTOR_2, CCW, usSpeed);
}

void Backward()
{
Serial.println("BACKWARD");
motorGo(MOTOR_1, CCW, usSpeed);
motorGo(MOTOR_2, CW, usSpeed);
}

void Right()
{
Serial.println("RIGHT");
motorGo(MOTOR_1, CW, usSpeed);
motorGo(MOTOR_2, CW, usSpeed);
}

void Left()
{
Serial.println("LEFT");
motorGo(MOTOR_1, CCW, usSpeed);
motorGo(MOTOR_2, CCW, usSpeed);
}

void motorGo(uint8_t motor, uint8_t direct, uint8_t pwm)
//Function that controls the variables: motor(0 ou 1), direction (CW ou CCW) e pwm (entra 0 e 255);
{
if(motor == MOTOR_1)
{
if(direct == CW)
{
digitalWrite(MOTOR_A1_PIN, LOW);
digitalWrite(MOTOR_B1_PIN, HIGH);
}
else if(direct == CCW)
{
digitalWrite(MOTOR_A1_PIN, HIGH);
digitalWrite(MOTOR_B1_PIN, LOW);
}
else
{
digitalWrite(MOTOR_A1_PIN, LOW);
digitalWrite(MOTOR_B1_PIN, LOW);
}

analogWrite(PWM_MOTOR_1, pwm);

}
if(motor == MOTOR_2)
{
if(direct == CW)
{
digitalWrite(MOTOR_A2_PIN, LOW);
digitalWrite(MOTOR_B2_PIN, HIGH);
}
else if(direct == CCW)
{
digitalWrite(MOTOR_A2_PIN, HIGH);
digitalWrite(MOTOR_B2_PIN, LOW);
}
else
{
digitalWrite(MOTOR_A2_PIN, LOW);
digitalWrite(MOTOR_B2_PIN, LOW);
}

analogWrite(PWM_MOTOR_2, pwm);

}
}