Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #include <Servo.h>
- Servo servo;
- #define SERVO_PIN 11
- #define LED 13
- #define BUZZER 10
- #define PRZYCISK 2
- #define L_DIR 4
- #define L_VEL 5
- #define R_DIR 9
- #define R_VEL 6
- #define TRIG_PIN 7
- #define ECHO_PIN 8
- #define LEFT 0
- #define RIGHT 1
- void setup() {
- pinMode(LED, OUTPUT);
- pinMode(BUZZER, OUTPUT);
- pinMode(PRZYCISK, INPUT_PULLUP);
- digitalWrite(LED, LOW);
- digitalWrite(BUZZER, LOW);
- pinMode(L_DIR, OUTPUT);
- pinMode(L_VEL, OUTPUT);
- pinMode(R_DIR, OUTPUT);
- pinMode(R_VEL, OUTPUT);
- pinMode(TRIG_PIN, OUTPUT);
- pinMode(ECHO_PIN, INPUT);
- servo.attach(SERVO_PIN);
- digitalWrite(L_DIR, 0);
- analogWrite(L_VEL, 0);
- digitalWrite(R_DIR, 0);
- analogWrite(R_VEL, 0);
- digitalWrite(TRIG_PIN, LOW);
- delay(100);
- servo.write(90);
- delay(500);
- }
- static void Forward(void){ // poruszenie się do przodu o jedno pole
- digitalWrite(L_DIR, 0);
- analogWrite(L_VEL, 100);
- digitalWrite(R_DIR, 0);
- analogWrite(R_VEL, 100);
- delay(500); // czas w którym robot pokona jedno pole ( do wyznaczenia)
- analogWrite(L_VEL, 0);
- analogWrite(R_VEL, 0);
- delay(500);
- }
- int Distance(){ // pomiar odległości
- int czas, distance;
- digitalWrite(TRIG_PIN, LOW);
- delayMicroseconds(2);
- digitalWrite(TRIG_PIN, HIGH);
- delayMicroseconds(10);
- digitalWrite(TRIG_PIN, LOW);
- czas = pulseIn(ECHO_PIN, HIGH, 10000);
- if(czas == 0)
- czas = 10000;
- distance = czas / 58;
- return distance;
- }
- void Turnleft (void){ // Skręcanie w lewo (do tyłu dla lepszego pola manewru
- digitalWrite(L_DIR, 1);
- analogWrite(L_VEL, 255-200);
- delay(300);
- digitalWrite(L_DIR, 0);
- analogWrite(L_VEL, 0);
- }
- void Turnright (void){
- digitalWrite(R_DIR, 1);
- analogWrite(R_VEL, 25-200);
- delay(300);
- digitalWrite(R_DIR, 0);
- analogWrite(R_VEL, 0);
- }
- bool Checkright(){ // obrót serva w prawo i zczytanie odległości
- servo.write(0);
- delay(500);
- bool ret = (Distance() > 30);
- servo.write(90);
- delay(400);
- return ret;
- }
- bool Checkleft(){
- servo.write(180);
- delay(500);
- bool ret = (Distance() > 30);
- servo.write(90);
- delay(400);
- return ret;
- }
- bool Checkforward() {
- servo.write(90);
- return (Distance() > 30);
- }
- void Back(int i, int j, int *For, int *Tur){ // tryb powrotu
- int dire, times_for;
- Turnright(); Turnright();
- for( ; i>0 ; i--){
- times_for = For[i];
- for( ; times_for > 0; times_for--){
- Forward();
- }
- dire = Tur[j];
- if(dire == LEFT) Turnright();
- else Turnleft();
- j--;
- }
- }
- void loop() {
- static int For[200], Tur[50]; // tablice 1: ile kroków do przodu, 2: w którą strone skręcił
- static int times_for = 0, i = 0, j = 0;
- Tur[0] = 0;
- if(Checkforward()){
- times_for++;
- Forward();
- }
- else {
- For[i] = times_for;
- times_for = 0;
- i++;
- if(Tur[j] == LEFT){
- if(Checkright()) { Tur[j] = RIGHT; j++; Turnright();
- }
- else {
- if(Checkleft()){ Tur[j] = LEFT; j++; Turnleft();
- }
- else{ Back(i, j, For[0], Tur[0]);
- }
- }
- }
- else{
- if(Checkleft()){ Tur[j] = LEFT; j++; Turnleft();
- }
- else {
- if(Checkright()) { Tur[j] = RIGHT; j++; Turnright();
- }
- else{ Back(i, j, For[0], Tur[0]);
- }
- }
- }
- }
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement