linienfolger

[inka]

hallo,

es ist schon erstaunlich mit was allem man in berührung kommt, wenn man "nur" eine ladestation finden will. IR, induktives laden, oszilloskop, jede menge anderer elektronik und software. Gut so. In diesem zusammenhang taucht jetzt die linienerkennung auf...

Der RP6 ist ja nicht "von natur aus" dafür gemacht, es gibt auch relativ wenig dazu im forum zu lesen - wenn ich da an den asuro denke :-) Da waren viele unterwegs - stochri, waste, sterntaler, radbruch und, und und...

ich hab da ein beispiel gefunden, einfach irre:


/************************************************** *****************************
*
* Description: Asuro Linienverfolgung mit PID-Regler
* Version 1: Korrektur auf beide Motoren verteilt
* Autor: Waste 26.8.05
*
************************************************** ***************************/
#include "asuro.h"
#include <stdlib.h>

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;
FrontLED(OFF);
LineData(lineData); // Messung mit LED OFF
doff = (lineData[0] - lineData[1]); // zur Kompensation des Umgebungslicht
FrontLED(ON);
LineData(lineData); // Messung mit LED ON
don = (lineData[0] - lineData[1]);
x = don - doff; // Regelabweichung
isum += x;
if (isum > 16000) isum =16000; //Begrenzung um Überlauf zu vermeiden
if (isum < -16000) isum =-16000;
yi = isum/625 * ki; //I-Anteil berechnen
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;
if (isum > 15000) BackLED(OFF,ON); // nur zur Diagnostik
else if (isum < -15000) BackLED(ON,OFF);
else BackLED(OFF,OFF);
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;
MotorDir(FWD,FWD);
if ( y > 0) { // nach rechts
StatusLED(GREEN);
speedLeft = speed + y2; // links beschleunigen
if (speedLeft > 255) {
speedLeft = 255; // 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
StatusLED(RED);
speedRight = speed - y2; // rechts beschleunigen
if (speedRight > 255) {
speedRight = 255; // falls Begrenzung
y2 = speed - speedRight; // dann Rest links berücksichtigen
}
y = y - y2;
speedLeft = speed + y; // links abbremsen
if (speedLeft < 0) {
speedLeft = 0;
}
}
leftDir = rightDir = FWD;
if (speedLeft < 20) leftDir = BREAK; // richtig bremsen
if (speedRight < 20) rightDir = BREAK;
MotorDir(leftDir,rightDir);
MotorSpeed(abs(speedLeft),abs(speedRight));
}

int main(void)
{
unsigned char sw;
Init();
MotorDir(FWD,FWD);
StatusLED(GREEN);
speed = 150;
kp = 3; ki = 10; kd = 70; // Regler Parameter kd enthält bereits Division durch dt
sw = PollSwitch();
if (sw & 0x01)
{ki=20;}
if (sw & 0x02)
{speed = 200;}
if (sw & 0x04)
speed = 100;
if (sw & 0x08)
kd = 35;
if (sw & 0x10)
kd = 70;
if (sw & 0x20)
kd = 140;
FrontLED(ON);
LineData(lineData);
speedLeft = speedRight = speed;
while(1){
FollowLine();
}
return 0;
}

ich will es um gottes willen nicht nachbauen, bzw. für den RP6 umschreiben - dazu wäre ich garnicht in der lage. Aber wenn man bedenkt wieviel hirnschmalz dahintersteckt....

aber zur realität wieder zurück. Ich habe ein viel einfacheres beispiel angefangen, das linienfolgemodul muss ja genutzt werden!

das hier gibt nur die werte der Vishay-sensoren aus. Trotz der enormen speed von 20links/20rechts geht das aber so schnell, dass ich die zeile mit dem move befehl lieber auskommentiert habe und schiebe und drehe von hand um zu erkennen, was da passiert:


while(true)
{
// moveAtSpeed(20, 20);

setCursorPosLCD(0, 0);
writeStringLCD(" li mi re ");
setCursorPosLCD(1, 0);
lfs_l = getLFS(CH_LFS_L);
writeIntegerLCD(lfs_l, DEC);
setCursorPosLCD(1, 8);
lfs_m = getLFS(CH_LFS_M);
writeIntegerLCD(lfs_m, DEC);
setCursorPosLCD(1, 16);
lfs_r = getLFS(CH_LFS_R);
writeIntegerLCD(lfs_r, DEC);
mSleep(500);

setCursorPosLCD(2, 0);
lfs_l = getLFS(CH_LFS_L);
writeIntegerLCD(lfs_l, DEC);
setCursorPosLCD(2, 8);
lfs_m = getLFS(CH_LFS_M);
writeIntegerLCD(lfs_m, DEC);
setCursorPosLCD(2, 16);
lfs_r = getLFS(CH_LFS_R);
writeIntegerLCD(lfs_r, DEC);
mSleep(500);

setCursorPosLCD(3, 0);
lfs_l = getLFS(CH_LFS_L);
writeIntegerLCD(lfs_l, DEC);
setCursorPosLCD(3, 8);
lfs_m = getLFS(CH_LFS_M);
writeIntegerLCD(lfs_m, DEC);
setCursorPosLCD(3, 16);
lfs_r = getLFS(CH_LFS_R);
writeIntegerLCD(lfs_r, DEC);
mSleep(500);

clearLCD();

setStopwatch1(0);
}


das hier geht schon weiter. mit zwei aufeinanderfolgenden if-abfragen der lfs-werte, deren vergleich - letztendlich kommt es ja "nur" drauf an, die differenz möglichst klein zu halten (abgestuft wäre natürlich besser!)....


while(true)
{
if(getStopwatch3() > 500) // 0.5s
{
moveAtSpeed(20, 20);

lfs_l = getLFS(CH_LFS_L);
lfs_m = getLFS(CH_LFS_M);
lfs_r = getLFS(CH_LFS_R);

clearLCD();
// for (i = 0; i < 10; i++)
// {
if(lfs_r > lfs_l)
{
lfs_l = getLFS(CH_LFS_L);
lfs_m = getLFS(CH_LFS_M);
lfs_r = getLFS(CH_LFS_R);

if(lfs_r > lfs_l)
{
moveAtSpeed(00, 40);
setbumperLEDR(1);
// mSleep(100); // Left bumper LED on
setbumperLEDL(0);
}
}

if(lfs_l > lfs_r)
{
lfs_l = getLFS(CH_LFS_L);
lfs_m = getLFS(CH_LFS_M);
lfs_r = getLFS(CH_LFS_R);

if(lfs_l > lfs_r)
{
moveAtSpeed(40, 00);
setbumperLEDL(1); // right bumper LED on
// mSleep(100);
setbumperLEDR(0);
}
}
// }
setStopwatch3(0);

}
}

ich bin nicht der typ, der für sich allein herumtüftelt, ich brauche eine "umgebung" und austausch, es gibt ja hier ein paar, die das liniensuchboard der multi IO haben, hätte da jemand lust (und zeit)?

in dem zweiten beispiel frage ich z.b. auch den wert des mittleren sensors ab, benutze aber für das abfahren der linie (http://www.youtube.com/watch?v=R0Ego5qMcBI&feature=youtu.be) nur die beiden äußeren. Würde sicher sinn machen den mittleren auch einzubinden, hat das z.b. schon jemand gemacht? Wie wäre die überlegung für die realisierung?

[inka]

hallo,

frage:

das hier:

inline void MotorSpeed (
unsigned char left_speed,
unsigned char right_speed)
{
OCR1A = left_speed;
OCR1B = right_speed;
}

lässt sich ja durch moveAtSpeed() ersetzen...

gäbe es auch eine ersatz dafür hier?

inline void MotorDir (
unsigned char left_dir,
unsigned char right_dir)
{
PORTD = (PORTD &~ ((1 << PD4) | (1 << PD5))) | left_dir;
PORTB = (PORTB &~ ((1 << PB4) | (1 << PB5))) | right_dir;
}

in den Movement functions der RP6Control_I2CMasterLib.c fand ich nichts, was so auf anhieb passen würde, entweder mit richtungswechsel für beide motoren gleichzeitig oder eben move bzw. rotate passen nicht so richtig....

wie könnte ich das angehen? Oder gibt es außer den hier erwähnten bewegungsfunktionen noch andere?

[Dirk]

Hi inka,
was hast du vor?
Mit changeDirection(dir) kann man ja mit dem Parameter dir in alle Richtungen (FWD, BWD, LEFT, RIGHT) steuern.
Klappt das damit nicht?

[inka]

Hi Dirk,

was ich vorhabe? Den versuch den code aus meinem ersten post für den RP6 "auferstehen" zu lassen. Die ganzen berechnungen so zu lassen, wie sie sind und die asuro-funktionen durch ihre RP6 äquivalente zu ersetzten und schauen, was der RP6 damit tut...

dabei ist noch die funktion

inline void MotorDir ( unsigned char left_dir, unsigned char right_dir)
{
PORTD = (PORTD &~ ((1 << PD4) | (1 << PD5))) | left_dir;
PORTB = (PORTB &~ ((1 << PB4) | (1 << PB5))) | right_dir;
}

aus der asuro-lib motor_low.c geblieben, die - so wie ich sie verstehe - die drehrichtung der beiden motoren unabhängig voneinander und auch noch in der größe unterschiedlich, abhängig von der variablen verändert. Meine frage war, ob ich das so richtig sehe und wie ich das nachbilden kann.

hier noch die motor_low.c zum verständnis:


/************************************************** **************************/
/*!
\file motor_low.c

\brief Low Level Funktionen zur Steuerung der Motoren.

Die Motorsteuerung erfolgt grundsaetzlich ueber die auf der Asuro-Platine\n
aufgebauten H-Bruecken. Dies ist eine Schaltung, ueber die ein Strom in\n
verschiedene Richtungen durch die Motoren geleitet werden kann.\n
Zur Geschwindigkeitssteuerung werden die beiden im Prozessor vorhandenen\n
PWM-Kanaele genutzt, deren Ausgangssignale die Staerke des Stromflusses in\n
den H-Bruecken beinflusst.\n
Die Initialisierung der PWM-Funktionalitaet erfolgt in der Funktion Init().

\see Defines fuer die Auswahl der ADC-Kanaele in asuro.h\n
FWD, RWD, BREAK, FREE

\version V--- - 10.11.2003 - Jan Grewe - DLR\n
Original Version von der ASURO CD\n
\version V--- - bis zum 07.01.2007 - \n
Bitte in Datei CHANGELOG nachsehen.\n
\version V001 - 13.01.2007 - m.a.r.v.i.n\n
+++ Alle Funktionen\n
Zerlegte Sourcen in einzelne Dateien fuer eine echte Library.
\version V002 - 05.02.2007 - Sternthaler\n
+++ Alle Funktionen\n
Kommentierte Version (KEINE Funktionsaenderung)
\version V003 - 18.02.2007 - m.a.r.v.i.n\n
Datei gesplitted in motor_low.c und motor.c
************************************************** ***************************/
/************************************************** ***************************
* *
* This program is free software; you can redistribute it and/or modify *
* it under the terms of the GNU General Public License as published by *
* the Free Software Foundation; either version 2 of the License, or *
* any later version. *
* *
************************************************** ***************************/
#include "asuro.h"



/************************************************** **************************/
/*!
\brief
Steuert die Geschwindigkeit der Motoren.

\param[in]
left_speed Geschwindigkeit linker Motor (Bereich 0..255)
\param[in]
right_speed Geschwindigkeit rechter Motor (Bereich 0..255)

\return
nichts

\see
Die Initialisierung der PWM-Funktionalitaet erfolgt in der Funktion Init().

\par Hinweis:
Diese Funktion ist als 'inline'-Funktion definiert.

\par Arbeitsweise:
Ueber die Parameter werden die beiden Kanaele der PWM-Funktionalitaet im\n
Prozessor angesteuert. Diese Art der Geschwindigkeitsreglung beruht darauf,\n
dass ein digitaler Output-Pin in schneller Folge an- und ausgeschaltet wird.\n
Mit dem Parameter wird nun gesteuert wie \b lange der Strom im \b Verhaeltniss \n
zur Zykluszeit \b angeschaltet ist.\n
Wird z.B. ein Wert von 150 fuer einen Parameter uebergeben, dann wird fuer\n
150 / 255-tel der Zykluszeit der Port auf 1 geschaltet und somit ist die\n
Motorleistung entsprechend reduziert.\n
Daraus ergibt sich auch dass der Asuro \b noch \b nicht bei einem Wert von\n
20 fahren wird, da diese Leistung nicht ausreicht ihn 'anzuschieben'.\n
(PWM = Pulsweitenmodulation)

\par Beispiel:
(Nur zur Demonstration der Parameter/Returnwerte)
\code
// Setzt die Geschwindigkeit fuer den linken Motor
// auf 150 und stoppt den rechten Motor.
MotorSpeed (150, 0);
\endcode
************************************************** ***************************/
inline void MotorSpeed (
unsigned char left_speed,
unsigned char right_speed)
{
OCR1A = left_speed;
OCR1B = right_speed;
}



/************************************************** **************************/
/*!
\brief
Steuert die Drehrichtung der Motoren.

\param[in]
left_dir Richtung des linken Motors [ FWD | RWD | BREAK | FREE ]
\param[in]
right_dir Richtung des rechten Motors [ FWD | RWD | BREAK | FREE ]

\return
nichts

\par Hinweis:
Diese Funktion ist als 'inline'-Funktion definiert.

\par Arbeitsweise:
Ueber die Parameter werden die Port-Pin's zu den H-Bruecken beider Motoren so\n
gesetzt, dass die jeweils 4 beteiligten Transitoren einer Bruecke den Strom\n
durch die Motoren entweder
- FWD vorwaerts durchleiten
- RWD rueckwaerts durchleiten
- BREAK den Motor kurzschliessen (Bremswirkung)
- FREE oder von der Stromversorgung trennen (Motor laeuft aus)

\par Beispiel:
(Nur zur Demonstration der Parameter/Returnwerte)
\code
// Setze die Richtung fuer den rechten Motor auf Rueckwaerts
// und blockiert den linken Motor.
MotorDir (BREAK, RWD);
\endcode
************************************************** ***************************/
inline void MotorDir (
unsigned char left_dir,
unsigned char right_dir)
{
PORTD = (PORTD &~ ((1 << PD4) | (1 << PD5))) | left_dir;
PORTB = (PORTB &~ ((1 << PB4) | (1 << PB5))) | right_dir;
}

[Dirk]

Hi inka,

das Problem ist nicht, dass es das nicht gibt (in der RP6RobotBaseLib):
setMotorPower(uint8_t left_power, uint8_t right_power)
setMotorDir(uint8_t left_dir, uint8_t right_dir)

Das Problem ist, dass der I2C-Slave diese Funktionen so nicht kennt.
Aber: Du hast ja auch schon die direkte IR-Empfänger-Abfrage in den Slave aufgenommen,- warum nicht auch diese 2 kleinen neuen Funktionen?

[inka]

Hi Dirk,
ich bin keineswegs sicher, aber für die setMotorDir funktion würde ich folgendes ändern:

in der RP6Control_I2CMasterLib.c einfügen:

// Direction
uint8_t left_dir;
uint8_t right_dir;

-------------------------------

/**
* Set Motor dir function
*/

{
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;

[Dirk]

Ja, sieht doch gut aus.
Kleine Änderung in der "Set Motor dir function":

/**
* 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();
}

P.S.: Vorsicht mit den Funktionen, die die Motorrichtung und/oder Speed direkt schalten: Man kann (z.B. wenn man bei voll Speed vorwärts auf voll Speed rückwärts umschaltet) den RP6 evtl. schrotten!

[inka]

Hi Dirk,

Ja, sieht doch gut aus.
ok, danke, das void habe ich übersehen, mich wunderts sowieso, dass da so weing falsch war :-). Also mache ich mich an die zweite funktion...



Vorsicht mit den Funktionen, die die Motorrichtung und/oder Speed direkt schalten: Man kann (z.B. wenn man bei voll Speed vorwärts auf voll Speed rückwärts umschaltet) den RP6 evtl. schrotten!
wie ist das zu vermeiden? Oder ist das so, dass das risiko nur mit steigender speed auch steigt?

Eine andere idee: solche änderungen am funktionsumfang der I2C_slave (und zusammenhängende änderungen an der RP6Control_I2CMasterLib) wie die IR geschichte, oder jetzt die motorsachen, wäre es nicht sinnvoll das irgendwo "offiziell", also z.b. im wiki für die allgemeinheit zu sammeln? Ich wüsste selber nicht, ob ich heute noch alle infos zu der IR sache finden würde, müsste schon mehr suchen, als wenn es irgendwo zusammengefasst wäre...

[Dirk]

Hi inka,

wie ist das zu vermeiden? Oder ist das so, dass das risiko nur mit steigender speed auch steigt?
In der Original-RP6-Lib wird die Speed nur mit "Rampen" verändert und z.B. bei Richtungswechsel erst die Speed reduziert, dann die Richtung geändert und wieder in die andere Richtung beschleunigt.
Wenn man ohne diese "Sicherheitsfunktionen" arbeiten will, dann muss man sich eigene Kontroll-Regeln einbauen.
Z.B.: Schalte die Drehrichtung eines Motors nur um, wenn die Speed des Motors < 40 ist. Wenn der Unterschied zwischen neuer (gewünschter) Speed und aktueller Speed > 30 ist, dann begrenze die neue Speed auf (aktuelle Speed + 30). Usw.


solche änderungen am funktionsumfang der I2C_slave (und zusammenhängende änderungen an der RP6Control_I2CMasterLib) wie die IR geschichte, oder jetzt die motorsachen, wäre es nicht sinnvoll das irgendwo "offiziell", also z.b. im wiki für die allgemeinheit zu sammeln? Ich wüsste selber nicht, ob ich heute noch alle infos zu der IR sache finden würde, müsste schon mehr suchen, als wenn es irgendwo zusammengefasst wäre...
Ja, da wäre ich sehr dafür!
Meine (bescheidene) Vorstellung:
Dein aktuelles Linienfolge-Programm gehört ja eigentlich zu deinem großen Projekt "RP6 sucht autonom eine induktive Ladestation auf".
Ich fände es absolut klasse, wenn das ganze Projekt zu einem eigenen Wiki-Artikel würde. Du hast dich da mit so viel Energie den Hardware- und Software-Fragen gestellt, dass das ein "Lehrstück" für Viele sein könnte (das Interesse zeigt sich ja z.B. durch die große Zahl der Aufrufe deiner Themen!).

[inka]

Hi Dirk,

das Problem ist nicht, dass es das nicht gibt (in der RP6RobotBaseLib):
setMotorPower(uint8_t left_power, uint8_t right_power)
setMotorDir(uint8_t left_dir, uint8_t right_dir)

bist Du sicher, dass es sinnvoll ist anstelle der asuro-funktion:


inline void MotorSpeed ( unsigned char left_speed, unsigned char right_speed)
{
OCR1A = left_speed;
OCR1B = right_speed;
}

die RP6-funktion:


void setMotorPower(uint8_t left_power, uint8_t right_power)
{
if(left_power > 210) left_power = 210;
if(right_power > 210) right_power = 210;
mright_power = right_power;
mleft_power = left_power;
}

zu verwenden, statt das "moveAtSpeed()" direkt (und sichrere) zu verwenden?

das wird ja in der "RP6RobotBaseLib" sogar vorgeschlagen:
-------------------------------------------------------------
* IT IS A BETTER IDEA NOT TO USE THIS FUNCTION AT ALL!
* Use moveAtSpeed together with task_motionControl instead.
* YOU CAN NOT USE setMotorPower AND setMotorDir WHEN YOU USE
* task_motionControl! This will not work!
* -------------------------------------------------------------

oder geht es aus irgendwelchen anderen gründen nicht?

[Dirk]

Hi inka,

bist Du sicher, dass es sinnvoll ist anstelle der asuro-funktion:
...
die RP6-funktion:
...
zu verwenden, statt das "moveAtSpeed()" direkt (und sichrere) zu verwenden?

Die RP6-Lib Funktionen sind auf jeden Fall sicherer.
Leider haben sie den Nachteil beim Linienfolgen, dass die Reaktion auf Änderungen der Richtung oder Geschwindigkeit etwas verzögert kommt.
Da ist man dann manchmal schon über die Linie hinaus geschossen. ;)
Das ist sicher auch der Grund, warum in dem Asuro-Programm der direkte Weg gewählt wurde.

[inka]

hallo allerseits,

auch hier kleine fortschritte. Habe das programm für den asuro mit PID-regler (https://www.roboternetz.de/community/threads/10604-Asuro-Linienfolger-mit-PD-Regler/page15) 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:


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


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

/************************************************** *****************************
*
* 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_transmissio nError);

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,
(http://www.youtube.com/watch?v=DsHUjMfyGo0&feature=youtu.be)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...

[RolfD]

@Inka
Wenn ich das richtig sehe, postet ihr da verschiedenen Quellcode.. der eine was von der Base/I2C-Slave, der andere was von der I2C-Master lib?
Grundsätzlich gilt, es gibt Funktionen die die IO-Ports direkt steuern (böse/schnell) und es gibt welche die Rampensteuerung erledigen (gut/lahm). Letztere laufen auch als Task. Die Rampensteuerung auf der Base ist auf Grund des Trägheitsmomentes von Zahnrädern notwendig damit es kein Getriebesalat gibt.
Die I2C-Master Lib muss sich um die Details wie Rampen jedoch eigentlich nicht kümmern wenn sie Fahranweisungen gibt... da sich schon die Base/Slave darum kümmert. Die I2C-Master lib kann eh kein Port der Base direkt ansteuern. Baut man sich ein Linienfolger, lässt man die Software dafür besser auf der Base laufen - schon wegen der trägen Laufzeiten auf dem I2C Bus sammt en/decodierung der Motorbefehle und Übertragungsfehler... und verwendet die Base ports ADC0/1 für den Sensorabgriff. Alles andere ist von hinten durch die Brust ins Auge...
Gruß

[inka]

@RolfD

keine grundsätze ohne ausnahmen :-)

danke für die erklärung, ich bin mir der problematik mit dem getriebe durchaus bewusst, es kommt mir nicht auf schnelligkeit beim folgen der linie an, es hat sich aber gezeigt, dass der lauf bei diesem code sehr viel gleimäßiger als bei anderen meinen versuchen ist...

der linienfolger hier läuft später nicht allein, sondern ist bestandteil meiner induktiven ladestation (https://www.roboternetz.de/community/threads/62934-RP6-Projekt-induktive-Ladestation) und der suche danach. Die suche beginnt mit der groben orientierung nach einer IR-bake (https://www.roboternetz.de/community/threads/63467-IR-bake) und der annäherung ans ziel, dann übernimmt der linienfolger und es geht wohl nicht - zumindest meines wissens nach - , dass innerhalb eines programms zwischen m32 und der base ohne weiteres gewechselt wird wo was läuft und wer den hut auf hat, oder?

[RolfD]

Hallo Inka,
türlich geht das mit dem Hut wechseln... nur müsste die m32 ständig die Messwerte der Sensoren per i2c an die Base geben und die Base daraus Fahrbefehle basteln.
Es wäre auch denkbar, das man (die m32) der Base per i2c Modus Befehl sagt "fahr du die nächsten Meter aber achte auf die Sensorwerte auf dem i2c Bus".
Dazu müsste einiges an der base lib bzw. i2c slave umgeschrieben werden aber letztendlich bestimmst doch du, was die beiden miteinander aushandeln.
Immerhin sind das 2 unabhängige Prozessoren, und das verhalten dazwischen ist umsetzbar von master/slave über preemptiver Prozess und Steuerungskontrolle und noch komplizierteren Verfahren.
Da z.B. die m32 nichts von den Bumpern mitkriegt, diese aber mit in das Fahrmodel mit einfließen.. bzw. die Bumper die Fahrsteuerung beeinflussen, hast du eh schon eine Art "verteilte Intelligenz".
Das lässt sich auch gescheit ausbauen.
Gruß

[inka]

hallo RolfD,

dann habe ich es falsch verstanden...
ich habe mich schon gewundert dass in der base nur das slave programm läuft und sonst nix. Durch verschiedene änderungen die ich im laufe dieses projekts an der "RP6Base_I2CSlave.c" und den I2c dateien machen musste bröckelt diese "mauer" etwas.

Momentan möchte ich beim linien-folger aber erstmal alles so lassen wie es ist, es funktioniert immerhin...

Ich habe mich mit verschiedenen sachen sowieso schon ziemlich verzettelt, jetzt sollte das projekt fertig werden. Da habe ich noch die baustelle mit der IR-bake, das einmal (eigentlich mehrmals) gelungene und gefilmte auffinden der bake (http://www.youtube.com/watch?v=HqvyWI0y8Z0&feature=youtu.be) war wohl mehr oder weniger zufall. Zumindest gelingt es jetzt nicht mehr das zu wiederholen, es gibt dort zeit- bzw. koordinationsprobleme zwischen dem senden der bake und dem empfang bei RP6...

[RolfD]

Das master/slave bezieht sich nur auf das i2c Protokoll... es steht nirgends, das ein (i2c)Slave strohdumm sein muss oder der Master alles erledigt. Natürlich kann (und soll) der Slave um funktionen erweitert werden die der Master z.B. nur "anstubst" oder die beiden sich sonst wie koordinieren.
Das is wie beim Menschen... das Großhirn ist auch nicht für Puls, Refelxe und körperinterne Abläufe zuständig. Das macht das Kleinhirn und noch kleinteiliger das Rückenmark. Aber es gibt auch da definierte Aufgaben und Protokolle dazwischen, welche den Ablauf steuern. In beide Richtungen und je nach Situation auch autark. Wenn Du mit dem Dicken Zeh wacklen willst, musst du dich nicht damit beschäftigen welche Muskeln dafür notwendig sind.. du wackelst einfach..den Rest macht dein Rückenmark. Und dein Gehirn bestimmt auch nicht den Neigungswinkel des Zehs wenn du gegen den Bettpfosten läufst... es tut einfach nur weh! Das sind alles einfache Signale welche dezentralisiert verarbeitet werden. Das kannst du auch mit der Base und einem m32 nachstellen. Wichtig dabei ist nur, das du auf beiden CPUs die Multitaskineigenschaften nicht blockierst. Auch da ein Beispiel: Wenn du einen im Tee hast, läufst du auch nicht mehr gradeaus :D
Deswegen muss man aber halt auch Laufzeitprobleme beachten, denn wenn der Slave nur wie blöde den i2c Bus oder ein Sensor bewacht und man selbst noch eigene Funktionen einbaut die nicht Task-konform sind, kann er kaum noch andere Aufgaben umzusetzen. Ich weis jetzt nicht im Detail wo das bei Dir der Fall sein könnte aber wenn der Bot irgendwo in einer Schleife hängt, werden Fahrbefehle zeitlich falsch abgearbeitet. Das sind eben keine Programmierfehler in dem Sinn, sondern eher logische oder konzeptionelle Fehler. Meiner Erfahrung nach am ehesten die Quelle für so Probleme wie "der bot macht nicht mehr was er soll obwohl ich an der Stelle nichts verändert habe."
Aber das mit dem verzetteln kenn ich auch zu gut.. ich drück mal weiter Däumchen für dein Projekt.
Gruß