Hochregalrobo mit Arduino steuern

[Bernd1104]

Hallo an alle,
ich bin ein neuling im Bereich der Arduino Steuerung und Fischertechnik.
Mein Projekt, ein Fischertechnik Hochregal Robo mit einem Arduino Uno und einen Adafruit Motor Shield V2 steuern.

Nun suche ich jemanden der Erfahrung mit der Hartware ( Anschluß am Arduino und Motor Shield Pinbelegung usw.) und Software Programierung ( Endcoder auslesen, Endschalter usw. ).

Nach viel suchen in verschiedenen Foren hat mich fast schon der Mut verlasse, da steht zwar viel aber so richtig weiter hilft mir das nicht.

Wenn jemand Interesse hat mit mir das Projekt in kleinen Schritten Aufzubauen dann bitte melden.

Gruß Bernd

[bummtschick]

Hallo Bernd,

prinzipiell ist es nicht schwer, einen ft-Motor an den Adafruit-Motor-Shield V2 anzuschließen und ein- und auszuschalten. Die größere Herausforderung liegt darin, in einem Interrupthandler die Encodersignale auszulesen und damit die Position des Gliedes zu errechnen, das vom Motor bewegt wird.

Ich habe mich da selbst vor zwei Jahren reingebissen und anschließend mehrere Roboterarme gebaut. Bilder hier http://www.ftcommunity.de/categories.php?cat_id=2844 (und in den Unterordnern).

Ich würde Dir empfehlen, erst einmal diesen Artikel zu lesen: https://shop.heise.de/katalog/arduino-s ... ik-modelle -- Kostet ca. €3, aber das Geld ist gut investiert. Der erläutert Schritt für Schritt, wie man das zusammen bekommt, damit habe ich auch angefangen!

Wenn der Motor auf diese Weise läuft und Dein Arduino die Encodersignale mitzählen kann, kann man als nächsten Schritt darauf aufbauend Code schreiben, der nach einer Kalibrierung die Motorposition errechnet.

Herzliche Grüße
bummtschick

[Bernd1104]

Hallo bummtschick,

danke für deine Antwort ich habe das Heft gelesen aber dort wird ein Arduino Mega verwendet und ich habe einen Uno und der hat leider nur zwei Interupt Pin´s.
Dort wird auch mit einer anderen Softwareumgebung gearbeitet.
Den Sketch zu dem Heft habe ich aber nicht gefunden um überhaupt mal zu sehen wie dieser aufgebaut ist.
Ich habe aber auch gelesen das es mit einem Uno auch geht.
Nun ja du hast sicherlich recht das ich erst mal versuchen sollte einem Motor zu steuern.

Gruß Bernd

[bummtschick]

Du kannst auch mit einem Uno mehr als zwei Interrupts verwalten. Die zwei Interrupts sind die sog. "external interrupts", die mit den Arduino-Funktionen leicht zu bedienen sind. Daneben gibt es noch sog. "pin change interrupts", die sind allerdings etwas komplizierter zu steuern, wenn ich mich richtig erinnere. Ich muss zuhause mal in meinem Code nachschauen.

Weiteres schon mal hier: http://playground.arduino.cc/Code/Interrupts

Gruß bummtschick

[Bernd1104]

Ich habe mir einen Arduino Mega bestellt, ich glaube damit ist es erst mal einfacher das von Make beschriben Projekt umzusetzen.

Danke für die Info.

Werde bei kleinen Erfolgen und sicherlich auch bei großen was dazu schreiben.

Gruß Bernd

[Bernd1104]

Hallo bummtschick,

kannst du mir sagen was an dem Folgenden Sketch falsch ist ? Eine fehlermeldung bekomme ich nicht aber laufen tut er auch nicht!!!!!
Arduino Mega 2560 und Adafruit MotorShield V2

Code: Alles auswählen

// Arduino steuert Fischertechnik
// Versuchs Sketch 01.02.2016
// Bernd Hendrich

#include <Wire.h>                                       // zum ansprechen der I2C:
#include <Adafruit_MotorShield.h>                       // zum ansprechen des Motor Shields:
#include "utility/Adafruit_PWMServoDriver.h"            // zum ansprechen des Motor Shields:

Adafruit_MotorShield AFMS = Adafruit_MotorShield();     // Erzeugt das MotorShield Objekt:

Adafruit_DCMotor *myMotor0 = AFMS.getMotor(1);          // Gleichstrom Motor an M1:
Adafruit_DCMotor *myMotor1 = AFMS.getMotor(2);          // Gleichstrom Motor an M2:
Adafruit_DCMotor *myMotor2 = AFMS.getMotor(3);          // Gleichstrom Motor an M3:
Adafruit_DCMotor *myMotor3 = AFMS.getMotor(4);          // Gleichstrom Motor an M4:


int bufferCount;    // Anzahl der eingelesenen Zeichen
char buffer[80];    // Serial Input-Buffer

long debounceDelay = 50;    // the debounce time; increase if the output flickers

////////////////////////////////////////////////////////////////////////////////
// The default maximum values for the Encoders must be determined for each Robot
// Please be careful and have the finger on the reset button when testing
const int defaultMax[] = { 2286, 3838, 2375, 587};
//
////////////////////////////////////////////////////////////////////////////////

const int latchPin[] = { 30, 28, 26, 24};
const int encdrPin[] = { 21, 20, 19, 18};
int latchState[4];
int lastlState[] = { HIGH, HIGH, HIGH, HIGH};
long lastDTime[] = { 0, 0, 0, 0};
volatile int targetPr[] = { 0, 0, 0, 0};
volatile int maxEncdr[] = { 0, 0, 0, 0};
volatile int encdrPos[] = { 0, 0, 0, 0};
volatile boolean encdrSet[] = { 0, 0, 0, 0};
volatile boolean blocked = false;
volatile boolean calibrate = false;
volatile boolean targetmove = false;
volatile int increment[] = { 1, 1, 1, 1};

boolean printed[] = { false, false, false, false};

void setup() {
  Serial.begin(115200);           // set up Serial library at 115200 bps

  for (int i=0;i<4;i++){
    pinMode(encdrPin[i], INPUT);
    pinMode(latchPin[i], INPUT);
    digitalWrite(encdrPin[i], HIGH);
    digitalWrite(latchPin[i], HIGH);
    maxEncdr[i] = defaultMax[i];
  }
  attachInterrupt(2, doE0, FALLING); // interrupt 2 is on pin 21
  attachInterrupt(3, doE1, FALLING); // interrupt 3 is on pin 20
  attachInterrupt(4, doE2, CHANGE); // interrupt 4 is on pin 19
  attachInterrupt(5, doE3, CHANGE); // interrupt 5 is on pin 18

  myMotor0->run(RELEASE);
  myMotor1->run(RELEASE);
  myMotor2->run(RELEASE);
  myMotor3->run(RELEASE);

}

void loop() {
  char dstr[180];
  boolean movefwd[] = { false, false, false, false};
  boolean worthit[] = { false, false, false, false};
  int moveMin[] = { 10, 10, 10, 5};
  int loopcnt = 0;
  int calspeed[] = { 65, 135, 125, 85};
  int targetPos[] = { 0, 0, 0, 0};
  int caldelay = 100;
  int setdelay = 10;
  double value;
  boolean stoploop = true;
  boolean done[] = { false, false, false, false};

  if (targetmove) {
    stoploop = false;
    for (int i=0;i<4;i++) {
      value = float(targetPr[i])/1000.0;
      value = float(maxEncdr[i])*value;
      targetPos[i] = int(value);
      movefwd[i] = (targetPos[i]>encdrPos[i]);
      worthit[i] = (abs(targetPos[i]-encdrPos[i])>moveMin[i]);
      done[i] = !worthit[i];
      lastlState[i] = HIGH;
    }
    // due to the inaccurate motor control of the grip, moving the motor 4 is disabled
    done[3] = true;
    worthit[3] = false;
    calspeed[3] = 0;
    if (!stoploop){
      myMotor0->setSpeed(calspeed[0]);
      delay(setdelay);
      myMotor1->setSpeed(calspeed[1]);
      delay(setdelay);
      myMotor2->setSpeed(calspeed[2]);
      delay(setdelay);
      myMotor3->setSpeed(calspeed[3]);
      delay(setdelay);
      if (worthit[0]){
        if (movefwd[0]) {
          myMotor0->run(FORWARD);
          increment[0] = 1;
        } else {
          myMotor0->run(BACKWARD);
          increment[0] = -1;
        }
        delay(setdelay);
      }
      if (worthit[1]){
        Serial.print("Moving motor 2 ");
        if (movefwd[1]) {
          myMotor1->run(FORWARD);
          increment[1] = 1;
          Serial.println("forward");
        } else {
          myMotor1->run(BACKWARD);
          increment[1] = -1;
          Serial.println("backward");
        }
        delay(setdelay);
      }
      if (worthit[2]){
        if (movefwd[2]) {
          myMotor2->run(FORWARD);
          increment[2] = 1;
        } else {
          myMotor2->run(BACKWARD);
          increment[2] = -1;
        }
        delay(setdelay);
      }
      if (worthit[3]){
        if (movefwd[3]) {
          myMotor3->run(FORWARD);
          increment[3] = 1;
        } else {
          myMotor3->run(BACKWARD);
          increment[3] = -1;
        }
      }
      delay(caldelay);
    }
    delay(caldelay);
    while (!stoploop){
      if (!done[0]){
        if (( movefwd[0]) && (encdrPos[0]>=targetPos[0])) {done[0]=true; }
        if ((!movefwd[0]) && (encdrPos[0]<=targetPos[0])) {done[0]=true; }
      }
      if (!done[1]){
        if (( movefwd[1]) && (encdrPos[1]>=targetPos[1])) {done[1]=true; }
        if ((!movefwd[1]) && (encdrPos[1]<=targetPos[1])) {done[1]=true; }
      }
      if (!done[2]){
        if (( movefwd[2]) && (encdrPos[2]>=targetPos[2])) {done[2]=true; }
        if ((!movefwd[2]) && (encdrPos[2]<=targetPos[2])) {done[2]=true; }
      }
      if (!done[3]){
        if (( movefwd[3]) && (encdrPos[3]>=targetPos[3])) {done[3]=true; }
        if ((!movefwd[3]) && (encdrPos[3]<=targetPos[3])) {done[3]=true; }
      }
      if (loopcnt>0) {
        checkLatch();
        if (!latchState[0] || done[0]) {
          myMotor0->setSpeed(0);
          myMotor0->run(RELEASE);
          if (!latchState[0]) {encdrPos[0]=0;}
          done[0] = true;
        }
        if (!latchState[1] || done[1]) {
          myMotor1->setSpeed(0);
          myMotor1->run(RELEASE);
          if (!latchState[1]) {encdrPos[1]=0;}
          done[1] = true;
        }
        if (!latchState[2] || done[2]) {
          myMotor2->setSpeed(0);
          myMotor2->run(RELEASE);
          if (!latchState[2]) {encdrPos[2]=0;}
          done[2] = true;
        }
        if (!latchState[3] || done[3]) {
          myMotor3->setSpeed(0);
          myMotor3->run(RELEASE);
          if (!latchState[3]) {encdrPos[3]=0;}
          done[3] = true;
        }
      }
      loopcnt++;
      stoploop = done[0] && done[1] && done[2] && done[3];
    }
    for (int i=0;i<4;i++) {
      sprintf(dstr,"SAFMm%d%6dE",i+1,encdrPos[i]);
      Serial.println(dstr);
      delay(setdelay);
    }
    targetmove = false;
    delay(setdelay);
    myMotor0->setSpeed(0);
    delay(setdelay);
    myMotor0->run(RELEASE);
    delay(setdelay);
    myMotor1->setSpeed(0);
    delay(setdelay);
    myMotor1->run(RELEASE);
    delay(setdelay);
    myMotor2->setSpeed(0);
    delay(setdelay);
    myMotor2->run(RELEASE);
    delay(setdelay);
    myMotor3->setSpeed(0);
    delay(setdelay);
    myMotor3->run(RELEASE);
    delay(setdelay);
  }
  if (calibrate) {
    stoploop = false;
    for (int i=0;i<4;i++) {
      encdrPos[i] = 0;
      increment[i] = 1;
    }
    myMotor0->setSpeed(calspeed[0]);
    delay(caldelay);
    myMotor1->setSpeed(calspeed[1]);
    delay(caldelay);
    myMotor2->setSpeed(calspeed[2]);
    delay(caldelay);
    myMotor3->setSpeed(calspeed[3]);
    delay(caldelay);
    myMotor0->run(BACKWARD);
    delay(caldelay);
    myMotor1->run(BACKWARD);
    delay(caldelay);
    myMotor2->run(BACKWARD);
    delay(caldelay);
    myMotor3->run(BACKWARD);
    while (!stoploop){
      checkLatch();
      if (!latchState[0]) {
        myMotor0->setSpeed(0);
        myMotor0->run(RELEASE);
        done[0] = true;
      }
      if (!latchState[1]) {
        myMotor1->setSpeed(0);
        myMotor1->run(RELEASE);
        done[1] = true;
      }
      if (!latchState[2]) {
        myMotor2->setSpeed(0);
        myMotor2->run(RELEASE);
        done[2] = true;
      }
      if (!latchState[3]) {
        myMotor3->setSpeed(0);
        myMotor3->run(RELEASE);
        done[3] = true;
      }
      stoploop = done[0] && done[1] && done[2] && done[3];
    }
    /**/
    calibrate = false;
    delay(caldelay);
    myMotor0->setSpeed(0);
    delay(caldelay);
    myMotor0->run(RELEASE);
    delay(caldelay);
    myMotor1->setSpeed(0);
    delay(caldelay);
    myMotor1->run(RELEASE);
    delay(caldelay);
    myMotor2->setSpeed(0);
    delay(caldelay);
    myMotor2->run(RELEASE);
    delay(caldelay);
    myMotor3->setSpeed(0);
    delay(caldelay);
    myMotor3->run(RELEASE);
    delay(caldelay);
    sprintf(dstr,"SAFMCal.%6d.%6d.%6d.%6d.E",encdrPos[0],encdrPos[1],encdrPos[2],encdrPos[3]);
    Serial.println(dstr);
    for (int i=0;i<4;i++) {
      maxEncdr[i] = encdrPos[i];
      encdrPos[i] = 0;
      // motorState[i] = RELEASE;
      // motorSpeed[i] = 0;
      // lastSpeedM[i] = 0;
      // lastStateM[i] = RELEASE; 
    }
    delay(1000);
  }
  checkLatch();
  if (!latchState[0]) {
    if (!printed[0]){
      myMotor0->setSpeed(0);
      myMotor0->run(RELEASE);
      sprintf(dstr,"SAFMm1%6dE",encdrPos[0]);
      Serial.println(dstr);
      encdrPos[0] = 0;
      printed[0] = true;
    }
  } else {
    printed[0]=false;
  }
  if (!latchState[1]) {
    if (!printed[1]){
      myMotor1->setSpeed(0);
      myMotor1->run(RELEASE);
      sprintf(dstr,"SAFMm2%6dE",encdrPos[1]);
      Serial.println(dstr);
      encdrPos[1] = 0;
      printed[1] = true;
    }
  } else {
    printed[1]=false;
  }
  if (!latchState[2]) {
    if (!printed[2]){
      myMotor2->setSpeed(0);
      myMotor2->run(RELEASE);
      sprintf(dstr,"SAFMm3%6dE",encdrPos[2]);
      Serial.println(dstr);
      encdrPos[2] = 0;
      printed[2] = true;
    }
  } else {
    printed[2]=false;
  }
  if (!latchState[3]) {
    if (!printed[3]){
      myMotor3->setSpeed(0);
      myMotor3->run(RELEASE);
      sprintf(dstr,"SAFMm4%6dE",encdrPos[3]);
      Serial.println(dstr);
      encdrPos[3] = 0;
      printed[3] = true;
    }
  } else {
    printed[3]=false;
  }
}

void checkLatch(){
  int reading;
  for (int i=0;i<4;i++) {
    reading = digitalRead(latchPin[i]);
    if (reading != lastlState[i]) {
      lastDTime[i] = millis();
    }
    if ((millis() - lastDTime[i]) > debounceDelay) {
      latchState[i] = reading;
    }
    lastlState[i] = reading;
  }
}

void doE0(){
  encdrPos[0] += increment[0];
}
void doE1(){
  encdrPos[1] += increment[1];
}
void doE2(){
  encdrPos[2] += increment[2];
}
void doE3(){
  encdrPos[3] += increment[3];
}

void serialEvent(){
  char ch = Serial.read();
  buffer[bufferCount] = ch;
  bufferCount++;
  if(ch == 13){
    evalSerialData();
  }
}

void evalSerialData()
{
  int ptr;
  int val;
  blocked = true;
  if ((buffer[0]=='S')&&(buffer[bufferCount-2]=='E')) {
    if ((buffer[1]=='A')&&(buffer[2]=='F')&&(buffer[3]=='M')){
      if (buffer[4]=='C'){
        // calibration run
        calibrate = true;
      }
//      Serial.println(buffer);
      if (buffer[4]=='T'){
        // SAFMT.vvvv.vvvv.vvvv.vvvvE
        // 012345678901234567890123456789
        //           1         2
        for (int i=0;i<4;i++){
          targetPr[i] = (buffer[6+i*5]-48)*1000 + (buffer[7+i*5]-48)*100 + (buffer[8+i*5]-48)*10 + (buffer[9+i*5]-48);
        }
        targetmove = true;
      }
      if (buffer[4]=='M'){
        // SAFMMiFWDE
        // SAFMMiBWDE
        // SAFMMiRELE
        // SAFMMiSvvvvE
        // 01234567890
        ptr = (buffer[5]-48)-1;
        switch (buffer[6]){
          case 'F':
            // motorState[ptr] = FORWARD;
            increment[ptr] = 1;
            switch (ptr){
              case 0:
                myMotor0->run(FORWARD);
                break;
              case 1:
                myMotor1->run(FORWARD);
                break;
              case 2:
               myMotor2->run(FORWARD);
                break;
              case 3:
                myMotor3->run(FORWARD);
                break;
            }
            break;
          case 'B':
            // motorState[ptr] = BACKWARD;
            increment[ptr] = -1;
            switch (ptr){
              case 0:
                myMotor0->run(BACKWARD);
                break;
              case 1:
                myMotor1->run(BACKWARD);
                break;
              case 2:
                myMotor2->run(BACKWARD);
                break;
              case 3:
                myMotor3->run(BACKWARD);
                break;
            }
            break;
          case 'R':
            // motorState[ptr] = RELEASE;
            switch (ptr){
              case 0:
                myMotor0->run(RELEASE);
                myMotor0->run(RELEASE);
                break;
              case 1:
                myMotor1->run(RELEASE);
                myMotor1->run(RELEASE);
                break;
              case 2:
                myMotor2->run(RELEASE);
                myMotor2->run(RELEASE);
                break;
              case 3:
                myMotor3->run(RELEASE);
                myMotor3->run(RELEASE);
                break;
            }
            break;
          case 'S':
            val = (buffer[7]-48)*1000 + (buffer[8]-48)*100 + (buffer[9]-48)*10 + (buffer[10]-48);
            // motorSpeed[ptr] = val;
            switch (ptr){
              case 0:
                myMotor0->setSpeed(val);
                break;
              case 1:
                myMotor1->setSpeed(val);
                break;
              case 2:
                myMotor2->setSpeed(val);
                break;
              case 3:
                myMotor3->setSpeed(val);
                break;
            }
            break;
        }
      }
    }
  }
  bufferCount=0;
  blocked = false;
  //Serial.println("--Arduino--");
}