[C] PILNE Sterowanie robotem przy użyciu magnetometru

[gogu]

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!