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);
}