Sieht ungefähr so aus
Code: Alles auswählen
#include <Ftduino.h>
const int SERIAL_BAUD_RATE = 115200; // Baudrate für die serielle Kommunikation
const int RPM_IMPULSES_PER_REVOLUTION = 63; // Impulse pro Umdrehung der Getriebeachse
unsigned long lastTime = 0;
unsigned long lastImpulseCount1 = 0;
unsigned long lastImpulseCount2 = 0;
unsigned long lastImpulseCount3 = 0;
unsigned long lastImpulseCount4 = 0;
void setup() {
ftduino.init(); // Initialisierung der Ftduino-Bibliothek
pinMode(LED_BUILTIN, OUTPUT);
// Initialisiere Counter für die Encoder
ftduino.counter_set_mode(Ftduino::C1, Ftduino::C_EDGE_RISING);
ftduino.counter_set_mode(Ftduino::C2, Ftduino::C_EDGE_RISING);
ftduino.counter_set_mode(Ftduino::C3, Ftduino::C_EDGE_RISING);
ftduino.counter_set_mode(Ftduino::C4, Ftduino::C_EDGE_RISING);
ftduino.counter_clear(Ftduino::C1);
ftduino.counter_clear(Ftduino::C2);
ftduino.counter_clear(Ftduino::C3);
ftduino.counter_clear(Ftduino::C4);
Serial.begin(SERIAL_BAUD_RATE); // Serielle Kommunikation starten mit der angegebenen Baudrate
}
// Funktion zum Zählen der Impulse und Berechnen der RPM
void calculateRPM() {
unsigned long currentTime = millis();
unsigned long elapsedTime = currentTime - lastTime;
if (elapsedTime >= 1000) {
// Impulse der Encoder zählen
uint16_t impulseCount1 = ftduino.counter_get(Ftduino::C1);
uint16_t impulseCount2 = ftduino.counter_get(Ftduino::C2);
uint16_t impulseCount3 = ftduino.counter_get(Ftduino::C3);
uint16_t impulseCount4 = ftduino.counter_get(Ftduino::C4);
// RPM berechnen
float rpm1 = (float)(impulseCount1 - lastImpulseCount1) * 60.0 / (elapsedTime / 1000.0) / RPM_IMPULSES_PER_REVOLUTION;
float rpm2 = (float)(impulseCount2 - lastImpulseCount2) * 60.0 / (elapsedTime / 1000.0) / RPM_IMPULSES_PER_REVOLUTION;
float rpm3 = (float)(impulseCount3 - lastImpulseCount3) * 60.0 / (elapsedTime / 1000.0) / RPM_IMPULSES_PER_REVOLUTION;
float rpm4 = (float)(impulseCount4 - lastImpulseCount4) * 60.0 / (elapsedTime / 1000.0) / RPM_IMPULSES_PER_REVOLUTION;
// Meldung nur, wenn sich RPM um mehr als 1 RPM verändert
static float prevRpm1 = 0;
static float prevRpm2 = 0;
static float prevRpm3 = 0;
static float prevRpm4 = 0;
if (abs(rpm1 - prevRpm1) > 1 || abs(rpm2 - prevRpm2) > 1 ||
abs(rpm3 - prevRpm3) > 1 || abs(rpm4 - prevRpm4) > 1) {
Serial.print("RPM1: ");
Serial.print(rpm1);
Serial.print(", RPM2: ");
Serial.print(rpm2);
Serial.print(", RPM3: ");
Serial.print(rpm3);
Serial.print(", RPM4: ");
Serial.println(rpm4);
prevRpm1 = rpm1;
prevRpm2 = rpm2;
prevRpm3 = rpm3;
prevRpm4 = rpm4;
}
lastTime = currentTime;
lastImpulseCount1 = impulseCount1;
lastImpulseCount2 = impulseCount2;
lastImpulseCount3 = impulseCount3;
lastImpulseCount4 = impulseCount4;
}
}Code: Alles auswählen
// Funktion zur Motorsteuerung
void handleMotorControl() {
if (Serial.available() > 0) {
String msg = Serial.readStringUntil('\n');
msg.trim();
// Wenn die Nachricht ein gültiges Motor-Kommando enthält, setze die Motor-Geschwindigkeit
if (msg.startsWith("ML(") && msg.endsWith(")")) {
msg = msg.substring(3, msg.length() - 1); // Ausschneiden des Geschwindigkeitswerts für linke Motoren
int motor_speed = msg.toInt();
if (motor_speed >= 0 && motor_speed <= 100) {
// Motor-Geschwindigkeit setzen (0 bis 100)
ftduino.motor_set(Ftduino::M1, Ftduino::LEFT, motor_speed);
ftduino.motor_set(Ftduino::M2, Ftduino::LEFT, motor_speed);
}
} else if (msg.startsWith("MR(") && msg.endsWith(")")) {
msg = msg.substring(3, msg.length() - 1); // Ausschneiden des Geschwindigkeitswerts für rechte Motoren
int motor_speed = msg.toInt();
if (motor_speed >= 0 && motor_speed <= 100) {
// Motor-Geschwindigkeit setzen (0 bis 100)
ftduino.motor_set(Ftduino::M3, Ftduino::RIGHT, motor_speed);
ftduino.motor_set(Ftduino::M4, Ftduino::RIGHT, motor_speed);
}
}
}
}
void loop() {
// Zähle die Impulse der Encoder und berechne RPM
calculateRPM();
// Motorsteuerung durch das Python-Skript
handleMotorControl();
}
}
Die Kommandos habe ich aus der Anleitung. Jetzt habe ich festgestellt, dass meine Motoren scheinbar eine große Streuung aufweisen. Geradeaus fahren wird evtl. nicht klappen. Manche meiner Motoren schaffen 150 rpm manche 170 rpm. Manche laufen ab 15rpm manche ab 50rpm Ich möchte die gerne bei Bedarf synchron laufen lassen.
Der Txt scheint da ja schon etwas an Board zu haben für einen Synchronlauf, hat der ftduino das auch? Wenn nicht, wie mach ichs am besten? PID?
Meine erste Idee war vielleicht aus der Konsole heraus direkt eine Ziel rpm anzufordern, anstatt eines motor_set speed. Dazu könnte ich Paare aus RPM und motor_set speed X experimentell bestimmen und dann "clampen". Das würde dann als Startwert dienen und PID müsste das dann feintunen, die RPM werden ja sowieso schon überwacht.
Code: Alles auswählen
// // Wenn die Nachricht ein gültiges Motor-Kommando enthält, setze die Motor-Geschwindigkeit
if (msg.startsWith("ML(") && msg.endsWith(")")) {
msg = msg.substring(3, msg.length() - 1); // Ausschneiden des Geschwindigkeitswerts für linke Motoren
int motor_speed = mapRpmToSpeed(msg.toInt());
if (motor_speed >= 0 && motor_speed <= 100) {
// Motor-Geschwindigkeit setzen (0 bis 100)
ftduino.motor_set(Ftduino::M1, Ftduino::LEFT, motor_speed);
ftduino.motor_set(Ftduino::M2, Ftduino::LEFT, motor_speed);
Serial.print("Linken Motor auf Geschwindigkeit eingestellt: ");
Serial.println(motor_speed);
}
} else if (msg.startsWith("MR(") && msg.endsWith(")")) {
msg = msg.substring(3, msg.length() - 1); // Ausschneiden des Geschwindigkeitswerts für rechte Motoren
int motor_speed = mapRpmToSpeed(msg.toInt());
if (motor_speed >= 0 && motor_speed <= 100) {
// Motor-Geschwindigkeit setzen (0 bis 100)
ftduino.motor_set(Ftduino::M3, Ftduino::RIGHT, motor_speed);
ftduino.motor_set(Ftduino::M4, Ftduino::RIGHT, motor_speed);
Serial.print("Rechten Motor auf Geschwindigkeit eingestellt: ");
Serial.println(motor_speed);
}
}
}
}
Funktion zum Umrechnen der RPM in Motorgeschwindigkeit, hier müssten noch weitere Werte bestimmt werden
int mapRpmToSpeed(int rpm) {
if (rpm <= 50) return 0;
else if (rpm <= 80) return map(rpm, 50, 80, 10, 20);
else if (rpm <= 120) return map(rpm, 80, 120, 20, 30);
else if (rpm <= 138) return map(rpm, 120, 138, 30, 40);
else return 50;
}
