Task motionControl() mit der M32

**inka** · 08.04.2013, 20:14

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?

**Thorben W** · 08.04.2013, 20:23

Die motionControl wird doch vom RP6 aus der I²C Slave aufgerufen.
Thorben

**inka** · 08.04.2013, 21:05

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

**Thorben W** · 08.04.2013, 21:44

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

**inka** · 09.04.2013, 10:45

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?

**Dirk** · 09.04.2013, 19:23

@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).

**inka** · 09.04.2013, 20:11

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...

**Dirk** · 09.04.2013, 22:04

Soweit ich weiß, steht distance (3. Parameter von move) auch für "Encoder Zählschritte" und nicht für Millimeter.

**inka** · 10.04.2013, 07:40

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...