Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- const int speedMotorA = 9;
- const int speedMotorB = 3;
- const int motorPin1 = 4;
- const int motorPin2 = 7;
- const int motorPin3 = 12;
- const int motorPin4 = 13;
- void setup() {
- Serial.begin(9600);
- pinMode(speedMotorA, OUTPUT);
- pinMode(speedMotorB, OUTPUT);
- pinMode(motorPin1, OUTPUT);
- pinMode(motorPin2, OUTPUT);
- pinMode(motorPin3, OUTPUT);
- pinMode(motorPin4, OUTPUT);
- }
- void loop(){
- TurnMotorA();
- TurnMotorB();
- }
- void TurnMotorA() {
- int Speed = analogRead(A2);
- Speed = Speed*0.24926686;
- Serial.println(Speed);
- analogWrite(speedMotorA,Speed);
- digitalWrite(motorPin1, HIGH);
- digitalWrite(motorPin2, LOW);
- }
- void TurnMotorB() {
- int Speed = analogRead(A2);
- Speed = Speed*0.24926686;
- analogWrite(speedMotorB,Speed);
- digitalWrite(motorPin3, HIGH);
- digitalWrite(motorPin4, LOW);
- }
- #include <NewPing.h>
- #include <Servo.h>
- #define TRIG_PIN A4
- #define ECHO_PIN A5
- #define MAX_DISTANCE 200
- NewPing sonar(TRIG_PIN, ECHO_PIN, MAX_DISTANCE);
- Servo myservo;
- const int laser = 8;
- int distance = 0;
- const int speedMotorA = 9;
- const int speedMotorB = 3;
- const int motorPin1 = 4;
- const int motorPin2 = 7;
- const int motorPin3 = 12;
- const int motorPin4 = 13;
- int hitlaser(){
- digitalWrite(laser, HIGH);
- delay(200);
- digitalWrite(laser, LOW);
- }
- void setup() {
- pinMode(laser, OUTPUT);
- pinMode(speedMotorA, OUTPUT);
- pinMode(speedMotorB, OUTPUT);
- pinMode(motorPin1, OUTPUT);
- pinMode(motorPin2, OUTPUT);
- pinMode(motorPin3, OUTPUT);
- pinMode(motorPin4, OUTPUT);
- myservo.attach(5);
- myservo.write(65);
- delay(2000);
- distance = readPing();
- delay(100);
- distance = readPing();
- delay(100);
- distance = readPing();
- delay(100);
- distance = readPing();
- delay(100);
- }
- void loop() {
- int distanceR = 0;
- int distanceL = 0;
- delay(40);
- digitalWrite(laser, HIGH);
- if(distance<=50)
- {
- moveStop();
- delay(100);
- moveBackward();
- delay(300);
- moveStop();
- delay(200);
- distanceR = lookRight();
- delay(200);
- distanceL = lookLeft();
- delay(200);
- if(distanceR>=distanceL)
- {
- turnRight();
- moveStop();
- }else
- {
- turnLeft();
- moveStop();
- }
- }else
- {
- moveForward();
- }
- distance = readPing();
- }
- int lookRight()
- {
- myservo.write(5);
- delay(500);
- int distance = readPing();
- delay(100);
- myservo.write(65);
- return distance;
- }
- int lookLeft()
- {
- myservo.write(150);
- delay(500);
- int distance = readPing();
- delay(100);
- myservo.write(65);
- return distance;
- delay(100);
- }
- int readPing() {
- delay(70);
- int cm = sonar.ping_cm();
- if(cm==0)
- {
- cm = 250;
- }
- return cm;
- }
- void moveStop() {
- analogWrite(speedMotorA, 0);
- analogWrite(speedMotorB, 0);
- digitalWrite(motorPin1, LOW);
- digitalWrite(motorPin2, LOW);
- digitalWrite(motorPin3, LOW);
- digitalWrite(motorPin4, LOW);
- }
- void moveForward() {
- int Speed = analogRead(A2);
- Speed = Speed*0.24926686;
- Serial.println(Speed);
- analogWrite(speedMotorA,Speed);
- analogWrite(speedMotorB,Speed);
- digitalWrite(motorPin1, HIGH);
- digitalWrite(motorPin2, LOW);
- digitalWrite(motorPin3, HIGH);
- digitalWrite(motorPin4, LOW);
- }
- void moveBackward() {
- int Speed = analogRead(A2);
- Speed = Speed*0.24926686;
- Serial.println(Speed);
- analogWrite(speedMotorA,Speed);
- analogWrite(speedMotorB,Speed);
- digitalWrite(motorPin1, LOW);
- digitalWrite(motorPin2, HIGH);
- digitalWrite(motorPin3, LOW);
- digitalWrite(motorPin4, HIGH);
- }
- void turnRight() {
- int Speed = analogRead(A2);
- Speed = Speed*0.24926686;
- Serial.println(Speed);
- analogWrite(speedMotorA,Speed);
- analogWrite(speedMotorB,Speed);
- digitalWrite(motorPin1, HIGH);
- digitalWrite(motorPin2, LOW);
- digitalWrite(motorPin3, LOW);
- digitalWrite(motorPin4, HIGH);
- delay(300);
- moveForward();
- }
- void turnLeft() {
- int Speed = analogRead(A2);
- Speed = Speed*0.24926686;
- Serial.println(Speed);
- analogWrite(speedMotorA,Speed);
- analogWrite(speedMotorB,Speed);
- digitalWrite(motorPin1, LOW);
- digitalWrite(motorPin2, HIGH);
- digitalWrite(motorPin3, HIGH);
- digitalWrite(motorPin4, LOW);
- delay(300);
- moveForward();
- }
Add Comment
Please, Sign In to add comment