Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #include <Servo.h>
- const int trigPin = 9;
- const int echoPin = 10;
- unsigned long previousMillis;
- unsigned long interval = 500;
- int MoveForward = 0;
- int MoveBackward = 1;
- int MoveLeft = 3;
- int MoveRight = 4;
- int distanceTraveled = 0;
- bool lastState;
- Servo myservo;
- void setup() {
- Serial.begin(9600);
- pinMode(A3, INPUT);
- pinMode(A4, INPUT);
- pinMode(4, INPUT);
- pinMode(5, INPUT);
- pinMode(7, OUTPUT);
- pinMode(8, OUTPUT);
- pinMode(12, OUTPUT);
- pinMode(13, OUTPUT);
- pinMode(10, OUTPUT);
- pinMode(11, OUTPUT);
- previousMillis = millis();
- myservo.attach(A1);
- pinMode(A0, INPUT);
- pinMode(trigPin, OUTPUT); // Sets the trigPin as an Output
- pinMode(echoPin, INPUT); // Sets the echoPin as an Input
- }
- bool targetReatched = false;
- void loop() {
- int v1 = digitalRead(4);
- Serial.print(v1);
- int v2 = digitalRead(5);
- Serial.print(v2);
- String str = Serial.readString();
- ParseInput(str);
- }
- float EchoDistance()
- {
- digitalWrite(trigPin, LOW);
- delayMicroseconds(2);
- // Sets the trigPin on HIGH state for 10 micro seconds
- digitalWrite(trigPin, HIGH);
- delayMicroseconds(10);
- digitalWrite(trigPin, LOW);
- // Reads the echoPin, returns the sound wave travel time in microseconds
- float duration = pulseIn(echoPin, HIGH);
- // Calculating the distance
- float distance = duration*0.034 / 2;
- return distance;
- }
- void ParseInput(String str)
- {
- if (str.length() <= 0)
- return;
- char func = str[0];
- int arg = str.substring(2).toInt();
- if (str[0] == 's')
- {
- Stop();
- }
- else if (str[0] == 'm')
- {
- GoStraightDistance(arg);
- }
- else if (str[0] == 'r')
- {
- TurnDistance(arg);
- }
- else if (str[0] == 'b')
- {
- Serial.println(EchoDistance());
- }
- else if (str[0] == 'p')
- {
- RotateServo(arg);
- }
- else if (str[0] == 'c')
- {
- Serial.println(ReadDistanceFromInfraRed());
- }
- else if (str[0] == 'w')
- {
- RotateServo(arg);
- Serial.println(ReadDistanceFromInfraRed());
- }
- else if (str[0] == 's')
- {
- LoopRotateAndCheckDistance();
- }
- }
- float ReadDistanceFromInfraRed()
- {
- float v = digitalRead(A0);
- float u = v / 1023.0f * 3.30f;
- if (u < 0.50f)
- {
- u = 0.50f;
- }
- return 70.0f / u - 6.50f;
- }
- void LoopRotateAndCheckDistance()
- {
- int currentRot = 0;
- for (currentRot = 0; currentRot <= 180; currentRot += 1) {
- myservo.write(currentRot);
- Serial.println("rot: %i, dist: %f", currentRot, ReadDistanceFromInfraRed());
- delay(15);
- }
- for (currentRot = 180; currentRot >= 0; currentRot -= 1) {
- myservo.write(currentRot);
- Serial.println("rot: %i, dist: %f", currentRot, ReadDistanceFromInfraRed());
- delay(15);
- }
- }
- void RotateServo(int angle)
- {
- myservo.write(angle);
- }
- void GoStraightDistance(int distanceToTravel)
- {
- distanceTraveled = 0;
- int dir = MoveForward;
- if (distanceToTravel < 0)
- {
- dir = MoveBackward;
- distanceToTravel = distanceToTravel * -1;
- Serial.print(distanceToTravel);
- }
- while (distanceTraveled < distanceToTravel)
- {
- if (dir == MoveForward)
- {
- Front();
- }
- else if (dir == MoveBackward)
- {
- Back();
- }
- CalcDistanceTraveled();
- }
- Stop();
- }
- void TurnDistance(int distanceToTravel)
- {
- distanceTraveled = 0;
- int dir = MoveLeft;
- if (distanceToTravel < 0)
- {
- dir = MoveRight;
- distanceToTravel = distanceToTravel * -1;
- }
- while (distanceTraveled < distanceToTravel)
- {
- if (dir == MoveLeft)
- {
- LeftFront();
- }
- else if (dir == MoveRight)
- {
- RightFront();
- }
- CalcDistanceTraveled();
- }
- Stop();
- }
- void CalcDistanceTraveled() {
- int state = digitalRead(A3);
- if (state != lastState)
- {
- distanceTraveled++;
- Serial.print("distanceTraveled:");
- Serial.print(distanceTraveled);
- Serial.println();
- }
- lastState = state;
- }
- void RightFront()
- {
- digitalWrite(7, HIGH);
- digitalWrite(8, LOW);
- digitalWrite(10, HIGH);
- digitalWrite(12, LOW);
- digitalWrite(13, HIGH);
- digitalWrite(11, HIGH);
- }
- void LeftFront()
- {
- digitalWrite(7, LOW);
- digitalWrite(8, HIGH);
- digitalWrite(10, HIGH);
- digitalWrite(12, HIGH);
- digitalWrite(13, LOW);
- digitalWrite(11, HIGH);
- }
- void Front()
- {
- digitalWrite(7, HIGH);
- digitalWrite(8, LOW);
- digitalWrite(10, HIGH);
- digitalWrite(12, HIGH);
- digitalWrite(13, LOW);
- digitalWrite(11, HIGH);
- }
- void Back()
- {
- digitalWrite(7, LOW);
- digitalWrite(8, HIGH);
- digitalWrite(10, HIGH);
- digitalWrite(12, LOW);
- digitalWrite(13, HIGH);
- digitalWrite(11, HIGH);
- }
- void Stop()
- {
- digitalWrite(10, 0);
- digitalWrite(11, 0);
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement