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] = 2pi - 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;
}
}`