Modelle aus Baukasten und Buch Fischertechnik-Roboter mit Arduino

[Dirk Fox]

Hallo Claus,
ein wunderschönes Modell...
Das glaube ich Dir sofort, dass mit den Schrittmotoren "die Post" abgeht.
Herzlicher Gruß
Dirk

[schröttel]

Hallo Dirk,
danke für die Blume, daß Lob kann ich nur zurückgeben. Ich habe ja nur von euch abgekupfert.

[geometer]

Mir ist jetzt schon öfter die Hand in das Gestell oder gegen den Boden gefahren.

Das kenne ich!

Wie könnte man sinnvoll den Arbeitsraum einschränken, damit es nicht mehr zu Kollisionen kommen kann?

Ich habe damals keine Einschränkungen vorgenommen. Bei den Spielen (Solitär, Tic-Tac-Toe) rechnen wir die Potentiometerdaten ja überhaupt nicht in kartesischen Koordinaten um. Die Winkelkoordinaten eignen sich nicht, um passende Ungleichungen anzugeben. Wenn man aber eine Anwendung hat, bei der man kartesische Koordinaten verwendet, ist es sicher eine gute Idee, die vertikale Bewegung einzuschränken.

Wenn Du den Delta mit Schrittmotoren zum Laufen bekommen hast, hast Du schon viel geleistet! Die Schrittmotoren sind natürlich viel besser, um die Hand von einer Position zur anderen geradlinig zu führen. Das ist mit den S-Motoren aufgrund der nicht garantierten gleichen Drehzahl bei gleicher Spannung viel schwieriger. Andererseits musst Du mit Schrittmotoren die Positionen anders anlernen. Wie machst Du das denn?

Und zum Schluss: Woher hast Du so viele schicke schwarze Schneckenmuttern?

[juh]

Und zum Schluss: Woher hast Du so viele schicke schwarze Schneckenmuttern?

Ich schätze mal daher, wo auch die anderen schwarzen Teile herkommen.

vg
Jan

[schröttel]

Und zum Schluss: Woher hast Du so viele schicke schwarze Schneckenmuttern?

Jan hat recht, die Schneckenmuttern sind aus meinem 3D-Drucker gefallen.

Andererseits musst Du mit Schrittmotoren die Positionen anders anlernen. Wie machst Du das denn?

Als erstes fahren die Arme gegen den oberen Anschlag, dazu sind Endtaster montiert. Diese Position ist die Ausgangslage. Die Schrittzähler der Motoren werden auf 0 gesetzt. Alle Bewegungskommandos beziehen sich absolut auf diesen Punkt. Gleichzeitig ist dort auch der Bezugspunkt für die Berechnung der zu fahrenden Schritte. Dazu wurde der Oberarmwinkel am oberen Anschlag als Konstante hinterlegt.
Wird nun eine neue anzufahrende Koordinate vorgegeben werden folgende Schritte ausgeführt:
1. Berechnung der neuen Oberarmwinkel mit Deiner Funktion "hand_winkel".
2. Berechnung der Schritte aus den Oberarmwinkeln in der Funktion "winkel_schritte".
Dazu wird einfach der Winkel mit der Anzahl Schritte pro Grad der Schrittmotoren multipliziert. Diese ergibt sich aus der bekannten
Anzahl Schritte pro Umdrehung der Motoren (200/Umdr.), Ansteuerung in 1/16 Schritten und der Übersetzung des Getriebes (4:1).
3. Anfahrt der neuen Position über die angepassten Funktionen "ansteuern" bzw. "gehezu".

In dem angefügten Programm sind alle benutzten Methoden enthalten. Es entspricht deinem Programm "Delta_Manipulator".

Code: Alles auswählen

#include <Wire.h>
#include <ArduinoNunchuk.h>
#include <ftPwrDrive.h>
#include <Ftduino.h>

#define magnet Ftduino::M4
#define magnet_plus Ftduino::RIGHT
#define magnet_minus Ftduino::LEFT
#define magnet_aus Ftduino::OFF

ArduinoNunchuk nunchuk = ArduinoNunchuk();
ftPwrDrive Drive = ftPwrDrive(32);

static const uint8_t M_All = 0b00000111;

long schritte_soll[3];
long schritte_ist[3];
float phi_soll[3]; // Winkelwerte
float y0, z0, x1, y1, z1, x2, y2, z2; // Ellenbogen
float x_soll, y_soll, z_soll;
float x_ziel, y_ziel, z_ziel;
float x = 0.0, y = 0.0, z = 91.90; // aktuelle Pos. vorbelegt mit oberen Anschlag


const float s = 40.0; // effektive Entfernung der Schultern vom Zentrum
const float l = 75.0; // Länge der Oberarme
const float L = 195.0; // Länge der Unterarme
const float h = 217.5;
const float phi_oben = 0.5547; // rad Winkel Oberarm am oberen Anschlag
const long v_max = 3000; // max. Schritte/Sek.

// mathematische Konstanten

const float sin120 = 0.866025;
const float cos120 = -0.5;
const float sin240 =-sin120;
const float cos240 = cos120;

byte center_x, center_y;
int center_accel_y;
unsigned long letzter_zButton, startzeit, dauer;
boolean magnetisch = false;

// Setup

void setup() {
  ftduino.init();
  Drive.setMicrostepMode(SIXTEENTHSTEP);
  nunchuk.init();
  delay(10);
  do {
    nunchuk.update();
    delay(20);
  } while (nunchuk.cButton == 0);
  center_x = nunchuk.analogX;
  center_y = nunchuk.analogY;
  center_accel_y = nunchuk.accelY;
  letzter_zButton = millis();
  dauer = 0;
  home(false);

}

void loop() {
  delay(5);
  nunchuk.update();
  x_ziel = 0.6*(nunchuk.analogX - center_x);
  y_ziel = 0.6*(nunchuk.analogY - center_y);
  z_ziel = 55.0 - 0.18*(nunchuk.accelY - center_accel_y);
  ansteuern();
  if (millis() > letzter_zButton + 500) {
    if (nunchuk.zButton != 0) {
      letzter_zButton = millis();
      if (magnetisch) {
        magnetisch = false;
        ftduino.motor_set(magnet, magnet_minus, Ftduino::MAX);
        delay(100);
        ftduino.motor_set(magnet, magnet_aus, Ftduino::MAX);

      }
      else {
        magnetisch = true;
        ftduino.motor_set(magnet, magnet_plus, Ftduino::MAX);
      }       
    }
  }
}


void home(byte disable) {
  Drive.setMaxSpeed(FTPWRDRIVE_M1, 500);
  Drive.setMaxSpeed(FTPWRDRIVE_M2, 500);
  Drive.setMaxSpeed(FTPWRDRIVE_M3, 500);
  Drive.homingOffset(FTPWRDRIVE_M1, 48);
  Drive.homingOffset(FTPWRDRIVE_M2, 48);
  Drive.homingOffset(FTPWRDRIVE_M3, 48);
  Drive.homing(FTPWRDRIVE_M1, 3000, disable);
  Drive.homing(FTPWRDRIVE_M2, 3000, disable);
  Drive.homing(FTPWRDRIVE_M3, 3000, disable);
  while (Drive.isHoming(FTPWRDRIVE_M1) || Drive.isHoming(FTPWRDRIVE_M2) || Drive.isHoming(FTPWRDRIVE_M3))
    ;
  Drive.setPosition(FTPWRDRIVE_M1, 0);
  Drive.setPosition(FTPWRDRIVE_M2, 0);
  Drive.setPosition(FTPWRDRIVE_M3, 0);

  schritte_ist[0] = 0;
  schritte_ist[1] = 0; 
  schritte_ist[2] = 0;

}
boolean ansteuern () {
  float abstand, dx, dy, dz;
  const float schrittweite = 3.0;
  boolean aktion = true;
  dx = x_ziel-x;
  dy = y_ziel-y;
  dz = z_ziel-z;
  abstand = sqrt( dx*dx + dy*dy + dz*dz );
  if (abstand > schrittweite) {
    x_soll = x + schrittweite*dx/abstand;
    y_soll = y + schrittweite*dy/abstand;
    z_soll = z + schrittweite*dz/abstand;
  }
  else {
    x_soll = x_ziel;  y_soll = y_ziel; z_soll = z_ziel;
    aktion = false;
  }
  hand_winkel();
  winkel_schritte();
  gehe_zu();
  x = x_soll;
  y = y_soll;
  z = z_soll;
  return (aktion);
}

void gehe_zu() {
  byte i;
  long v, diff;
  float d_max;

  d_max = 0;
 
  for ( i = 0; i < 3; i++) {
    diff = abs(schritte_ist[i] - schritte_soll[i]);
    if (diff > d_max) d_max = diff;
  }
 
  for ( i = 0; i < 3; i++) {
    if (schritte_ist[i] == schritte_soll[i]) v = v_max;
    else v = abs(schritte_ist[i] - schritte_soll[i]) * ( v_max / d_max);
    Drive.setMaxSpeed(FTPWRDRIVE_M[i], v);
    schritte_ist[i] = schritte_soll[i];
  }
  Drive.setAbsDistanceAll(schritte_soll[0], schritte_soll[1], schritte_soll[2], 0);
  Drive.startMovingAll(M_All, 0b00000000);
  while (Drive.isMoving(FTPWRDRIVE_M1) || Drive.isMoving(FTPWRDRIVE_M2) || Drive.isMoving(FTPWRDRIVE_M3))
    ;
}

void winkel_schritte() {
  for (byte i = 0; i < 3; i++) {
    schritte_soll[i] = -(( phi_oben - phi_soll[i]) * 2037.1879); //57.296 Rad -> Grad  * 35.5555 Steps/Grad bei 1/16 Steps
  }
}

void hand_winkel() {

  float a = x_soll*x_soll + (y_soll+s)*(y_soll+s) + (z_soll-h)*(z_soll-h) + l*l - L*L - 2*(y_soll+s)*l;
  float b = 4.0*(h-z_soll)*l;
  float c = a + 4.0*(y_soll+s)*l;
  float t = 0.5*(-b + sqrt(b*b-4.0*a*c))/a;
  phi_soll[0] = 2.0*atan(t);
  
  a = (x_soll-s*sin120)*(x_soll-s*sin120) + (y_soll+s*cos120)*(y_soll+s*cos120)
        + (z_soll-h)*(z_soll-h) + l*l - L*L - 2.0*(-x_soll*sin120+y_soll*cos120+s)*l;
  c = a + 4.0*(-x_soll*sin120+y_soll*cos120+s)*l;
  t = 0.5*(-b + sqrt(b*b-4.0*a*c))/a;
  phi_soll[1] = 2.0*atan(t);
  
  a = (x_soll-s*sin240)*(x_soll-s*sin240) + (y_soll+s*cos240)*(y_soll+s*cos240)
        + (z_soll-h)*(z_soll-h) + l*l - L*L - 2.0*(-x_soll*sin240+y_soll*cos240+s)*l;
  c = a + 4.0*(-x_soll*sin240+y_soll*cos240+s)*l;
  t = 0.5*(-b + sqrt(b*b-4.0*a*c))/a;
  phi_soll[2] = 2.0*atan(t);
}

[atzensepp]

Ich habe den Delta-Roboter aus dem Buch - in Ermangelung von 3 XS-Motoren - etwas abgewandelt und mit XM-Motoren ausgestattet.
Die Positionsmessung erfolgt über magnetische AS5600-Encoder.

Delta-Roboter mit XM-Motoren (noch ohne Winkel-Encoder)

Delta1.JPG (127.3 KiB) 238 mal betrachtet

Aufsicht

Delta2.JPG (150.79 KiB) 238 mal betrachtet

Motor-Befestigung (hier mit XM-Encodermotoren)

Delta3.JPG (115.51 KiB) 238 mal betrachtet

Motor-Befestigung mit Magnet-Encoder

Delta4.JPG (148.43 KiB) 238 mal betrachtet

Ich habe auch XM-Motoren mit Encodern ausprobiert. Die Regelung der drei Winkel erfolgt jeweils über geschachtelte PID-Regler:
Äußerer Regler: Position, Stellgröße (output) : Geschwindigkeit
Innerer Regler: Geschwindigkeit, Stellgröße (output): PWM-Wert

[calliope]

Der AS5600-Encoder braucht doch Magneten am beweglichen Teil? Welche Magneten sind geeignet und wie baue ich die an?

[atzensepp]

Als Mangete, verwende ich 4mm-Zylindermagneten (fix-o-moll) , die ich in die Rastadapter 36227 stecke.
Hier mal ein paar Bilder dazu:

Delta5.JPG (110.65 KiB) 87 mal betrachtet

Delta6.JPG (109.79 KiB) 87 mal betrachtet