- #define encoderr 2
- #define encoderl 3
- #define echo1 64
- #define echo2 66
- #define echo3 68
- #define trig1 65
- #define trig2 67
- #define trig3 69
- #define echo4 41
- #define echo5 31
- #define trig4 43
- #define trig5 33
- #define motorr_p 7 // motor - and + conectors
- #define motorr_m 5
- #define motorl_p 8
- #define motorl_m 4
- long int dispulser = 0; // enchoder pulses till now
- long int dispulsel = 0;
- long duration, distance ;
- int rightsensor, midsensor, leftsensor ,right2sensor, mid2sensor, left2sensor;
- void ultra_sonics(int trigPin, int echoPin)
- {
- digitalWrite(trigPin, 0);
- delayMicroseconds(2);
- digitalWrite(trigPin, 1);
- delayMicroseconds(10);
- digitalWrite(trigPin, 0);
- duration = pulseIn(echoPin, 1 , 26000);
- distance = distance;
- distance = duration * 0.034/2;
- }
- void enr()
- {
- dispulser ++;
- }
- void enl()
- {
- dispulsel ++ ;
- }
- void check_ul_sen()
- {
- ultra_sonics(trig1, echo1);
- leftsensor = distance;
- ultra_sonics(trig2, echo2);
- rightsensor = distance;
- ultra_sonics(trig3, echo3);
- midsensor = distance;
- ultra_sonics(trig4, echo4);
- right2sensor = distance;
- ultra_sonics(trig5, echo5);
- left2sensor = distance;
- }
- void setup() {
- // put your setup code here, to run once:
- pinMode(motorl_p , OUTPUT);
- pinMode(motorl_m , OUTPUT);
- pinMode(motorr_p , OUTPUT);
- pinMode(motorr_m , OUTPUT);
- pinMode(trig1, OUTPUT);
- pinMode(trig2, OUTPUT);
- pinMode(trig3, OUTPUT);
- pinMode(trig4, OUTPUT);
- pinMode(trig5, OUTPUT);
- pinMode(echo1, INPUT);
- pinMode(echo2, INPUT);
- pinMode(echo3, INPUT);
- pinMode(echo4, INPUT);
- pinMode(echo5, INPUT);
- Serial.begin(115200);
- attachInterrupt(digitalPinToInterrupt(encoderr), enr, RISING);
- attachInterrupt(digitalPinToInterrupt(encoderl), enl, RISING);
- }
- void loop() {
- // put your main code here, to run repeatedly:
- check_ul_sen();
- //analogWrite(motorl_p , 255);
- //analogWrite(motorl_m , 0);
- //analogWrite(motorr_p , 255);
- // analogWrite(motorr_m , 0);
- Serial.print('L');
- Serial.print(leftsensor);
- Serial.print(" R ");
- Serial.print(rightsensor);
- Serial.print(" M ");
- Serial.print(midsensor);
- Serial.print(" L2 ");
- Serial.print(left2sensor);
- Serial.print(" R2 ");
- Serial.println(right2sensor);
- Serial.print(" enl ");
- Serial.print(dispulsel);
- Serial.print(" enr ");
- Serial.println(dispulser);
- delay(300);
- }
Recent Pastes