Projekte
mifritscher
motorregler
main.c
Projekte
Forum
Doku
Öffentliche Projekte
Startseite
Beispielprogramme
Projekte von anderen
Welcome
Username
Passwort
Eingeloggt bleiben
Zugangsdaten vergessen?
Registrieren
Projektverwaltung
⇨ Please choose! ⇦
——————————————————
✎ Create new project...
★ Browse existing projects...
——————————————————
⚬ MotorTest#1
⚬ C Tutorial 8#1
⚬ NIBO2 C Project#1
⚙ C Tutorial 15#1
⚬ 2010_11_18_el_test001#1
motorregler
main.c
Project details
Compiler settings
Nachrichten
Sie sind nicht eingeloggt.
Neuigkeiten
★
NiboRoboLib 3.6
2017-01-17: Neue Version 3.6
★
NiboRoboLib 3.4.1
2016-04-16: Neue Version 3.4.1
★
Coding Tutorial
2015-11-22: Jetzt auch für den NIBO burger!
Site-Statistic
7426 private projects
385 public projects
16180353 lines compiled
58212 builds
NIBO
@
motorregler/main.c [read only]
//Motor #include <nibobee/delay.h> #include <nibobee/iodefs_nibobee.h> #include <nibobee/led.h> #include <nibobee/motpwm.h> #include <nibobee/odometry.h> #include <nibobee/sens.h> #include <avr/interrupt.h> #include <nibobee/usart.h> #include <nibobee/clock.h> #include <nibobee/analog.h> #include <nibobee/iodefs.h> #include <nibobee/line.h> #include <stdlib.h> #define FACTOR 6 void print_status() { } int16_t factor_left = 10 << FACTOR; int16_t factor_left_backward = 10 << FACTOR; int16_t factor_right = 10 << FACTOR; int16_t factor_right_backward = 10 << FACTOR; int16_t speed_left_old = 0; int16_t speed_right_old = 0; uint16_t zyclus_duration = 100; //speed: odo-ticks pro Sekunde, Duration: in Millisekunden void drive(int16_t speed_left, int16_t speed_right, uint16_t duration, uint8_t boost) { print_status(";Drive; %5d; %5d; %5u", speed_left, speed_right, duration); //Anstossen if (boost) { if (speed_left_old == 0) { motpwm_setLeft(speed_left > 0 ? 800 : -800); } if (speed_right_old == 0) { motpwm_setRight(speed_right > 0 ? 800 : -800); } delay(50); } //Wert holen speed_left_old = speed_left; speed_right_old = speed_right; odometry_getLeft(1); odometry_getRight(1); //eigentliche Regelschleife int16_t target_ticks_left = speed_left / (1000 / zyclus_duration); int16_t target_ticks_right = speed_right / (1000 / zyclus_duration); int16_t duration_ticks = duration / zyclus_duration; int16_t factor_left_intern = factor_left; int16_t factor_right_intern = factor_right; int16_t error_left[3] = {0,0,0}; int16_t error_right[3] = {0,0,0}; while (duration_ticks > 0) { //Wert setzen int32_t motpwm_left_value_temp = ((int32_t)speed_left) * factor_left_intern; int16_t motpwm_left_value = motpwm_left_value_temp >> FACTOR; if (motpwm_left_value > 1023) { motpwm_left_value = 1023; } if (motpwm_left_value < -1023) { motpwm_left_value = -1023; } motpwm_setLeft(motpwm_left_value); int32_t motpwm_right_value_temp = ((int32_t)speed_right) * factor_right_intern; int16_t motpwm_right_value = motpwm_right_value_temp >> FACTOR; if (motpwm_right_value > 1023) { motpwm_right_value = 1023; } if (motpwm_right_value < -1023) { motpwm_right_value = -1023; } motpwm_setRight(motpwm_right_value); duration_ticks--; delay(zyclus_duration); int16_t actual_ticks_left = odometry_getLeft(1); if (actual_ticks_left != 0) { int32_t t = (target_ticks_left << FACTOR); int32_t correction_factor = t / actual_ticks_left; if (correction_factor > (int16_t)(1.2 * (1 << FACTOR))) { correction_factor = (int16_t)(1.2 * (1 << FACTOR)); } if (correction_factor < (int16_t)(0.8 * (1 << FACTOR))) { correction_factor = (int16_t)(0.8 * (1 << FACTOR)); } t = factor_left_intern * correction_factor; factor_left_intern = t >> FACTOR; if (factor_left_intern < 3 << FACTOR) { factor_left_intern = 3 << FACTOR; } } else { factor_left_intern = factor_left_intern + (factor_left_intern >> 1) ; } if (factor_left_intern > 50 << FACTOR) { factor_left_intern = 50 << FACTOR; } if (factor_left_intern <= 0) { factor_left_intern = 3 << FACTOR; } int16_t actual_ticks_right = odometry_getRight(1); if (actual_ticks_right != 0) { int32_t t = target_ticks_right << FACTOR; int32_t correction_factor = t / actual_ticks_right; if (correction_factor > (int16_t)(1.2 * (1 << FACTOR))) { //print_status("n"); correction_factor = (int16_t)(1.2 * (1 << FACTOR)); } if (correction_factor < (int16_t)(0.8 * (1 << FACTOR))) { //print_status("b"); correction_factor = (int16_t)(0.8 * (1 << FACTOR)); } t = factor_right_intern * correction_factor; /*print_status("f %d %d", factor_right_intern, factor_right_intern >> FACTOR); print_status("c %ld %ld", correction_factor, correction_factor >> FACTOR); print_status("t %ld %ld", t,t >> FACTOR);*/ factor_right_intern = t >> FACTOR; if (factor_right_intern < 3 << FACTOR) { //print_status("1 %ld %ld", factor_right_intern, factor_right_intern >> FACTOR); factor_right_intern = 3 << FACTOR; } } else { factor_right_intern = factor_right_intern + (factor_right_intern >> 1) ; } if (factor_right_intern > 50 << FACTOR) { //print_status("2"); factor_right_intern = 50 << FACTOR; } if (factor_right_intern <= 0) { //print_status("3"); factor_right_intern = 3 << FACTOR; } error_left[0] = error_left[1]; error_left[1] = error_left[2]; error_left[2] = target_ticks_left - actual_ticks_left; print_status("l;pwm; %5d; fact; %6d/%6d; tar; %4d; act; %4d", motpwm_left_value, factor_left_intern, factor_left_intern >> FACTOR, target_ticks_left, actual_ticks_left); error_right[0] = error_right[1]; error_right[1] = error_right[2]; error_right[2] = target_ticks_right - actual_ticks_right; print_status("r;pwm; %5d; fact; %6d/%6d; tar; %4d; act; %4d", motpwm_right_value, factor_right_intern, factor_right_intern >> FACTOR, target_ticks_right, actual_ticks_right); } //kann man das als neuen Faktor nehmen? int16_t sum_error_left = abs(error_left[0]) + abs(error_left[1]) +abs(error_left[2]); if (sum_error_left <= 5) { print_status("guter Faktor"); factor_left = factor_left_intern; } int16_t sum_error_right = abs(error_right[0]) + abs(error_right[1]) +abs(error_right[2]); if (sum_error_right <= 5) { print_status("guter Faktor"); factor_right = factor_right_intern; } } int main() { usart_enable(); usart_setbaudrate(38400); motpwm_init(); odometry_init(); sei(); print_status("hallo"); while(1) { drive(80,80,1000,1); drive(40,40,1000,0); drive(80,40,1000,0); drive(40,80,1000,0); motpwm_setRight(0); motpwm_setLeft(0); while(1); } print_status("fertig"); while(1); }
Compiler results:
Werbung
Online
camillakau92347540508
conniewum948426930