Edu_drive_ros2 umschreiben für Lenkung mit 6 Servos

Hi ,
wir sind dabei einen Roboter zu bauen, der 6 Räder hat. An jedem Rad ist eine Servo zum Einschlagen vom Rad, damit wir nach links bzw rechts linear fahren können, Beim rotieren müssen wir aber die Räder bestimmt einschlagen und auch rotieren nun ist das Problem, dass die inneren langsamer fahren müssen als die Äußeren und dass dadurch das wir die Seros nicht um 360° schwenken können auch der Gegenwinkel eingeschlagen werden muss und der Motor dann in die umgekehrte Richtung rotieren muss. Ich habe dafür auch schon Code geschrieben um die Servos zu steuern bloss ich weiß nicht wo ich in edu_drive_ros2 Hand anlegen muss. Hier mein Code zum Berechnen der Motorgeschwindigkeiten ich bin davon mal ausgegangen dass 1 vorwärts Vollgas ist und -1 rückwärst Vollgas ist:

` // angular movement
const float pi = 3.1415926535897932385;
float sr = Rwidth / 2;
float br = sqrtf(Rwheeldistance * Rwheeldistance + (Rwidth/2) * (Rwidth/2));
float beta = atanf(sr/ Rwheeldistance);
// angulars
// 0 = left front; 1 = right front; 2 = left middle; 3 = right middle; 4 = left back; 5 = right back;
float betaServos[6];
betaServos[0] = beta;
betaServos[1] = 2 * pi - beta;
betaServos[2] = 0.5 * pi;
betaServos[3] = 2 * pi - betaServos[2];
betaServos[4] = 2pi - betaServos[0];
betaServos[5] = 2
pi - betaServos[1];
// velocity
float velocityMotors[6];
velocityMotors[0] = fabsf(msg.angular.z);
velocityMotors[1] = velocityMotors[0];
velocityMotors[2] = (sr / br) * velocityMotors[1];
velocityMotors[3] = velocityMotors[2];
velocityMotors[4] = velocityMotors[1];
velocityMotors[5] = velocityMotors[0];

// Morph gamma and beta
// Delta is gamma and betas morphed
float dServos[6];
// End velocity
float endVelocity[6];

float x[6];

float y[6];

if (msg.angular.z != 0)
{
    for (int i = 0; i < 6; i++)
    {
        x[i] = cosf(betaServos[i]) * velocityMotors[i]  * ((msg.angular.z > 0) ? 1.0f : -1.0f) + msg.linear.x ;
        y[i] = sinf(betaServos[i]) * velocityMotors[i]  * ((msg.angular.z > 0) ? 1.0f : -1.0f) + msg.linear.y ;
        endVelocity[i] = sqrtf(y[i] * y[i] + x[i] * x[i]);
    }
}
else
{
    for (int i = 0; i < 6; i++)
    {
        x[i] = msg.linear.x;
        y[i] = msg.linear.y;
        endVelocity[i] = sqrtf(y[i] * y[i] + x[i] * x[i]);
    }
}

// final movement
float alpha[6];

for(int i =0;i<6;i++){
        if(x[i]>0 && y[i]>0){
            alpha[i]= atanf(fabsf(y[i])/fabsf(x[i]));
        }else if(x[i]<0 && y[i]>0){
            alpha[i]=pi - atanf(fabsf(y[i])/fabsf(x[i]));
        }else if(x[i]<0 && y[i]<0){
            alpha[i]=pi + atanf(fabsf(y[i])/fabsf(x[i]));
        }else if(x[i]>0 && y[i]<0){
            alpha[i]= 2*pi - atanf(fabsf(y[i])/fabsf(x[i]));
        }
        // converting for Servos and convert Wheel direction
        if(alpha[i]> pi){
            alpha[i] -=pi;
            endVelocity[i] *= -1;
        }
    }`

Hallo Jonas,
die Umrechnung von Twist-Größen in Raddrehzahlen wird über die Kinematikmatrix gemacht. Im Prinzip sind das drei Umrechnungsfaktoren, die vx, vy und omega in die Raddrehzahlen omega_i umrechnen. Eingestellt werden diese Werte in der Parameter-Datei (YAML). In Eurem Fall ändern sich diese Werte aber in Abhängigkeit der Servoposition, d.h. Ihr müsst diese Werte zur Laufzeit anpassen. Dazu ist wahrscheinlich eine Änderung der Klasse EduDrive notwendig. Entweder Ihr speist irgendwie diesen Winkel ein und berechnet in der Methode controlMotors die passenden Faktoren oder Ihr übergebt die Faktoren an eine neue Methode (z.B. setKinematics). Seht doch bitte mal die Zeilen 221 bis 236 des Quellcodes in folgender Datei an: edu_drive_ros2/src/EduDrive.cpp at master · EduArt-Robotik/edu_drive_ros2 · GitHub
Da muss das irgendwie beachtet werden.
Viele Grüße
Stefan May