QRectF Plansza::przemiesc(Czolg& czolg, double predkosc_lewa, double predkosc_prawa, double czas) {
QRectF wynik;
if(predkosc_lewa != 0 || predkosc_prawa != 0) {
QVector2D kierunek(cos(czolg.kierunek), -sin(czolg.kierunek));
QPointF przemieszczenie;
double kat = 0.0;
if(predkosc_lewa == predkosc_prawa) {
przemieszczenie = (kierunek * predkosc_prawa * czolg.predkosc_maksymalna * czas).toPointF();
} else if(predkosc_lewa == -predkosc_prawa) {
kat = 2 * predkosc_prawa * czolg.predkosc_maksymalna * czas / this->czolg.rozmiar.width();
wynik = QRectF(czolg.pozycja - QPointF(1, 1), czolg.pozycja + QPointF(1, 1));
} else {
double droga_lewa, droga_prawa, promien_skretu;
droga_lewa = predkosc_lewa * czolg.predkosc_maksymalna * czas;
droga_prawa = predkosc_prawa * czolg.predkosc_maksymalna * czas;
promien_skretu = (2 * czolg.rozmiar.width() * droga_prawa / (droga_prawa - droga_lewa) - czolg.rozmiar.width()) / 2;
kat = (droga_lewa + droga_prawa) / (2 * promien_skretu);
QVector2D przemieszczenie_wzgledne(promien_skretu * cos(kat) - promien_skretu, promien_skretu * sin(kat));
QVector2D przemieszczenie_bezwzgledne_dla_wzglednego_x(przemieszczenie_wzgledne.x() * -kierunek.y(), przemieszczenie_wzgledne.x() * kierunek.x());
QVector2D przemieszczenie_bezwzgledne_dla_wzglednego_y(przemieszczenie_wzgledne.y() * kierunek.x(), przemieszczenie_wzgledne.y() * kierunek.y());
QVector2D przemieszczenie_bezwzgledne = przemieszczenie_bezwzgledne_dla_wzglednego_x + przemieszczenie_bezwzgledne_dla_wzglednego_y;
przemieszczenie = przemieszczenie_bezwzgledne.toPointF();
wynik = QRectF(czolg.pozycja + QPointF(promien_skretu * cos(czolg.kierunek + M_PI_2) - fabs(promien_skretu), promien_skretu * -sin(czolg.kierunek + M_PI_2) - fabs(promien_skretu)), QSizeF(2 * fabs(promien_skretu), 2 * fabs(promien_skretu)));
}
czolg.kierunek = fmod(czolg.kierunek + kat, 2 * M_PI);
/* if(!przemieszczenie.isNull()){
double a, b, d2;
int i = 0;
do{
a = (this->odcinki[i].y1() - this->odcinki->y2()) / (this->odcinki[i].x1() - this->odcinki->x2());
b = (this->odcinki[i].x1() * this->odcinki->y2() - this->odcinki[i].y1() * this->odcinki->x2()) / (this->odcinki[i].x1() - this->odcinki->x2());
d2 = pow(a * (czolg.pozycja.x() + przemieszczenie.x()) - czolg.pozycja.y() - przemieszczenie.y() + b, 2) / (a * a + 1);
}while(2 * d2 > pow(this->czolg.rozmiar.width(), 2) + pow(this->czolg.rozmiar.height(), 2) && ++i < this->ilosc_odcinkow);
qDebug() << d2;
if(i == this->ilosc_odcinkow)*/
czolg.pozycja += przemieszczenie;
// }
}
if(this->rotacja_wiezy != 0)
this->czolg.kierunek_wiezy = fmod(this->czolg.kierunek_wiezy + this->rotacja_wiezy * czas * this->czolg.predkosc_obrotu_wieza, 2 * M_PI);
return wynik;
}
void pozycjonowanie()
{
if(bit_is_clear(PIN_SWITCH, KRANC));
else
{
kierunek(TYL);
int x=1;
while(x)
{
if(bit_is_clear(PIN_SWITCH, KRANC))
{
x=0;
}
}
stop();
}
stop();
}
void wykonaj (int ile_petli, int dlugosc)
{
int licznik=0;
char ch_liczba[3];
pozycjonowanie();
_delay_ms(500);
relay_on();
_delay_ms(50);
init_LCD();
init_PWM();
_delay_ms(200);
int delay = 370;
if(dlugosc>MIN_DLUGOSC)
delay = delay + (dlugosc-MIN_DLUGOSC)*190;
for (licznik=0; licznik<ile_petli; licznik++)
{
int do_konca = ile_petli-licznik;
gen_char(ch_liczba, &do_konca); //wyswietla ile cykli pozostało do konca naswietlania
wyswietl_LCD(ch_liczba);
kierunek(PRZOD);
_delay_ms(delay); //odleglosc dokad dojadą lampy
stop();
_delay_ms(2000);
pozycjonowanie(); //jezdie do tylu az napotka krancowe
_delay_ms(2000);
}
_delay_ms(1000);
wyswietl_LCD("KONIEC PRACY");
_delay_ms(1000);
relay_off();
beep();
}
