- #define trig 9
- #define ech 10
- #define motor1 5
- #define motor2 6
- int duration = 0;
- int distance = 0;
- // int current_speed = 0;
- // int max_speed = 0;
- int ultra_sonic(int trigger, int echo){
- while(1){
- digitalWrite(trigger, LOW);
- delayMicroseconds(2);
- digitalWrite(trigger, HIGH);
- delayMicroseconds(10);
- digitalWrite(trigger, LOW);
- duration = pulseIn(echo, HIGH, 50000);
- distance = duration * 0.034 / 2;
- if (distance!=0){
- return distance;
- }
- }
- }
- void setup() {
- pinMode(trig,OUTPUT);
- pinMode(ech,INPUT);
- pinMode(motor1, OUTPUT);
- pinMode(motor2, OUTPUT);
- Serial.begin(9600);
- }
- void loop() {
- int distance = ultra_sonic(trig,ech);
- Serial.println(distance);
- if (distance > 40){
- analogWrite(motor1, 255);
- digitalWrite(motor2, LOW);
- distance = ultra_sonic(trig, ech);
- }
- else if (distance > 35 && distance < 40){
- analogWrite(motor1, 125);
- analogWrite(motor2, 0);
- distance = ultra_sonic(trig, ech);
- }
- else if (distance > 30 && distance < 35){
- analogWrite(motor1, 63);
- analogWrite(motor2, 0);
- distance = ultra_sonic(trig, ech);
- }
- else if (distance < 30 && distance > 20){
- digitalWrite(motor1, 0);
- analogWrite(motor2, 0);
- distance = ultra_sonic(trig, ech);
- }
- else if (distance < 20){
- digitalWrite(motor1, 0);
- digitalWrite(motor2, 1);
- }
- }
Recent Pastes