-         

Ergebnis 1 bis 9 von 9

Thema: Task motionControl() mit der M32

  1. #1
    Erfahrener Benutzer Roboter Genie Avatar von inka
    Registriert seit
    29.10.2006
    Ort
    nahe Dresden
    Alter
    70
    Beiträge
    1.344

    Task motionControl() mit der M32

    Anzeige

    hi allerseits,

    im zusammenhang mit der neuen I/O beschäftige ich mich auch wieder mit der M32:

    Code:
    #include "RP6ControlLib.h"
    #include "RP6I2CmasterTWI.h"
    #include "RP6Control_I2CMasterLib.h"
    
    
    void I2C_transmissionError(uint8_t errorState)
    {
        clearLCD();
        writeStringLCD_P("I2C ERROR -->");
        setCursorPosLCD(1, 0);        // line 2
        writeStringLCD_P("TWI STATE: 0x");
        writeIntegerLCD(errorState, HEX);
    }
    
    
    
    void I2C_requestedDataReady(uint8_t dataRequestID)
    {
        checkRP6Status(dataRequestID);
    }
    
    int main(void)
    {
    
    
    I2CTWI_initMaster(100);
    I2CTWI_setRequestedDataReadyHandler(I2C_requestedDataReady);
    I2CTWI_setTransmissionErrorHandler(I2C_transmissionError);
    
        initRP6Control();
        setLEDs(0b111111);
        mSleep(1000);
    
    //    powerON();
        setLEDs(0b101001);
        mSleep (1000);
    
        while(true)
        {
    //        task_motionControl();
    
            setLEDs(0b111111);
    
            move(150, 0, 1000, 1);
    
            setLEDs(0b100000);
    
            mSleep(2500);
    
            setLEDs(0b000100);
    
            rotate(150, 2, 130, 1);
    
            mSleep(2500);
    
    //        move(100, FWD, DIST_MM(1000), true);
    
    //        setLEDs(0b000100);
    
            //setLEDs(0b100000);
    
            //rotate(100, LEFT, 360, true);
    
            //mSleep(2500);
        }
    
        return 0;
    }
    das progrämmchen läuft, die kompassdaten werden angezeigt, allerdings fährt der roboter keinen meter, sondern höchstens 20cm. Ich vermute, das liegt daran, dass die funktion "motionControl()" von der M32 aus nicht läuft. Zumindest ist es mir nicht gelungen sie soweit zu brinden, dass es beim kompilieren keine fehlermeldungen gab...

    ist das die ursache, oder gibt es dafür andere gründe/erklärungen?
    gruß inka

  2. #2
    Erfahrener Benutzer Fleißiges Mitglied Avatar von Thorben W
    Registriert seit
    17.02.2012
    Ort
    Niedersachsen
    Beiträge
    108
    Die motionControl wird doch vom RP6 aus der I²C Slave aufgerufen.
    Thorben

  3. #3
    Erfahrener Benutzer Roboter Genie Avatar von inka
    Registriert seit
    29.10.2006
    Ort
    nahe Dresden
    Alter
    70
    Beiträge
    1.344
    hi Thorben,
    könntest du mir bitte etwas genauer erklären wie du das meinst?

    Auf der base des RP6 läuft ja die RP6Base_I2CSlave.hex...

    danke
    gruß inka

  4. #4
    Erfahrener Benutzer Fleißiges Mitglied Avatar von Thorben W
    Registriert seit
    17.02.2012
    Ort
    Niedersachsen
    Beiträge
    108
    Wenn ich das richtig gesehen habe, gibt es eine Funktion für den I²C Bus für den motion control die das selbe Bewirkt die du nur auslesen musst
    Code:
    /**
     * Motion Control Event Handler
     */ 
    void motionControlStateChanged(void)
    {
    	drive_status.movementComplete = isMovementComplete();
    	drive_status.motorOvercurrent = motion_status.overcurrent;
    	interrupt_status.driveSystemChange = true;
    	signalInterrupt();
    }
    Als Beispiel des funktionierenden Fahrens würde ich mal bei den WIFI Beispielprogrammen mir das RP6_M256_09_Move.c ansehen
    Thorben

  5. #5
    Erfahrener Benutzer Roboter Genie Avatar von inka
    Registriert seit
    29.10.2006
    Ort
    nahe Dresden
    Alter
    70
    Beiträge
    1.344
    vielen dank Thorben, das von dir beschriebene ist eine andere funktion. Ich Suche ein entsprechendes pendant zu dem hier

    Code:
    void task_motionControl(void)
    {
        // Automatic motor overcurrent shutdown:
        if(overcurrent_timer >= 50) { // every 5ms
            overcurrent_timer = 0;
            if(!overcurrent_timeout) {
                if((adcMotorCurrentLeft > 770) || (adcMotorCurrentRight > 770)) {
                    overcurrent_errors++;
                    overcurrent_timeout = 10; 
                    mleft_power = 0;
                    mright_power = 0;                
                    left_i = 0;
                    right_i = 0;
                    motion_status.overcurrent = true;
                    return;
                }
                else
                    motion_status.overcurrent = false;
                
                // Emergency shutdown if there are too many (default: 3) overcurrent
                // events within ~20 seconds (100 * 200ms).
                if(overcurrent_error_clear > 100) {
                    overcurrent_errors = 0;
                    overcurrent_error_clear = 0;
                }
                else if(overcurrent_errors > 2)
                    emergencyShutdown(OVERCURRENT);
            }
            
            // Detect if one of the encoders or motors does not work properly and stop 
            // all operations immediately if this is the case! 
            if((adcMotorCurrentLeft < 150) && (mleft_speed == 0) 
              && (mleft_des_speed != 0) &&  (mleft_ptmp > 150))
                emergencyShutdown(ENCODER_MALFUNCTION_LEFT);
            if((adcMotorCurrentRight < 150) && (mright_speed == 0) 
              && (mright_des_speed != 0) && (mright_ptmp > 150))
                emergencyShutdown(ENCODER_MALFUNCTION_RIGHT);
        }
        
        // Motor Control
        if(motor_control) { // Everytime after the speed has been measured. (default: 200ms)
            if(!overcurrent_timeout) { // No overcurrent timeout? (default is to wait 2 seconds before new try)
                if(overcurrent_errors) // Overcurrent errors?
                    overcurrent_error_clear++; // Yes, Timeout to clear all error events.
                else
                    overcurrent_error_clear=0; // No, we set the timeout to zero.
                    
                // Move Distance left:
                if(motion_status.move_R) {
                    if(mleft_dist >= preStop_R) { // Stop a bit before the desired distance for ..
                        mleft_des_speed = 0;      // ... better accurancy.
                        left_i = 0;
                        mleft_power = 0;        
                        motion_status.move_R = false;
                    }
                    else if(mleft_dist >= preDecelerate_R) { // Start to decelerate?
                        mleft_des_speed /= 2;
                        if(mleft_des_speed < 22) mleft_des_speed = 22;
                    }    
                }
                
                // Move Distance right:
                if(motion_status.move_L) {
                    if(mright_dist >= preStop_L) { // Stop a bit before the desired distance for ..
                        mright_des_speed = 0;      // ... better accurancy.
                        right_i = 0;
                        mright_power = 0;
                        motion_status.move_L = false;
                    }
                    else if(mright_dist >= preDecelerate_L) { // Start to decelerate?
                        mright_des_speed /= 2;
                        if(mright_des_speed < 22) mright_des_speed = 22;
                    }    
                }
        
    // ----------------------------------------
    // The following settings are for users that want to tune the behaviour
    // of the robot when changing direction --> make it faster or slower.
    // Carefully read the comments!  
    // #define CHANGE_DIRECTION_SLOW   // this is enabled by default when none of the other options is enabled. 
    #define CHANGE_DIRECTION_MEDIUM  // Standard, keep it like this if you do not know what you are doing!
    // Uncomment this if you want the Robot to change motor direction as fast as possible: 
    //#define CHANGE_DIRECTION_FAST
    
    #ifdef CHANGE_DIRECTION_FAST
                // Change direction -- FAST Version.
                // This changes direction without any slowdown. 
          // Only use this mode if you need it as this will reduce
          // lifetime of the motors and gears.   
                if(mleft_des_dir != mleft_dir || mright_des_dir != mright_dir) {
                    mright_ptmp = 0;
                    mleft_ptmp = 0;
                    mright_power=0; 
                        mleft_power=0;
                        TCCR1A = 0;   
                        setMotorDir(mleft_des_dir,mright_des_dir);
                }
    #else 
    #ifdef CHANGE_DIRECTION_MEDIUM
                // Change direction -- Medium Version.
          // This stops before changing the motor direction but is a bit faster
          // than the original version. 
                if(mleft_des_dir != mleft_dir || mright_des_dir != mright_dir) {
                    if(mleft_des_speed || mright_des_speed) {
                        mleft_des_speed_tmp = mleft_des_speed; // store current speed
                        mright_des_speed_tmp = mright_des_speed; 
                        mleft_des_speed = 0;            
                        mright_des_speed = 0;
                        mright_power=0; // Soft PWM adjust to 0
                        mleft_power=0;
                        left_i = 0;
                        right_i = 0;
                    }
                    if(!TCCR1A) {
                        setMotorDir(mleft_des_dir,mright_des_dir);
                        mleft_des_speed = mleft_des_speed_tmp;
                        mright_des_speed = mright_des_speed_tmp;
                        left_i = mleft_des_speed / 2;
                        right_i = mright_des_speed / 2;
                    }
                }
    #else
                // Change direction -- slow Version from the original library. 
                // Slowdown and stop before changing direction  
                // to improve gears and motors lifetime.
                // This variant was the original Version, the current library has
                // medium enabled by default. 
                if(mleft_des_dir != mleft_dir || mright_des_dir != mright_dir) {
                    if(mleft_des_speed || mright_des_speed) {
                        mleft_des_speed_tmp = mleft_des_speed; // store current speed
                        mright_des_speed_tmp = mright_des_speed; 
                        mleft_des_speed = 0;            
                        mright_des_speed = 0;
                        left_i /= 2;
                        right_i /= 2;
                    }
                    if(mright_speed <= 40 && mleft_speed <= 40) {
                        mright_power=0; // Soft PWM adjust to 0
                        mleft_power=0;
                    }
                    if(!TCCR1A) // Is the PWM module turned off?
                    {            // Yes, change direction and restore old speed:
                        setMotorDir(mleft_des_dir,mright_des_dir);
                        mleft_des_speed = mleft_des_speed_tmp;
                        mright_des_speed = mright_des_speed_tmp;
                        left_i = 0;
                        right_i = 0;
                    }
                }
    #endif
    #endif
    
    
                // Left motor speed control:
                int16_t error_left = mleft_des_speed - mleft_speed;
                left_i = left_i + error_left;
                if(left_i > MC_LEFT_IMAX) left_i = MC_LEFT_IMAX;
                if(left_i < MC_LEFT_IMIN) left_i = MC_LEFT_IMIN;
                if(mleft_speed == 0 && mleft_des_speed == 0)
                    left_i = 0;
                mleft_power = left_i / 2; 
                if(mleft_power > 210) mleft_power = 210;
                if(mleft_power < 0) mleft_power = 0;
                
                // Right motor speed control:
                int16_t error_right = mright_des_speed - mright_speed;
                right_i = right_i + error_right;
                if(right_i > MC_RIGHT_IMAX) right_i = MC_RIGHT_IMAX;
                if(right_i < MC_RIGHT_IMIN) right_i = MC_RIGHT_IMIN;
                if(mright_speed == 0 && mright_des_speed == 0)
                    right_i = 0;
                mright_power = right_i / 2;
                if(mright_power > 210) mright_power = 210;
                if(mright_power < 0) mright_power = 0;
            }
            else
                overcurrent_timeout--;
            motor_control = false;
        }
        
        // Call event handlers if necessary:
        if(motion_status_tmp != motion_status.byte)
        {
            motion_status_tmp = motion_status.byte;
            MOTIONCONTROL_stateChangedHandler();
        }
    }
    
    /**
     * This function sets the desired speed value. 
     * The rest is done automatically. The speed is regulated to this speed value 
     * independent of Battery voltage, surface, weight of the robot and other things. 
     *
     * You need to call task_motorSpeedControl();  frequently out of the main loop!
     * otherwise this function will not work!
     * Or use task_RP6System which includes this call. 
     *
     * The function limits maximum speed to 200! This has been done because the maximum
     * possible speed gets lower over time due to battery discharging (--> voltage drop).
     * And the gears and motors will live longer if you don't stress them that much.
     *
     * Also 200 leaves a bit room to the maximum possible PWM value when you 
     * put additional load onto the Robot or drive up a ramp etc.  
     *
     */
    was von der M32 aus aufgerufen werden kann.

    Hat sich echt niemand mit den move/rotate und ähnlichen befehlen/funktionen und deren ausführung von der M32 aus beschäftigt?

    Oder liegt das problem woanders?
    gruß inka

  6. #6
    Erfahrener Benutzer Robotik Einstein Avatar von Dirk
    Registriert seit
    30.04.2004
    Ort
    NRW
    Beiträge
    3.791
    @inka: Die Funktion task_motionControl() macht auf der M32 keinen Sinn. Auf der Base läuft ja der I2C-Slave, der auch die task_motionControl() nutzt. Du brauchst auf der M32 (I2C Master) nur die Bewegungs-Befehle, die die RP6Control_I2CMasterLib zur Verfügung stellt. Wenn der RP6 also nicht fährt, gibt es grob 3 Möglichkeiten: 1. Das I2C-Slave-Programm läuft nicht in der RP6Base 2. Dein I2C Master Programm für die M32 ist nicht ok. 3. Es gibt ein technisches Problem (das nehme ich erstmal nicht an).
    Gruß
    Dirk

  7. #7
    Erfahrener Benutzer Roboter Genie Avatar von inka
    Registriert seit
    29.10.2006
    Ort
    nahe Dresden
    Alter
    70
    Beiträge
    1.344
    hi Dirk,
    das die funktion task_motionControl() auf der M32 keinen sinn mach war mir klar, ich hatte nur den eindruck, dass da irgendwas an der kommunikation zwischen der M32 und der BASE nicht stimmt und ich dachte ich kann sie mit dem aufruf in gang bringen...

    1) das slave programm läuft auf der BASE
    2) I2C masterprogramm neu ins verzeichnis kopiert, neu kompiliert...
    3) akkus neu geladen (spannung lag bei 6,3V), nachher 8,2V

    trotzdem fährt der RP6 nach der zeile

    move(150, 0, 1000, 1);

    nicht 1m sondern nur ca 25cm...
    gruß inka

  8. #8
    Erfahrener Benutzer Robotik Einstein Avatar von Dirk
    Registriert seit
    30.04.2004
    Ort
    NRW
    Beiträge
    3.791
    Soweit ich weiß, steht distance (3. Parameter von move) auch für "Encoder Zählschritte" und nicht für Millimeter.
    Gruß
    Dirk

  9. #9
    Erfahrener Benutzer Roboter Genie Avatar von inka
    Registriert seit
    29.10.2006
    Ort
    nahe Dresden
    Alter
    70
    Beiträge
    1.344
    ja, da habe ich mir das umsetzen von (war im programm für BASE so drinn):

    move(150, FWD, DIST_MM(1000), true);

    in: move(60, 0, 1000, 1);


    zu einfach gemacht und das makro unterschlagen...
    gruß inka

Ähnliche Themen

  1. Zusätzlich zu der M32 noch ein mega8?
    Von AsuroPhilip im Forum Robby RP6
    Antworten: 40
    Letzter Beitrag: 04.11.2011, 11:43
  2. Zusammenspiel der BASE und M32 Control
    Von inka im Forum Robby RP6
    Antworten: 5
    Letzter Beitrag: 01.09.2010, 13:04
  3. EEPROM der M32: Flüchtig oder Nicht?
    Von Fabian E. im Forum Robby RP6
    Antworten: 3
    Letzter Beitrag: 01.06.2010, 22:09
  4. Antworten: 1
    Letzter Beitrag: 15.05.2010, 21:36
  5. Motorsteuerung mit M32
    Von Thomas12 im Forum Robby RP6
    Antworten: 22
    Letzter Beitrag: 24.08.2009, 06:59

Berechtigungen

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