#define CW 0 #define CCW 1 // Motor definitions to make life easier: #define MOTOR_A 0 #define MOTOR_B 1 // Pin Assignments // // Don't change these! These pins are statically defined by shield layout const byte PWMA = 3; // PWM control (speed) for motor A const byte PWMB = 11; // PWM control (speed) for motor B const byte DIRA = 12; // Direction control for motor A const byte DIRB = 13; // Direction control for motor B int Trig = 6; //pin 2 Arduino połączony z pinem Trigger czujnika int Echo = 7; //pin 3 Arduino połączony z pinem Echo czujnika int CM; //odległość w cm long CZAS; //długość powrotnego impulsu w uS int wPrawo=0; const int SPEED=200; const int HALF_SPEED=127; const int QUARTER_SPEED=80; long randNumber; void setup() { setupArdumoto(); // Set all pins as outputs setupCzujnik(); randomSeed(analogRead(0)); } void loop() { distanceMeasurement(); //pomiar odległości if (CM > 40) { //both motors forward driveArdumoto(MOTOR_A, CW, HALF_SPEED); // Set motor A to CCW at max driveArdumoto(MOTOR_B, CCW, HALF_SPEED); // Set motor B to CCW at max }; if (CM <= 40 and CM >17) { randNumber = random(300); if (randNumber>=200) { //turn left driveArdumoto(MOTOR_A, CW, HALF_SPEED); // Set motor A tQUARTER_SPEEDo CCW at max driveArdumoto(MOTOR_B, CW, HALF_SPEED); // Set motor B to CCW at max delay(300); } else { //turn right driveArdumoto(MOTOR_A, CCW, HALF_SPEED); // Set motor A to CCW at max driveArdumoto(MOTOR_B, CCW, HALF_SPEED); // Set motor B to CCW at max } }; if (CM <= 17) { //both motors backwards driveArdumoto(MOTOR_A, CCW, HALF_SPEED); // Set motor A to CCW at max driveArdumoto(MOTOR_B, CW, HALF_SPEED); // Set motor B to CCW at max delay(300); }; delay(550); } void driveArdumoto(byte motor, byte dir, byte spd) { if (motor == MOTOR_A) { digitalWrite(DIRA, dir); analogWrite(PWMA, spd); } else if (motor == MOTOR_B) { digitalWrite(DIRB, dir); analogWrite(PWMB, spd); } } void stopArdumoto(byte motor) { driveArdumoto(motor, 0, 0); } void distanceMeasurement () { digitalWrite(Trig, HIGH); delayMicroseconds(10); digitalWrite(Trig, LOW); CZAS = pulseIn(Echo, HIGH); CM = CZAS / 58; } // setupArdumoto initialize all pins void setupArdumoto() { // All pins should be setup as outputs: pinMode(PWMA, OUTPUT); pinMode(PWMB, OUTPUT); pinMode(DIRA, OUTPUT); pinMode(DIRB, OUTPUT); // Initialize all pins as low: digitalWrite(PWMA, LOW); digitalWrite(PWMB, LOW); digitalWrite(DIRA, LOW); digitalWrite(DIRB, LOW); } void setupCzujnik() { Serial.begin(9600); //inicjalizaja monitora szeregowego pinMode(Trig, OUTPUT); //ustawienie pinu 2 w Arduino jako wyjście pinMode(Echo, INPUT); //ustawienie pinu 3 w Arduino jako wejście } void setupDiody() { pinMode(9, OUTPUT); }