-         

Ergebnis 1 bis 4 von 4

Thema: Nibobee Motor Steuerung gelungen, incl Source

  1. #1
    Erfahrener Benutzer Fleißiges Mitglied
    Registriert seit
    19.03.2010
    Beiträge
    161

    Nibobee Motor Steuerung gelungen, incl Source

    Anzeige

    Hallo Leute,
    dank euer Hilfe ich mit nun die Motorsteuerung gelungen. Mein Roboter kann jetzt folgendes Kunststück:

    1) Vier mal 1.6 Meter weit gerade (!) vor und zurück fahren. Wobei ich 200mm/sec für
    die Vorwärts-Fahrt angegeben habe, und 400mm/sec für die Rückwärts fahrt.
    2) 1.0 Meter vorwärts fahren.
    3) Auf der Stelle umdrehen (180°).
    4) Wieder zurück fahren (also wieder vorwärts 1.0 Meter).
    5) Eine Rechtskurve fahren, mit innenRadius 12cm und Außenradius 24cm. Fahrstrecke
    innen 1.5 Meter, außen 3.0 Meter.

    Es wird meine einfache nibobee Library benötigt, die ich hier schon veröffentlicht habe, und auch auf meiner Homepage.

    Nächster Schritt: Die drive() Funktion so zerlegen, dass sie nicht mehr blocking arbeitet sondern Teil einer Endlosschleife wird. Das dürfte einfach werden.

    Code:
    #include "nibobee.h"
    #include <util/delay.h>
    
    // Wait for start signal (touch any sensor)
    // While waiting, display debug information from sensors:
    //   LED0: Left odometer sensor
    //   LED3: Right odometer sensor
    //   LED1: System timer
    //   LED2: Center line sensor
    void wait_for_start() {
        while (!(SENS_SW1 || SENS_SW2 || SENS_SW3 || SENS_SW4)) {
            // Display status of odometry sensors while waiting
            set_LED0(ODO_L);
            set_LED3(ODO_R);
            // Display system timer (flashes every second)
            set_LED1((system_time() % 1000)==0);
            // Display line sensor
            set_LED2(analog(LINE_C)>600);
        }
        set_LED0(0);
        set_LED1(0);
        set_LED2(0);
        set_LED3(0);
        _delay_ms(10);
        while ((SENS_SW1 || SENS_SW2 || SENS_SW3 || SENS_SW4)) {}
        _delay_ms(400);
    }
    
    
    
    // Drive an exact distance and accellerate/decellerate softly to the given speed.
    // The distance of both wheels can be different.
    // Speed control works best in range 100-800.
    double pwm_l;
    double pwm_r;
    
    void drive(int32_t distance_mm_l, int32_t distance_mm_r, uint32_t speed_mm_sec) {
        #define ODOMETER_TICK_MM 6             // Distance of a single odometer tick in mm
    
        #define MOTOR_START_PWM 150            // Minimum PWM value to start the motors
    
        #define MOTOR_CONTROL_INTERVAL 200     // Interval of motor control.
                                               // Steering control does not work, if the interval is too small.
    
        #define SPEED_CONTROL_FACTOR 5         // Factor for speed correction.
                                               // Too high value causes slipping, much too high
                                               // value causes bucking.
    
        #define STEERING_CONTROL_FACTOR 0.3    // Factor forsteering correction.
                                               // Too high value causes swinging.
    
        // Set motor direction and remove sign from the distance value
        set_DIR_L(distance_mm_l > 0);
        set_DIR_R(distance_mm_r < 0);
        if (distance_mm_l<0) distance_mm_l *= -1;
        if (distance_mm_r<0) distance_mm_r *= -1;
        
        // Calculate the destination distance in odometer ticks
        uint32_t dest_distance_l=distance_mm_l/ODOMETER_TICK_MM;
        uint32_t dest_distance_r=distance_mm_r/ODOMETER_TICK_MM;
    
        // Calculate the steering factor of left to right motor speed
        double steering_factor;
        if (distance_mm_l>0 && distance_mm_r>0)
            steering_factor=(double) distance_mm_l / (double) distance_mm_r;
        else
            steering_factor=1;
    
        // Calculate the destination speed in odometer ticks per millisecond
        double dest_speed=(double) speed_mm_sec/ODOMETER_TICK_MM/1000;
    
        // Initialize variables
        reset_odometer();
        uint32_t last_odo_l=0;
        uint32_t last_odo_r=0;
        uint32_t last_time=system_time();
        
        // Start the motors, if necessary
        if (distance_mm_l>0 && PWM_L==0)
            pwm_l=MOTOR_START_PWM;
        if (distance_mm_r>0 && PWM_R==0)
            pwm_r=MOTOR_START_PWM;
    
        // Drive until both motors reached the nominal distance
        while (odometer_left()<dest_distance_l || odometer_right()<dest_distance_r) {
    
            // Calculate time interval
            uint32_t now=system_time();
            int32_t interval_time=now-last_time;
    
            if (interval_time>=MOTOR_CONTROL_INTERVAL) {
    
                // Get current distance in odometer ticks
                uint32_t odo_l=odometer_left();
                uint32_t odo_r=odometer_right();
                double delta_odo_l=(odo_l-last_odo_l);
                double delta_odo_r=(odo_r-last_odo_r);
    
                // average speed of both wheels in odometer ticks per millisecond
                double distance=(delta_odo_l+delta_odo_r)/2;
                double speed=distance/interval_time;
                double speed_error=dest_speed-speed;
    
                // Calculate steering error
                double steering_error=0;
                if (delta_odo_r>0 && delta_odo_l>0)
                    steering_error=steering_factor-(delta_odo_l/delta_odo_r);
    
                // Display speed status
                set_LED1(speed_error>0);
                set_LED2(speed_error>0);
    
                // Display steering status
                set_LED0(steering_error<0);
                set_LED3(steering_error>0);
    
                // Speed control with 
                if (speed_error!=0) {
                    pwm_l+=speed_error*interval_time*SPEED_CONTROL_FACTOR*steering_factor;
                    pwm_r+=speed_error*interval_time*SPEED_CONTROL_FACTOR/steering_factor;
                }            
    
                // Steering control with 
                if (steering_error!=0) {
                    pwm_l+=steering_error*interval_time*STEERING_CONTROL_FACTOR;
                    pwm_r-=steering_error*interval_time*STEERING_CONTROL_FACTOR;
                }
    
                // Limit PWM values to the valid range
                if (pwm_l>1023) pwm_l=1023;
                if (pwm_r>1023) pwm_r=1023;
                if (pwm_l<0) pwm_l=0;
                if (pwm_r<0) pwm_r=0;
    
                // Apply the new PWM values
                PWM_L=pwm_l;
                PWM_R=pwm_r;
    
                last_time=now;
                last_odo_l=odo_l;
                last_odo_r=odo_r;
            }
        }
    }
    
    
    
    // Stop driving.
    void stop() {
        PWM_L=0;
        PWM_R=0;
        set_LED0(1);
        set_LED1(1);
        set_LED2(1);
        set_LED3(1);
        _delay_ms(50);
        set_LED0(0);
        set_LED1(0);
        set_LED2(0);
        set_LED3(0);
        _delay_ms(50);
        set_LED0(1);
        set_LED1(1);
        set_LED2(1);
        set_LED3(1);
        _delay_ms(50);
        set_LED0(0);
        set_LED1(0);
        set_LED2(0);
        set_LED3(0);
        _delay_ms(50);
    }
    
    
    // Main program
    int main() {
        wait_for_start();
        // Drive a few time forward and backward for 1,6 meters
        for (int i=0; i<4; i++) {
            drive(1300,1300,200);
            drive(300,300,100);
            stop();
            _delay_ms(300);
            drive(-1000,-1000,400);
            drive(-600,-600,100);
            stop();
            _delay_ms(300);
        }
        // Drive forward for 1 meter
        drive(700,700,200);
        drive(300,300,100);
        stop();
        _delay_ms(300);
        // Turn 180° 
        drive(180,-180,200);
        stop();
        _delay_ms(300);
        // Drive back
        drive(700,700,200);
        drive(300,300,100);
        stop();
        // Drive a circle
        drive(3000,1500,200);
        stop();
        return 0;
    }

  2. #2
    Erfahrener Benutzer Fleißiges Mitglied
    Registriert seit
    19.03.2010
    Beiträge
    161
    Heute habe ich etwas neues probiert: Achten fahren. Dabei hat sich heraus gestellt, dass die Lenkung mittels I-Regler beim Wechsel der Lenk-Richtung zu träge ist. Ich habe darum einen P-Regler für die Lenkung hinzugefügt, nun klappt auch das.

    Meine Regelung entspricht nicht ganz dem Verfahren in der Wissens-Sammlung. Das Ergebnis des I-Reglers verwende ich nicht direkt als PWM Wert, sondern ich addiere es zum bisherigen PWM Wert. Andererseits bilde ich keine Summe der Fehler aller Intervalle. Unterm Strich ergibt sich ein Regelverhalten, dass dem I Regler ähnlich ist. Auch mein P-Regler ändert nur den aktuellen PWM Wert, weswegen er zu starken Schwingungen neigt, wenn nicht zugleich der I-Regler dagegen steuert. Insgesamt klappt das Zusammenspiel des I-Regler zur Geschwindigkeitssteuerung und des PI Reglers der Lenkung nun zufriedenstellend.

    Code:
    #include "nibobee.h"
    #include <util/delay.h>
    
    
    
    // Fine-Tuning settings for motor control
    
    
    #define ODOMETER_TICK_MM 6                // Distance of a single odometer tick in mm
    
    #define MOTOR_START_PWM 250               // Minimum PWM value to start the motors
    
    #define MOTOR_CONTROL_INTERVAL 100        // Interval of motor control.
                                              // Steering control does not work, if the
                                              // interval is too small.
    
    #define SPEED_CONTROL_FACTOR 5            // Factor for speed correction.
                                              // Too high value causes slipping or bucking.
    
    #define STEERING_CONTROL_FACTOR_I 0.1     // Factor for integral steering correction.
                                              // Too less and too high values cause swinging.
    
    #define STEERING_CONTROL_FACTOR_P 2       // Factor for pulse steering correction.
                                              // Too high value causes over-steering.
    
    
    // How to tune steering: set FACTOR_P to 0, then increase FACTOR_I to the highest
    // possible value that does not cause swinging. Then increase FACTOR_P to the
    // highest possible value that does not over-steering.
    
    
    
    // Wait for start signal (touch any sensor)
    // While waiting, display debug information from sensors:
    //   LED0: Left odometer sensor
    //   LED3: Right odometer sensor
    //   LED1: System timer
    //   LED2: Center line sensor
    void wait_for_start() {
        while (!(SENS_SW1 || SENS_SW2 || SENS_SW3 || SENS_SW4)) {
            // Display status of odometry sensors while waiting
            set_LED0(ODO_L);
            set_LED3(ODO_R);
            // Display system timer (flashes every second)
            set_LED1((system_time() % 1000)==0);
            // Display line sensor
            set_LED2(analog(LINE_C)>600);
        }
        set_LED0(0);
        set_LED1(0);
        set_LED2(0);
        set_LED3(0);
        _delay_ms(10);
        while ((SENS_SW1 || SENS_SW2 || SENS_SW3 || SENS_SW4)) {}
        _delay_ms(400);
    }
    
    
    
    // Drive an exact distance and accellerate/decellerate softly to the given speed.
    // The distance of both wheels can be different.
    // Speed control works best in range 100-800.
    void drive(int32_t distance_mm_l, int32_t distance_mm_r, uint32_t speed_mm_sec) {
    
        // Set motor direction and remove sign from the distance value
        set_DIR_L(distance_mm_l > 0);
        set_DIR_R(distance_mm_r < 0);
        if (distance_mm_l<0) distance_mm_l *= -1;
        if (distance_mm_r<0) distance_mm_r *= -1;
        
        // Calculate the destination distance in odometer ticks
        uint32_t dest_distance_l=distance_mm_l/ODOMETER_TICK_MM;
        uint32_t dest_distance_r=distance_mm_r/ODOMETER_TICK_MM;
    
        // Calculate the steering factor of left to right motor speed
        double steering_factor;
        if (distance_mm_l>0 && distance_mm_r>0)
            steering_factor=(double) distance_mm_l / (double) distance_mm_r;
        else
            steering_factor=1;
    
        // Calculate the destination speed in odometer ticks per millisecond
        double dest_speed=(double) speed_mm_sec/ODOMETER_TICK_MM/1000;
    
        // Initialize variables
        reset_odometer();
        uint32_t last_odo_l=0;
        uint32_t last_odo_r=0;
        uint32_t last_time=system_time();
        // Current pwm values
        uint16_t pwm_l=PWM_L;
        uint16_t pwm_r=PWM_R;
        
        // Start the motors, if necessary
        if (distance_mm_l>0 && pwm_l==0)
            pwm_l=MOTOR_START_PWM;
        if (distance_mm_r>0 && pwm_r==0)
            pwm_r=MOTOR_START_PWM;
    
        // Drive until both motors reached the nominal distance
        while (odometer_left()<dest_distance_l || odometer_right()<dest_distance_r) {
    
            // Calculate time interval
            uint32_t now=system_time();
            int32_t interval_time=now-last_time;
    
            if (interval_time>=MOTOR_CONTROL_INTERVAL) {
    
                // Get current distance in odometer ticks
                uint32_t odo_l=odometer_left();
                uint32_t odo_r=odometer_right();
                double delta_odo_l=(odo_l-last_odo_l);
                double delta_odo_r=(odo_r-last_odo_r);
    
                // average speed of both wheels in odometer ticks per millisecond
                double distance=(delta_odo_l+delta_odo_r)/2;
                double speed=distance/interval_time;
                double speed_error=dest_speed-speed;
    
                // Speed control
                if (speed_error!=0) {
                    double value=speed_error*interval_time*SPEED_CONTROL_FACTOR;
                    pwm_l+=value*steering_factor;
                    pwm_r+=value/steering_factor;
                }
    
                // Display speed status
                set_LED1(speed_error>1)
    
                // Calculate steering error
                double steering_error_i=0;
                if (delta_odo_r>0 && delta_odo_l>0) {
                    steering_error_i=delta_odo_l-delta_odo_r*steering_factor;
                }
    
                double steering_error_p=0;
                if (odo_r>0 && odo_l>0) {
                    steering_error_p=odo_l-odo_r*steering_factor;
                }
    
                // Integral steering control
                double steering=steering_error_i*interval_time*STEERING_CONTROL_FACTOR_I+
                                steering_error_p*STEERING_CONTROL_FACTOR_P;
                pwm_l-=steering;
                pwm_r+=steering;
    
                // Display steering status
                set_LED0(steering>1);
                set_LED3(steering<-1);
    
                // Apply the new PWM values to the motor driver, while
                // limiting to the allowed range.
                if (pwm_l>1023) pwm_l=1023;
                PWM_L=pwm_l;
                if (pwm_r>1023) pwm_r=1023;
                PWM_R=pwm_r;
               
                last_time=now;
                last_odo_l=odo_l;
                last_odo_r=odo_r;
            }
        }
    }
    
    
    
    // Stop driving.
    void stop() {
        PWM_L=0;
        PWM_R=0;
        set_LED1(1);
        _delay_ms(50);
        set_LED1(0);
        _delay_ms(50);
        set_LED1(1);
        _delay_ms(50);
        set_LED1(0);
        _delay_ms(50);
    }
    
    
    // Main program
    int main() {
        while (1) {
            wait_for_start();
            // Drive a few times forward and backward for 2 meters
            for (int i=0; i<3; i++) {
                drive(1700,1700,200);
                drive(300,300,100);
                stop();
                _delay_ms(300);
                drive(-1700,-1700,400);
                drive(-300,-300,100);
                stop();
                _delay_ms(300);
            }
            // Drive forward for 1 meter
            drive(700,700,200);
            drive(300,300,100);
            stop();
            _delay_ms(300);
            // Turn 180°
            drive(180,-180,200);
            stop();
            _delay_ms(300);
            // Drive back
            drive(700,700,200);
            drive(300,300,100);
            // Drive an eight
            uint32_t distance=1450;
            for (int i=0; i<3; i++) {
                drive(distance,distance/2,200);
                drive(distance/2,distance,200);
            }
            stop();
        }
        return 0;
    }

  3. #3
    Neuer Benutzer Öfters hier
    Registriert seit
    26.02.2010
    Ort
    Kreuzung BAB3 und B470
    Beiträge
    25
    Erstmal Glücklichen Herzwunsch;

    Betreff Regelung: Der P-Anteil gibt vor in welcher Richtung sich das Y bewegen soll, wenn eine Regelabweichung vorliegt. Schwingt ein Regler, ist der P-Anteil (die Verstärkung) zu groß (zu heftig).

    Ein reiner P-Regler erreicht nie den Sollwert, die Abweichung ist abhängig von der Verstärkung. Kleine Verstärkung = große Abweichung. Der I-Anteil soll letztendlich diese Regelabweichung über eine Zeit ausgleichen, und das Y Stückchen für Stückchen nach führen, bis die Regelabweichung null ist.

    Mit D-Anteilen habe ich noch nie gearbeitet, aber das ist in etwa so zu verstehen: Wenn man etwas mit Wasser kochen will. wird die Herdplatte erstmal auf volle Pulle gestellt, und dann zurückgedreht, wenn's unterm Deckel vorspritzt.

  4. #4
    Erfahrener Benutzer Fleißiges Mitglied
    Registriert seit
    19.03.2010
    Beiträge
    161
    Die aktuelle Version der Library (und Beispielprogramme) kann man auf meiner Homepage downloaden: http://stefanfrings.de/nibobee/index.html

Berechtigungen

  • Neue Themen erstellen: Nein
  • Themen beantworten: Nein
  • Anhänge hochladen: Nein
  • Beiträge bearbeiten: Nein
  •