Skocz do zawartości

Przeszukaj forum

Pokazywanie wyników dla tagów 'lissajous'.

  • Szukaj wg tagów

    Wpisz tagi, oddzielając przecinkami.
  • Szukaj wg autora

Typ zawartości


Kategorie forum

  • Elektronika i programowanie
    • Elektronika
    • Arduino i ESP
    • Mikrokontrolery
    • Raspberry Pi
    • Inne komputery jednopłytkowe
    • Układy programowalne
    • Programowanie
    • Zasilanie
  • Artykuły, projekty, DIY
    • Artykuły redakcji (blog)
    • Artykuły użytkowników
    • Projekty - DIY
    • Projekty - DIY roboty
    • Projekty - DIY (mini)
    • Projekty - DIY (początkujący)
    • Projekty - DIY w budowie (worklogi)
    • Wiadomości
  • Pozostałe
    • Oprogramowanie CAD
    • Druk 3D
    • Napędy
    • Mechanika
    • Zawody/Konkursy/Wydarzenia
    • Sprzedam/Kupię/Zamienię/Praca
    • Inne
  • Ogólne
    • Ogłoszenia organizacyjne
    • Dyskusje o FORBOT.pl
    • Na luzie

Kategorie

  • Quizy o elektronice
  • Quizy do kursu elektroniki I
  • Quizy do kursu elektroniki II
  • Quizy do kursów Arduino
  • Quizy do kursu STM32L4
  • Quizy do pozostałych kursów

Szukaj wyników w...

Znajdź wyniki, które zawierają...


Data utworzenia

  • Rozpocznij

    Koniec


Ostatnia aktualizacja

  • Rozpocznij

    Koniec


Filtruj po ilości...

Data dołączenia

  • Rozpocznij

    Koniec


Grupa


Imię


Strona

Znaleziono 1 wynik

  1. Witam Próbuję zaimplementować algorytm podążania robota mobilnego 2-kołowego po trajektorii zadanej z góry wzorem na podstawie znalezionych materiałów. materiały: https://www.researchgate.net/publication/225543929_Control_of_Wheeled_Mobile_Robots_An_Experimental_Overview (strona 16 - Nonlinear Control Design) https://kcir.pwr.edu.pl/~mucha/Pracki/Rafal_Cyminski_praca_magisterska.pdf (strona 38) Problem polega na tym, że robot albo jedzie tylko część trajektorii albo ruch wynikowy nie przypomina ani trochę trajektorii wynikowej. void lissajousInit(lissajousTypedef *liss,trajectoryTypedef *traj, sampsonTypedef *samp) { traj->ts = 0.01F; robotMove1.ts = traj->ts; liss->amplitudeX_f32 = 0.5F; liss->amplitudeY_f32 = 0.6F; liss->paramA_u8 = 1.0F; liss->paramB_u8 = 2.0F; liss->paramDelta_f32 = M_PI/2; samp->Kp_f32 = 0.526; samp->Kd_f32 = 0.00263; traj->translationDesigned_f32 = integrateTrapz(0, M_TWOPI, 1000.0F, lissajousTrajectoryVelocitySpecial); } void lissajousTrajectoryGenarate(lissajousTypedef *liss,trajectoryTypedef *traj, sampsonTypedef *samp, robotMoveTypedef *robMov) { if(robMov->translationMeters <= traj->translationDesigned_f32) { traj->time_f32 += traj->ts;//M_PI_DIV; traj->trajRunning_u8 = 1; lissajousTrajectoryPath(traj, liss); calcAngleRadians(traj); trajectoryVelocity(traj); trajectoryAcceleration(traj); samp->angleNow_f32 = robMov->robotAngleDiff; samp->angleErr_f32 = (traj->angleRadiansDiff_f32- robMov->robotAngleDiff); samp->xErr_f32 = (traj->px_f32 - robMov->x); samp->yErr_f32 = (traj->py_f32 - robMov->y); samp->progrVelDesigned_f32 = traj->vxy_f32; samp->angVelDesigned_f32 = ((traj->ay_f32*traj->vx_f32) - (traj->ax_f32*traj->vy_f32))/traj->vxy_f32; sampsonAlgorithm(samp); } else { traj->trajRunning_u8 = 0; //robMov->translationMeters = traj->translationDesigned_f32; } } void lissajousTrajectoryPath(trajectoryTypedef *traj, lissajousTypedef *liss) { traj->pxPrev_f32 = traj->px_f32; traj->pyPrev_f32 = traj->py_f32; traj->px_f32 = liss->amplitudeX_f32*arm_sin_f32(liss->paramA_u8*traj->time_f32 + liss->paramDelta_f32); traj->py_f32 = liss->amplitudeY_f32*arm_sin_f32(liss->paramB_u8*traj->time_f32); } void trajectoryVelocity(trajectoryTypedef *traj) { if(lisTraj.amplitudeX_f32 > 0) { traj->vx_f32 = lisTraj.amplitudeX_f32*lisTraj.paramA_u8*cosf(lisTraj.paramA_u8*traj->ts + lisTraj.paramDelta_f32); traj->vy_f32 = lisTraj.amplitudeY_f32*lisTraj.paramB_u8*cosf(lisTraj.paramB_u8*traj->ts); } else { //for anothor purpose traj->vxPrev_f32 = traj->vx_f32; traj->vyPrev_f32 = traj->vy_f32; traj->vx_f32 = (traj->pxPrev_f32 - traj->px_f32)/traj->ts; traj->vy_f32 = (traj->pyPrev_f32 - traj->py_f32)/traj->ts; if(traj->velocityDesigned_f32) { if(traj->vx_f32 != 0) traj->vx_f32 = (traj->velocityDesigned_f32/traj->vx_f32)*traj->vx_f32; if(traj->vy_f32 != 0) traj->vy_f32 = (traj->velocityDesigned_f32/traj->vy_f32)*traj->vy_f32; } } float32_t sumOfSqares = (traj->vx_f32*traj->vx_f32) + (traj->vy_f32*traj->vy_f32); traj->vxy_f32 = sqrtf(sumOfSqares); } void trajectoryAcceleration(trajectoryTypedef *traj) { if(lisTraj.amplitudeX_f32 > 0) { traj->ax_f32 = -lisTraj.amplitudeX_f32*lisTraj.paramA_u8*lisTraj.paramA_u8*sinf(lisTraj.paramA_u8*traj->ts + lisTraj.paramDelta_f32); traj->ay_f32 = -lisTraj.amplitudeY_f32*lisTraj.paramB_u8*lisTraj.paramB_u8*sinf(lisTraj.paramB_u8*traj->ts); } else { //for anothor purpose traj->ax_f32 = (traj->vxPrev_f32 - traj->vx_f32)/traj->ts; traj->ay_f32 = (traj->vyPrev_f32 - traj->vy_f32)/traj->ts; } float32_t sumOfSqares = (traj->ax_f32*traj->ax_f32) + (traj->ay_f32*traj->ay_f32); traj->axy_f32 = sqrtf(sumOfSqares); } void calcAngleRadians(trajectoryTypedef *traj) { traj->angleRadiansPrev_f32 = traj->angleRadians_f32; traj->angleRadians_f32 = atan2f(traj->py_f32, traj->px_f32); traj->angleRadiansDiff_f32 = traj->angleRadians_f32 - traj->angleRadiansPrev_f32; } float deg2rad(float num) { return num * (M_PI / M_PI_DEG); } float rad2deg(float num) { return num * (M_PI_DEG / M_PI); } static float32_t lissajousTrajectoryVelocitySpecial(float32_t mytime) { static float32_t vxySpecial; trajectory1.vx_f32 = lisTraj.amplitudeX_f32*lisTraj.paramA_u8*cosf((lisTraj.paramA_u8*mytime) + lisTraj.paramDelta_f32); trajectory1.vy_f32 = lisTraj.amplitudeY_f32*lisTraj.paramB_u8*cosf(lisTraj.paramB_u8*mytime); float32_t sumOfSqares = (trajectory1.vx_f32*trajectory1.vx_f32) + (trajectory1.vy_f32*trajectory1.vy_f32); vxySpecial = sqrtf(sumOfSqares); return vxySpecial; } float32_t integrateTrapz(float32_t xp, float32_t xk, uint16_t N, float32_t(*f)(float32_t)) { float32_t dx = 0, s = 0; s = 0; dx = (xk - xp) / N; for (int i = 1; i < N; i++) { s += (*f)(xp + i * dx); } s = (s + ((*f)(xp) + (*f)(xk)) / 2) * dx; return s; } Algorytm omawiany w wyżej wymienionych publikacjach: void sampsonAlgorithm(sampsonTypedef *alg) { alg->xTrackErr_f32 = cosf(alg->angleNow_f32)*alg->xErr_f32 + sinf(alg->angleNow_f32)*alg->yErr_f32; alg->yTrackErr_f32 = -sinf(alg->angleNow_f32)*alg->xErr_f32 + cosf(alg->angleNow_f32)*alg->yErr_f32; if(alg->angleErr_f32 == 0) alg->angleErr_f32 = FLT_MIN; //very small number - 0 casued NaN (Not a Number) alg->progrVelRef_f32 = alg->Kp_f32*alg->xTrackErr_f32 + alg->progrVelDesigned_f32*cosf(alg->angleErr_f32); alg->angVelRef_f32 = alg->angVelDesigned_f32 + (alg->Kd_f32*alg->angleErr_f32) + (alg->progrVelDesigned_f32*alg->yTrackErr_f32*(sinf(alg->angleErr_f32)/alg->angleErr_f32)); alg->velLeftWheel_f32 = alg->progrVelRef_f32 - (ROBOT_LENGHT/2)*alg->angVelRef_f32; alg->velRightWheel_f32 = alg->progrVelRef_f32 + (ROBOT_LENGHT/2)*alg->angVelRef_f32; alg->velLeftWheelTicks_f32 = (convertMetersToTicks(alg->velLeftWheel_f32)/100.0F); alg->velRightWheelTicks_f32 = (convertMetersToTicks(alg->velRightWheel_f32)/100.0F); } Obliczanie pozycji robota: void calculateRobotKinematics(encoderTypedef *encLeft, encoderTypedef *encRight, robotMoveTypedef *robMov) { robMov->translationMeters = (encLeft->distanceMeters_f32 + encRight->distanceMeters_f32)/2; robMov->translationMetersDiff = (encLeft->distanceMetersDiff_f32 + encRight->distanceMetersDiff_f32)/2; MPU6050_Read_Gyro(&hi2c3, &MPU6050); if(fabsf(MPU6050.Gz) < 0.1F) //high-pass filter [deg] { MPU6050.Gz = 0.0F; } robMov->rotationRadians += deg2rad(MPU6050.Gz)*robMov->ts; // (encRight->distanceMeters - encLeft->distanceMeters)/ROBOT_LENGHT; robMov->rotationRadiansDiff = robMov->rotationRadians - robMov->rotationRadiansPrev; //może *ts ??? robMov->robotAngleDiff = robMov->robotAngle - robMov->robotAnglePrev; //może *ts ??? robMov->x = robMov->translationMeters*cosf(robMov->rotationRadians); robMov->y = robMov->translationMeters*sinf(robMov->rotationRadians); robMov->robotAngle = atan2f(robMov->y, robMov->x); robMov->rotationRadiansPrev = robMov->rotationRadians; robMov->robotAnglePrev = robMov->robotAngle; } Wywołanie całości (co 10ms --> f = 100Hz): if(START_PILOT_VIENNA || START_PILOT_JSUMO) { lissajousTrajectoryGenarate(&lisTraj,&trajectory1, &sampsonData, &robotMove1); PIDwheelVelocity(&pidLeftWheel, &encoderLeft, sampsonData.velLeftWheelTicks_f32); PIDwheelVelocity(&pidRightWheel, &encoderRight, sampsonData.velRightWheelTicks_f32); if(trajectory1.trajRunning_u8) { MotorSpeed(pidLeftWheel.PWMout, pidRightWheel.PWMout); } else { MotorSpeed(0,0); } } else { MotorSpeed(0,0); }
×
×
  • 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.