Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #define ENA 5
- #define IN1 4
- #define IN2 3
- #define IN3 8
- #define IN4 9
- #define ENB 10
- #define potPin 0
- int Mspeed;
- void setup() {
- Serial.begin(9600);
- // initialize digital pin LED_BUILTIN as an output.
- pinMode(IN1, OUTPUT);
- pinMode(IN2, OUTPUT);
- pinMode(IN3, OUTPUT);
- pinMode(IN4, OUTPUT);
- pinMode(ENA,OUTPUT);
- pinMode(ENB, OUTPUT);
- pinMode(potPin,INPUT);
- }
- void loop()
- {
- Mspeed= analogRead(potPin)/4;
- Serial.println(Mspeed);
- delay(100);
- control_Robot(IN1,IN2,IN3,IN4,100);
- }
- void RobotMove(byte inR1, byte inR2, byte inL1, byte inL2,byte action)
- {
- /*
- * inR1 và inR2 là 2 chân tín hiệu điều khiển động cơ bên phải
- * inL1 và inL2 là 2 chân tín hiệu điều khiển động cơ bên trái
- * action 0 : đứng yên
- * action 1 : đi thẳng
- * action 2 : lệch phải
- * action 3 : lệch trái
- */
- switch (action)
- {
- case 0: // đứng yên
- controlMotor_direction(inR1, inR2, 0);
- controlMotor_direction(inL1, inL2, 0);
- break;
- case 1: // di thang
- controlMotor_direction(inR1, inR2, 1);
- controlMotor_direction(inL1, inL2, 1);
- break;
- case 2: // lech trai
- controlMotor_direction(inR1, inR2, 2);
- controlMotor_direction(inL1, inL2, 1);
- break;
- case 3: // lech phai
- controlMotor_direction(inR1, inR2, 1);
- controlMotor_direction(inL1, inL2, 2);
- break;
- default:
- action=0;
- }
- }
- void controlMotor_direction(byte in1, byte in2, byte direction)
- {
- // in1,in2 là tín hiệu điều khiển hướng motor
- //0 : stop
- //1 : di cùng chiều
- //2 : đi ngược chiều
- switch (direction)
- {
- case 0: // đứng im
- digitalWrite (in1, LOW);
- digitalWrite (in2, LOW);
- break;
- case 1: // quay cùng chiều
- digitalWrite (in1, HIGH);
- digitalWrite (in2, LOW);
- break;
- case 2: // quay ngược chiều
- digitalWrite (in1, LOW);
- digitalWrite (in2, HIGH);
- break;
- }
- }
- void control_Robot(byte inR1, byte inR2, byte inL1, byte inL2, byte center_distance)
- {
- // RobotMove(inR1,inR2,inL1,inL2,1);
- // Serial.println("Tiến");
- int distance = Mspeed;
- if(distance = center_distance)
- {
- RobotMove(inR1,inR2,inL1,inL2,1);
- analogWrite(ENA,Mspeed);
- analogWrite(ENB,Mspeed);
- Serial.println("đi thẳng");
- delay(1000);
- }
- if (distance > center_distance)
- {
- RobotMove(inR1,inR2,inL1,inL2,3);
- analogWrite(ENA,Mspeed);
- analogWrite(ENB,Mspeed);
- Serial.println("lệch phải");
- delay(1000);
- }
- if (distance < center_distance)
- {
- RobotMove(inR1,inR2,inL1,inL2,2);
- analogWrite(ENA,Mspeed);
- analogWrite(ENB,Mspeed);
- Serial.println("lệch trái");
- delay(1000);
- }
- }
Add Comment
Please, Sign In to add comment