Skocz do zawartości

[C] PILNE Sterowanie robotem przy użyciu magnetometru


gogu

Pomocna odpowiedź

Witam!

Mam bardzo duży problem. Do jutra do 9 muszę napisać nową 'koncepcje' sterowania robotem przy użycia magnetometru (robot ma utrzymywać początkowy kierunek - wrócić do wartości początkowej po obrocie i jechać prosto przez 5 metrów). Kompas to magnetometr 3osiowy HMC5883L, uC to Arduino Uno, mostek h to L293D.

Wystarczy sama zasada, które idealnie rozwiązywałaby ten problem. Na realizacje kodu i dostosowanie sprzętu będę miał miesiąc (jeśli uda mi się jutro)

Układ schematu przedstawia się nastepująco

Mój kod, który niby jest totalnie zly i niedorzeczny wygląda tak:

#include <Wire.h>
#include <HMC5883L.h>
HMC5883L compass;
unsigned long time;
int motor1 = 8; //silnik 1szy wejscie 1
int motor12 = 12; //silnik 1szy wejscie 2
int motor2 = 4; //silnik 2gi wejscie 1
int motor22 = 7; // silnik 2gi wejscie 2
int pwm1 = 11; // //PWM silnika
int pwm2 = 10; // PWM SILNIKA 
int previousDegree;
boolean readed = false; // Zmienna pomocnicza, służaca do zapisania 1szej wartosci kompasu
float firstReading; // 1sza zapisana wartość kompasu
float bmax; //blad maksymalny
float bmin; // blad minmalny
void setup()
{
 pinMode(motor1, OUTPUT);
pinMode(motor12, OUTPUT);
pinMode(motor2, OUTPUT);
pinMode(motor22, OUTPUT);
pinMode(pwm1, OUTPUT);
pinMode(pwm2, OUTPUT);  
 Serial.begin(9600);
 // Inicjalizacja HMC5883L
 Serial.println("Initialize HMC5883L");
 while (!compass.begin())
 {
   Serial.println("Nie odnaleziono HMC5883L, sprawdz polaczenie!");
   delay(500);
 }
 // Ustawienie zakresu pomiarowego
 compass.setRange(HMC5883L_RANGE_8_1GA);
 // Ustawienie trybu pracy
 compass.setMeasurementMode(HMC5883L_CONTINOUS); 
 // Ustawienie czestotliwosci pomiarow
 compass.setDataRate(HMC5883L_DATARATE_75HZ);
 // Liczba usrednionych probek
 compass.setSamples(HMC5883L_SAMPLES_8);
}
// Funkcja służaca do zapisania 1szego odczytu kompasu
void firstRead(float a){
 if(!readed){ 
   firstReading = a; 
   readed = true;
 }
}
void prosto(){ 
    digitalWrite(motor2, LOW);
    digitalWrite(motor22, HIGH);
    digitalWrite(motor1, HIGH);
    digitalWrite(motor12, LOW);
    analogWrite(pwm1, 127);
    analogWrite(pwm2, 128);  
}
void loop()
{
 time = millis();
 // Pobranie wektorów znormalizowanych
 Vector norm = compass.readNormalize();
 // Obliczenie kierunku (rad)
 float heading = atan2(norm.YAxis, norm.XAxis);
 // Ustawienie kata deklinacji dla Bytomia 4'26E (positive)
 // Formula: (deg + (min / 60.0)) / (180 / M_PI);
 float declinationAngle = (4.0 + (30.0 / 60.0)) / (180 / M_PI);
 heading += declinationAngle;
 // Korekta katow
 if (heading < 0)
 {
   heading += 2 * PI;
 }
 if (heading > 2 * PI)
 {
   heading -= 2 * PI;
 }
 // Zamiana radianow na stopnie
 float headingDegrees = heading * 180/M_PI;
 // Poprawka nierownomiernosci pomiarow HMC5883L
 float fixedHeadingDegrees;
 if (headingDegrees >= 1 && headingDegrees < 240)
 {
   fixedHeadingDegrees = map(headingDegrees, 0, 239, 0, 179);
 } else
 if (headingDegrees >= 240)
 {
   fixedHeadingDegrees = map(headingDegrees, 240, 360, 180, 360);
 }
 // Reakcja na zmiane  +/- 1 deg
 int smoothHeadingDegrees = round(fixedHeadingDegrees);
 if (smoothHeadingDegrees < (previousDegree + 1) && smoothHeadingDegrees > (previousDegree - 1))
 {
   smoothHeadingDegrees = previousDegree;
 }
previousDegree = smoothHeadingDegrees;
firstRead(smoothHeadingDegrees);
bmax = firstReading+1;
bmin = firstReading-1;

 // Wyniki
 //Serial.print(" Heading = ");
 //Serial.print(heading);
 //Serial.print(" Degress = ");
 //Serial.print(headingDegrees);
 //Serial.print(" Fixed Degress = ");
 //Serial.print(fixedHeadingDegrees);
// Serial.print(" Smooth Degress = ");
 Serial.print(smoothHeadingDegrees);  
 Serial.print(" ; ");
 Serial.print(time);  
 Serial.println();

 if((smoothHeadingDegrees > bmin) & (smoothHeadingDegrees < bmax)) {
    prosto();
  }
  if(smoothHeadingDegrees > bmax+1)){
    digitalWrite(motor2, LOW);
    digitalWrite(motor22, LOW);
    digitalWrite(motor1, HIGH);
    digitalWrite(motor12, LOW);
    analogWrite(pwm1, 96);
    analogWrite(pwm2, 96);
  }
  if(smoothHeadingDegrees < bmin-1){ 
    digitalWrite(motor2, LOW);
    digitalWrite(motor22, HIGH);
    digitalWrite(motor1, LOW);
    digitalWrite(motor12, LOW);
    analogWrite(pwm1, 96);
    analogWrite(pwm2, 96);
  }
}

W tym sposobie po prostu porównuje wartość bieżacą (smoothHeadingDregrees) z wartoscia zapisana na poczatku (fristReading) i na tej zasadzie steruję odpowiednią pracą silnikow poprzez mostek H.

Myslałem, żeby obliczać kąt odchylenia (wartość bieżaca - wartość pierwsza) i na podstawie tej wartosci obracać silnik np. o 40 stopni w lewo. Wydaje mi się to lepszym rozwiązaniem, ale nie mam pomysłu jak obrocic silnik akurat o 40 stopni.

Kolejnym problemem jest czas. Funkcją milis() mierze milisekundy wykonania każdego cyklu głownej pętli. Czas ten równy jest 92-96ms. Musze dokładnie wiedzieć, dlaczego ten czas jest tak wysoki. Czy winna jest tu tramsisja danych z kompasu do arduino?

Pozdrawiam!

Link do komentarza
Share on other sites

Dołącz do dyskusji, napisz odpowiedź!

Jeśli masz już konto to zaloguj się teraz, aby opublikować wiadomość jako Ty. Możesz też napisać teraz i zarejestrować się później.
Uwaga: wgrywanie zdjęć i załączników dostępne jest po zalogowaniu!

Anonim
Dołącz do dyskusji! Kliknij i zacznij pisać...

×   Wklejony jako tekst z formatowaniem.   Przywróć formatowanie

  Dozwolonych jest tylko 75 emoji.

×   Twój link będzie automatycznie osadzony.   Wyświetlać jako link

×   Twoja poprzednia zawartość została przywrócona.   Wyczyść edytor

×   Nie możesz wkleić zdjęć bezpośrednio. Prześlij lub wstaw obrazy z adresu URL.

×
×
  • Utwórz nowe...

Ważne informacje

Ta strona używa ciasteczek (cookies), dzięki którym może działać lepiej. Więcej na ten temat znajdziesz w Polityce Prywatności.