Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #include <SPI.h>
- #include <SD.h>
- #include <AFMotor.h>
- //#include <NewPing.h>
- #include "SR04.h"
- #include "TimerOne.h"
- #include <IRremote.h>
- //Ustawienia karty SD
- File myFile;
- const int8_t DISABLE_CHIP_SELECT = 10;
- int pinCS = 53; // Pin komunikacji z karta SD
- //Ustawienie timera
- unsigned long startTime;
- unsigned long followWallTime;
- unsigned long followWall_frontTime;
- //Ustawienie predkosciomierza
- const byte motor_A = 20; //Motor1 pin przerwania
- const byte motor_B = 21; //Motor2 pin przerwania
- volatile int counter_A = 0; //zmienne do licznika pulsow
- volatile int counter_B = 0;
- float diskslots = 20.00; // liczba otworow w dysku enkoderow
- const float wheeldiameter = 65.10; // srednica kola w milimetrach
- //Ustawienie sensorow
- int LEFT_SENSOR_TRIG = A2;
- int LEFT_SENSOR_ECHO = A3;
- int FRONT_SENSOR_TRIG = A0;
- int FRONT_SENSOR_ECHO = A1;
- SR04 sr04_left = SR04(LEFT_SENSOR_ECHO, LEFT_SENSOR_TRIG);
- SR04 sr04_front = SR04(FRONT_SENSOR_ECHO, FRONT_SENSOR_TRIG);
- //Ustawienie pilota
- int RECV_PIN = 22;
- IRrecv irrecv(RECV_PIN);
- decode_results results;
- //Ustawienie silnikow
- AF_DCMotor motor1(1, MOTOR12_1KHZ);
- AF_DCMotor motor2(3, MOTOR34_1KHZ);
- int NO_OBSTACLE = 0;
- int OBSTACLE = 1;
- //Zmienne PID
- const float Kp_wall = 16; // stala uzyta do skretu
- const float Ki_wall = 0.003; // stala calki uzyta do poprawy bledu
- const float Kd_wall = 11; // stala rozniczki uzyta do poprawy predkosci poprawy bledu
- const float Kp_front = 30; // stala uzyta do skretu
- const float Ki_front = 0; // stala calki uzyta do poprawy bledu
- const float Kd_front = 15; // stala rozniczki uzyta do poprawy predkosci poprawy bledu
- const int offset = 12; // docelowa odleglosc
- const int max_front_distance = 16; // maksymalna odleglosc z przodu
- int Tp = 128; // docelowa predkosc - target power
- float error_wall = 0;
- float lastError_wall = 0;
- int correction_wall = 0;
- float p_wall = 0; //proporcja
- float i_wall = 0; //calka
- float d_wall = 0; //rozniczka
- int Wall = 0;
- int Front = 0;
- float error_front = 0;
- float lastError_front = 0;
- float correction_front = 0;
- float p_front = 0; //proporcja
- float i_front = 0; //calka
- float d_front = 0; //rozniczka
- int leftMotorSpeed = 0;
- int rightMotorSpeed = 0;
- int front_status = 0;
- //int rangeFrontObstacle = 0;
- //int rangeWallObstacle = 0;
- //int rangeFront = 0;
- //int rangeWall = 0;
- //int speedSet = 0;
- //0 - true; 1 - false
- int rightTurnBegin = 0;
- int leftTurnBegin = 0;
- int straightLineBegin = 0;
- int speedSet = 0;
- void setup() {
- // Ustawienie pilota
- pinMode(RECV_PIN, INPUT);
- irrecv.enableIRIn();
- Serial.begin(9600);
- //Ustawienie czytnika kart SD
- pinMode(53, OUTPUT);
- delay(5000);
- //Ustawienie timera
- Timer1.initialize(1000000); // ustawienie timera na sekunde
- //zwieksz counter1 gdy pin predkosciomieza jest na Hig
- attachInterrupt(digitalPinToInterrupt (motor_A), ISR_count_A, RISING);
- //zwieksz counter2 gdy pin predkosciomieza jest na High
- attachInterrupt(digitalPinToInterrupt (motor_B), ISR_count_B, RISING);
- Timer1.attachInterrupt(ISR_timerone);
- if (SD.begin(SS))
- {
- Serial.println("SD card is ready to use.");
- } else
- {
- Serial.println("SD card initialization failed");
- return;
- }
- myFile = SD.open("DaneMapy.txt", FILE_WRITE);
- if(myFile){
- myFile.print("Lewy silnik cm/s, Prawy silnik cm/s");
- }
- myFile.close();
- }
- void loop() {
- // put your main code here, to run repeatedly:
- beginning:
- if (irrecv.decode(&results))
- {
- switch (results.value)
- {
- case 0xFF22DD: //vol-
- Serial.print("falling back");
- fall_back();
- break;
- case 0xFF02FD: //vol+
- drive_forward();
- break;
- case 0xFFC23D: //play/pause
- motor_stop();
- break;
- }
- irrecv.resume();
- }
- //sprawdzam czy jest przeszkoda z przodu
- front_status = GET_FRONT_STATUS();
- if (front_status == OBSTACLE) {
- if (leftTurnBegin == 0) {
- zero_wall();
- }
- followWall_front();
- // Zmieniam flage skretu na false do nastepnego czasu az natrafi na przeszkode
- rightTurnBegin = 1;
- straightLineBegin = 0;
- goto beginning;
- }
- followWall();
- }
- void followWall() {
- //Kiedy robot zaczyna jechac prosto (dopoki nie ma przeszkody) zmieniam flage na true
- rightTurnBegin = 0;
- Wall = sr04_left.Distance();
- error_wall = Wall - offset;
- i_wall = (i_wall + error_wall);
- d_wall = (error_wall - lastError_wall);
- correction_wall = Kp_wall * error_wall + Ki_wall * i_wall + Kd_wall * d_wall;
- //jezeli error_wall jest mniejszy od 10, oznacza to ze robot jedzie obok sciany poprawnie
- if (error_wall < 10) {
- if (straightLineBegin == 0) {
- zero_wall();
- }
- if (correction_wall > 127 && correction_wall > 0) {
- correction_wall = 127;
- }
- if (correction_wall < -127 && correction_wall < 0) {
- correction_wall = -127;
- }
- rightMotorSpeed = Tp + correction_wall;
- leftMotorSpeed = Tp - correction_wall;
- leftTurnBegin = 0;
- straightLineBegin = 1;
- } else {
- //jezeli error_wall jest wiekszy od 10, oznacza to ze robot musi skrecic w prawo
- //zeruje wartosci na poczatku skretu w lewo
- if (leftTurnBegin == 0) {
- zero_wall();
- zero_front();
- }
- //PID do skretu w lewo
- int speed = 2.5 * error_wall + 8 * d_wall;
- if (speed > 127 && speed > 0) {
- speed = 127;
- }
- if (speed < -127 && speed < 0) {
- speed = -127;
- }
- rightMotorSpeed = Tp + (speed);
- leftMotorSpeed = Tp - (speed);
- leftTurnBegin = 1;
- straightLineBegin = 0;
- }
- motor1.setSpeed(rightMotorSpeed);
- motor1.run(FORWARD);
- motor2.setSpeed(leftMotorSpeed);
- motor2.run(FORWARD);
- lastError_wall = error_wall;
- zero_front();
- }
- void zero_wall(void) {
- i_wall = 0;
- d_wall = 0;
- lastError_wall = 0;
- }
- void zero_front(void) {
- i_front = 0;
- d_front = 0;
- lastError_front = 0;
- }
- void followWall_front()
- {
- //tu umiec zczytywanie z sensora
- Front = sr04_front.Distance();
- error_front = (Front - max_front_distance);
- i_front = (i_front + error_front);
- d_front = (error_front - lastError_front);
- correction_front = Kp_front * error_front + Ki_front * i_front + Kd_front * d_front;
- if (correction_front > 160 && correction_front > 0) {
- correction_front = 160;
- }
- if (correction_front < -160 && correction_front < 0) {
- correction_front = -160;
- }
- rightMotorSpeed = 95 + correction_front;
- leftMotorSpeed = 95 - correction_front;
- motor1.setSpeed(rightMotorSpeed);
- motor1.run(FORWARD);
- motor2.setSpeed(leftMotorSpeed);
- motor2.run(FORWARD);
- lastError_front = error_front;
- }
- void drive_forward() {
- motor1.setSpeed(128);
- motor2.setSpeed(128);
- motor1.run(FORWARD);
- motor2.run(FORWARD);
- delay(400);
- }
- //zatrzymuje robota na 5 sekund
- void motor_stop() {
- motor1.run(RELEASE);
- motor2.run(RELEASE);
- delay(5000);
- }
- //wycofanie gdy robot wjedzie w przeszkode
- void fall_back() {
- motor1.setSpeed(128);
- motor2.setSpeed(128);
- motor1.run(BACKWARD);
- motor2.run(BACKWARD);
- delay(500);
- motor1.run(BACKWARD);
- motor2.run(FORWARD);
- delay(400);
- Serial.print("falling back");
- Serial.println();
- }
- //funkcja konwertujaca centymetry na kroki
- int RMPtoCmS(float RPM){
- int result;
- float radius = (wheeldiameter / 2);
- float metersPerSecond = (radius * 0.001) * RPM * 0.10472; //obliczenie predkosci m/s
- float cmPerSec = metersPerSecond * 100; //przeliczanie m/s na cm/s
- float f_result = cmPerSec;
- result = (int) f_result; //rzutowanie wyniku float na int, NIE ZAOKRAGLONE
- return result;
- }
- //obsluga przerwan (Interupt Service Routines ISR)
- // zliczanie pulsow motor1
- void ISR_count_A(){
- counter_A++;
- }
- //zliczanie pulsow motor2
- void ISR_count_B(){
- counter_B++;
- }
- //TimerOne ISR - timer przerwania
- void ISR_timerone(){
- Timer1.detachInterrupt(); //Stop timera
- Serial.print("Motor1 Speed: ");
- float rotation_A = (counter_A / diskslots) * 60.00; //obliczenie obrotow na minute (RPM) dla motor1
- Serial.print(RMPtoCmS(rotation_A));
- Serial.print(" Cm/s - ");
- counter_A = 0;
- Serial.print("Motor2 Speed: ");
- float rotation_B = (counter_B / diskslots) * 60.00; //obliczenie obrotow na minute (RPM) dla motor2
- Serial.print(RMPtoCmS(rotation_B));
- Serial.println(" Cm/s");
- counter_B = 0;
- float srednia_predkosc = (RMPtoCmS(rotation_A) + RMPtoCmS(rotation_B)) /2;
- myFile = SD.open("DaneMapy.txt", FILE_WRITE);
- if (myFile) {
- Serial.println("Writing to file...");
- myFile.print("Motor1: ");
- myFile.print(RMPtoCmS(rotation_A));
- myFile.print("Motor2: ");
- myFile.print(RMPtoCmS(rotation_B));
- myFile.print(" Średnia prędkość: ");
- myFile.print(srednia_predkosc);
- myFile.close();
- // delay(500);
- Serial.println("Done.");
- }
- Timer1.attachInterrupt(ISR_timerone);
- }
- int GET_LEFT_STATUS(void) {
- if (sr04_left.Distance() < offset)
- return OBSTACLE;
- else
- return NO_OBSTACLE;
- }
- int GET_FRONT_STATUS(void) {
- if (sr04_front.Distance() < max_front_distance)
- return OBSTACLE;
- else
- return NO_OBSTACLE;
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement