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