Zur Entspannung, glaubte ich, bin ich dann an meinen Arduino, dem ich die folgende Funtion zuweisen wollte:
Empfange seriell den Befehl "gehe x cm y Grad". Er soll dann die Motoren selbständig steuern und in regelmäßigen Abständen auch seriell "z % erlegigt" zurückgeben. Beispiel: gehe(100,0) wäre 1Meter geradeaus. gehe(0,90) -> dreht 90° rechtsrum auf der Stelle. gehe(-50,-180) -> Fährt einen 0,5 Meter langen Kreisbogen rückwärts linksrum.
Er soll ausserdem noch auf einen "notstop" Befehl reagieren, bei dem beide Motoren auf der Stelle angehalten werden.
Der Motortreiber MD22 kann analog, i2c-seriell und über PWM angesprochen werden. Ich dachte "bleibe ich doch solang wie möglich digital" und habe mich auf den I2C bus konzentriert und wäre fast verzweifel, weil nichts ging. Zum Glück bin ich gestern abend noch auf einer Webseite auf ein Script gestossen, in dem eine andere Basis-Adresse verwendet wurde. Entgegen aller Beschreibungen und Manuals hört der MD22 rev.10 NICHT auf 0xB0, sondern auf 0x58. Dann klappts auch mit den Nachbarn.
Gestern und heute schliesslich, sollte der Odometer ausprobiert werden. Bei Pollin gabs für ein paar Cent die IR-Gabel TCYS5201 mit Schmitt-Trigger Ausgang. Die dann an meinen Arduino angeschlossen und als Interrupttrigger verwendet. Bei LOW alles supi, aber bei HIGH zählte mein Arduino in ein paar Sekunden so viele Ticks, dass er übergelaufen steckenblieb. Erst dachte ich, die Verwendung von externen Interrupts sei vielleicht gar nicht gleichzeitig mit Serial.begin möglich, weil dort ja auch Interrupts ausgelöst wurden. Aber wer liest ist klar im Vorteil: im Datenblatt der Gabel wird empfohlen, einen Kondensator möglichst nah am Sensor zwischen VCC und GRD zu legen und dann zählte mein Ardu wie gewünscht.
Code:
/******************************************************************************
* Luises - Motorsteuerung - erste Erfolge
* Arduino analog input 5 - I2C SCL
* Arduino analog input 4 - I2C SDA
* Arne Groh 2009-04-03
* ******************************************************************************/
#include <WString.h>
#include <Wire.h>
volatile unsigned int LeftEncoderTick = 0;
int l_enc_v0 = 12;
int left_speed = 60;
int right_speed = 128;
int incomingByte = 0; // for incoming serial data
int revision;
int mot1;
int mot2;
int accel;
#define md22Address 0x58 // !! NOT 0xB0 !!
void setup() {
pinMode(l_enc_v0, OUTPUT);
digitalWrite(l_enc_v0, HIGH);
Wire.begin(0x53); // Host Address
Wire.beginTransmission(md22Address); // MD22 Address (All Switches ON = 0x58)
Wire.send(0x00); // Set Register 0 (Mode)
Wire.send(0x00); // Send Data Mode to 0 (0 to 255)
Wire.endTransmission();
Wire.beginTransmission(md22Address); // MD22 Address (All Switches ON = 0x58)
Wire.send(0x03); // Register of Acceleration
Wire.send(128); // Send Data for Acceleration
Wire.endTransmission();
readMD22();
attachInterrupt(0, LeftEncoderEvent, HIGH);
Serial.begin(9600);
write_MD22();
}
void loop() {
readSpeed();
delay(10);
}
void write_MD22(){
Wire.beginTransmission(md22Address); // MD22 Address (All Switches ON = 0x58)
Wire.send(0x01); // Register of Motor 1
Wire.send(left_speed); // Send Data for left Motor(1) Speed (0 = Full Reverse | 128 = Stop | 255 = Full Forward)
Wire.send(right_speed); // Send Data for right Motor(2) Speed
Wire.endTransmission();
}
void readSpeed(){
if (Serial.available() > 0){
char string[8] = " "; // declare string variable
for (int i = 0; i < 7; i++){
if (Serial.available() > 0) // send data only when you receive data:
{string[i] = Serial.read();}
}
String sstring = String(string);
String l_string = sstring.substring(0, 3);
String r_string = sstring.substring(4, 7);
left_speed = (atoi(l_string));
right_speed = (atoi(r_string));
write_MD22();
printmessage();
}
}
void readMD22(){ // Reads Data from MD22
Wire.beginTransmission(md22Address); // Calles software register
Wire.send(0x07); // Register of SoftwareRevision
Wire.endTransmission();
Wire.requestFrom(md22Address, 1); // Requests one byte
while(Wire.available() < 1); // Wait for it to arrive
revision = Wire.receive(); // Get byte
Wire.beginTransmission(md22Address); // Calles software register
Wire.send(0x01); // Register of Motor 1
Wire.endTransmission();
Wire.requestFrom(md22Address, 1); // Requests one byte
while(Wire.available() < 1); // Wait for it to arrive
mot1 = Wire.receive(); // Get byte
Wire.beginTransmission(md22Address);
Wire.send(0x02); // Register of Motor 2
Wire.endTransmission();
Wire.requestFrom(md22Address, 1); // Requests one byte
while(Wire.available() < 1); // Wait for it to arrive
mot2 = Wire.receive(); // Get byte
Wire.beginTransmission(md22Address);
Wire.send(0x03); // Register of Acceleration
Wire.endTransmission();
Wire.requestFrom(md22Address, 1); // Requests one byte
while(Wire.available() < 1); // Wait for it to arrive
accel = Wire.receive(); // Get byte
}
void LeftEncoderEvent(){
LeftEncoderTick++;
}
void printmessage(){
readMD22();
Serial.print("MD22-Ver: "); // Print to Serial
Serial.print(revision);
Serial.print(" | Current Mot 1 Speed: ");
Serial.print(mot1);
Serial.print(" | Current Mot 2 Speed: ");
Serial.print(mot2);
Serial.print(" | Current Accel: ");
Serial.print(accel); // Print to serial and CR
Serial.print(" | LeftEncoderTick: ");
Serial.println(LeftEncoderTick);
}
Als nächstes werde ich versuchen, meine Trigonometrie-Erinnerungen aufzufrischen, um die Kursumsetzung zu implementieren. Ich hatte anfangs mit dem Gedanken gespielt, hier einen PID-Regler einzusetzen, aber ich verstehe nicht wirklich, was der treibt...
Zitieren
Lesezeichen