hallo allerseits,

auch hier kleine fortschritte. Habe das programm für den asuro mit PID-regler doch für den RP6 "umgeschrieben". Um es von der M32 betreiben zu können waren in der "RP6Control_I2CMasterLib.c", der "RP6Control_I2CMasterLib..h" und der "RP6Base_I2CSlave.c" folgende änderungen notwendig:

Code:
set_motor_power_in_base_slave...



in der RP6Control_I2CMasterLib.c einfügen:
------------------------------------------

// Speed
uint8_t left_power;
uint8_t right_power;


/**
 * Set Motor power function
 */


void setMotorPower(uint8_t left_power, uint8_t right_power)
{
I2CTWI_transmit4Bytes(I2C_RP6_BASE_ADR, 0, CMD_SET_MOTOR_POWER, left_power, right_power );
while(I2CTWI_isBusy() || TWI_operation != I2CTWI_NO_OPERATION) task_I2CTWI();
}

in der RP6Control_I2CMasterLib.h einfügen:
------------------------------------------

#define CMD_SET_MOTOR_POWER        14

void setMotorPower(uint8_t left_power, uint8_t right_power);

// Speed
extern uint8_t left_power;
extern uint8_t right_power;


in der RP6Base_I2CSlave.c einfügen:
----------------------------------

#define CMD_SET_MOTOR_POWER        14

in der Funktion "void task_commandProcessor(void)" einfügen:

case CMD_SET_MOTOR_POWER: setMotorPower(param1, param2); break;
und

Code:
set_motor_dir_in_base_slave...



in der RP6Control_I2CMasterLib.c einfügen:
------------------------------------------

// Direction
uint8_t left_dir;
uint8_t right_dir;


/**
 * Set Motor dir function
 */


void setMotorDir(uint8_t left_dir, uint8_t right_dir)
{
I2CTWI_transmit4Bytes(I2C_RP6_BASE_ADR, 0, CMD_SET_MOTOR_DIR, left_dir, right_dir );
while(I2CTWI_isBusy() || TWI_operation != I2CTWI_NO_OPERATION) task_I2CTWI();
}

in der RP6Control_I2CMasterLib.h einfügen:
------------------------------------------

#define CMD_SET_MOTOR_DIR        13

void setMotorDir(uint8_t left_dir, uint8_t right_dir);

// Direction
extern uint8_t left_dir;
extern uint8_t right_dir;


in der RP6Base_I2CSlave.c einfügen:
----------------------------------

#define CMD_SET_MOTOR_DIR        13

in der Funktion "void task_commandProcessor(void)" einfügen:

case CMD_SET_MOTOR_DIR: setMotorDir(param1, param2); break;
der eigentlicher
Code:
/*******************************************************************************
*
* Description: Asuro Linienverfolgung mit PID-Regler
* Version 1: Korrektur auf beide Motoren verteilt
* Author: Waste   26.8.05
*
* Version 2: initialization of parameters in Main, measurement with LED off in Main
* Author: irobot22587 11.03.2007
* überarbeitet für RP6 inka / 2014_03_10
*****************************************************************************/
#include "RP6ControlLib.h"
#include "RP6I2CmasterTWI.h"
#include "RP6Control_MultiIOLib.h"
#include "RP6Control_I2CMasterLib.h"
#include "RP6Control_LFSBumperLib.h"
#include "RP6ControlServoLib.h"
#include "standard.h"

#define I2C_RP6_BASE_ADR 10


unsigned char speed;
int speedLeft,speedRight;
unsigned int lineData[2];
int x, xalt, don, doff, kp, kd, ki, yp, yd, yi, drest, y, y2, isum;

void FollowLine (void)
{
    unsigned char leftDir = FWD, rightDir = FWD;
    lineData[0] = getLFS(CH_LFS_L);
    lineData[1] = getLFS(CH_LFS_R);
    x = (lineData[0] - lineData[1]);
    isum += x;
    if (isum > 16000) isum =16000;         //Begrenzung um Überlauf zu vermeiden
    if (isum < -16000) isum =-16000;
    yi = isum/625 * ki;               //I-Anteil berechnen //625
    yd = (x - xalt)*kd;               // D-Anteil berechnen und mit
    yd += drest;                     // nicht berücksichtigtem Rest addieren
    if (yd > 255) drest = yd - 255;      // merke Rest
    else if (yd < -255) drest = yd + 255;
    else drest = 0;
    yp = x*kp;                        // P-Anteil berechnen
    y = yp + yi + yd;                  // Gesamtkorrektur
    y2 = y/2;                        // Aufteilung auf beide Motoren
    xalt = x;                        // x merken
    speedLeft = speedRight = speed;
    setMotorDir(FWD,FWD);
    if ( y > 0)
    {
        // nach rechts
        setbumperLEDL(1);
        setbumperLEDR(0);

        speedLeft = speed + y2;         // links beschleunigen
        if (speedLeft > 120) //255,155,100
        {
            speedLeft = 80;      //155,105,50      // falls Begrenzung
            y2 = speedLeft - speed;      // dann Rest rechts berücksichtigen
        }
        y = y - y2;
        speedRight = speed - y;         // rechts abbremsen
        if (speedRight < 0)
        {
            speedRight = 0;
        }
    }
    if ( y < 0)
    {
        // nach links
        setbumperLEDR(1);
        setbumperLEDL(0);

        speedRight = speed - y2;         // rechts beschleunigen !!!was speed - y2!!
        if (speedRight > 120) //255,105,100
        {
            speedRight = 80;   //155,105,50         // falls Begrenzung
            y2 = speed - speedRight;      // dann Rest links berücksichtigen !!was speed - speedRight!!
        }
        y = y - y2;
        speedLeft = speed + y;            // links abbremsen  !! was speed + y!!!
        if (speedLeft < 0)
        {
            speedLeft = 0;
        }
    }
    leftDir = rightDir = FWD;
    if (speedLeft < 20)  mleft_speed = 0;
    if (speedRight < 20) mright_speed = 0;
    setMotorDir(leftDir,rightDir);
    setMotorPower(abs(speedLeft),abs(speedRight));
}

int main(void)
{

    initRP6Control();
    multiio_init();
    initLCD();
    lfsbumper_init();

    I2CTWI_initMaster(100);
    I2CTWI_setTransmissionErrorHandler(I2C_transmissionError);

    setLEDs(0b1111);
    clearLCD();
    setCursorPosLCD(0, 0);
    writeStringLCD("irobot_linie");
    mSleep(1500);
    clearLCD();

    accuspannung();
    clearLCD();

    setMotorDir(FWD,FWD);
    speed = 80;
    kp = 1; //3
    ki = 6; //10
    kd =  40; //70      // Regler Parameter kd enthält bereits Division durch dt
    drest=0;
    isum=0;
    xalt=0;
    while(1)
    {
        FollowLine();
    }
    return 0;
}
und das ergebnis ist hier,
die eigentliche "asuro-testarena" schafft der RP6 (zumindest mit diesem code) nicht, er sucht sich seinen eigenen weg, bricht öfters aus, findet aber immer wieder zu linie zurück...