SRF05 mit Servo auf RP6 Control M32

[Jordi]

Hallo RP6-Bastler,

ich habe nach 4 Jahren meinen RP6 reaktiviert. Nun möchte ich mit dem Erweiterungsmodul den Ultraschallsensor SRF05 zur Hinderniserkennung einsetzen. Dafür soll dieser mit Hilfe eines Servos in drei Positionen gedreht werden (halblinks, mittig und halbrechts) und jedesmal eine Abstandsmessung durchführen. Ich habe zum Sensor und zum Servo Beispielprogramme gefunden und diese erfolgreich an meine Vorstellungen und Pinbelegungen angepasst, d.h. der Servo und der Sensor funktionieren getrennt voneinander. Wenn ich aber die einzelnen Programme zusammenstricken will dann macht der Servo einen kurzen Ruckler bei der Initialisierung und der Sensor geht auf Dauermessung.
Nach eigenen Nachforschungen vermute ich dass der Fehler darin liegt, dass beide Programme den Timer1 verwenden. Ich habe versucht Timer0 für den Sensor zu verwenden, komme da aber nicht weiter.

Bevor ich Code poste wollte ich erstmal folgende Fragen stellen um Anhaltspunkte zu bekommen wie ich selber das Problem lösen kann:

1. Ist es überhaupt möglich und sinnvoll einen 8-bit-Timer für die Ultraschallmessung oder die Servosteuerung zu verwenden?

2. Ist es möglich Timer1 parallel für beide Aufgaben zu verwenden?

3. Kann man eine der beiden Aufgaben ohne Timer realisieren (evtl mit einer Stopwatch?) und wenn ja, welche?

Natürlich bin ich bereit den Code zu Veröffentlichen (spätestens wenn er funktioniert), habe schliesslich selber von der Veröffentlichung von Code profitiert.

Grüsse, Jordi

[Jordi]

Hallo,

ich habe jetzt doch den Timer1 des Ultraschallsensors durch den Timer0 ersetzen können, die Entfernungen werden zwischen 3 und 50 cm auch linear berechnet (>50cm habe ich nicht geprüft). Aber ich habe das Gefühl dass trotzdem etwas faul ist. Eigentlich müsste ich doch den Timer an der Stelle starten wo die Zeilen auskommentiert sind, oder? Wenn ich diese Zeilen statt denen weiter unten im Code verwende, dann bekomme ich keine linearen Entfernungen angezeigt: bei 10cm Abstand werden 20cm angegeben, bei 20cm 30cm, bei 30cm 40cm, etc.
Mit einem Vorteiler von 256 habe ich denselben Effekt, nur dass Entfernungen oberhalb 40cm gar nicht mehr erkannt werden (wegen Overflow?).
Verwende ich den Timer evtl falsch? Muss ich einen anderen Modus einstellen? Für Hinweise wäre ich dankbar.


uint16_t range;

while(true)
{
DDRA |= ADC5; // ADC5 Ausgang
PORTA = ( 1<<PORTA5 ); // ADC5 high
_delay_us(10); // 10uS warten
PORTA = ( 0<<PORTA5 ); // ADC5 low

//TCNT0 = 0; // Timer0 zurücksetzen
//TCCR0 = (1<<CS02) | (1<<CS00); // Timer0 mit Prescaler 1024 starten

DDRA &= ~ADC5; // ADC5 Eingang
while(!(PINA&0x20)); // Warten bis steigende Flanke an ADC5

TCNT0 = 0; // Timer0 zurücksetzen
TCCR0 = (1<<CS02) | (1<<CS00); // Timer0 mit Prescaler 1024 starten

while(PINA&0x20); // Warten bis fallende Flanke an ADC5
TCCR0 = ~(1<<CS02); // Timer0 stoppen
range = TCNT0; // Wert aus Timer Counter Register = Abstand in cm.

displayData(range);
_delay_ms(1000); // 1s warten bis zur nächsten Messung
}
return 0;

Grüße, Jordi

[inka]

hi,

wir haben uns hier (https://www.roboternetz.de/community/threads/65894-HC-SR04-m32/page4?p=606795&viewfull=1#post606795) mit timern beschäftigt, es ging aber um den HC-SR04, aber den thread hast Du sicher schon gesehen...

[Jordi]

Hallo inka,

danke für den Hinweis. Über das Thema war ich schon mal gestolpert, hatte es aber vor lauter Suche im Internet wieder aus den Augen verloren. Werde mir das Thema genauer zu Gemüte führen. Beim Überfliegen ist mir eine Sache wieder in den Sinn gekommen: Die Timer 0 und 2 sind auf der RP6 Control bereits vom System belegt. Kann ich die M32 überhaupt mit sinnvollem Aufwand für mein Vorhaben (US mit Servosteuerung) verwenden oder brauche ich ein anderes Board mit 2 freien Timern?

Ich habe mal den Timer0 gegen Timer2 getauscht, dann machen Servo und Sensor was sie wollen. ;-) Diese Timer sind eine Wissenschaft für sich.

Der Stand ist jetzt, dass Servo und US gemeinsam Harmonieren: links messen->mitte messen->rechts messen->mitte messen->.... und die Mess-Ergebnisse plausibel sind....ABER: der Servo ist wesentlich träger als ohne US und wenn ich die Hinweise im Internet richtig verstanden habe, dann müsste ich den Timer für die Messung direkt nach dem Ausschalten des Ausgangspins starten...

Werde dranbleiben.

Grüsse, Jordi

[Unregistriert]

Das hier:
https://www.roboternetz.de/community/threads/40090-RP6Control-M32-Library-f%C3%BCr-8-Servos

kennst Du vermutlich schon. Da ist auch gut erklärt wies funktioniert.


Du könntest da auch den Stopwatch Timer für umbauen (die ISR in der Lib!), dass er sowohl Stopwatches als auch den einen Servo ansteuert.
Dann wäre Timer 1 frei für den SRF. Oder die Timer tauschen wenn die Hardware PWM Pins sonst auf Pins liegen die nicht frei sind.

[Jordi]

Hallo,

vielen Dank für Eure Hilfe. Jetzt funktioniert alles wie es soll. Ich habe die Stopwatch- und die Servo-Lib aus diesem Forum und Code für den Sensor aus dem holländischen Arexx-Forum verwendet und angepasst. Als nächstes werde ich mich mit der Kommunikation zwischen RP6 Base und RP6 Control befassen.


Hier ist der Code:



#include "./RP6Lib/RP6control/RP6ControlServoLib.h"
#include "./RP6Lib/RP6common/RP6Stopwatch0Lib.h"
#include "RP6ControlUltrasonic.h"

volatile uint16_t pos = 0;

//ULTRASONIC FUNCTIONS #######################################

void task_US_SW()
{
uint16_t range;
//MEASURING
DDRC |= IO_PC3; // IO_PC3 Ausgang
PORTC |= (1<<PORTC3); // IO_PC3 high IO_PC3;
_delay_us(10); // 10uS warten
PORTC &= (0<<PORTC3); // IO_PC3 low ~IO_PC3;

DDRC &= ~IO_PC3; // IO_PC3 Eingang
while(!(PINC & IO_PC3)); // Warten bis steigende Flanke an IO_PC3
setStopwatch01(0);
while(PINC & IO_PC3); // Warten bis fallende Flanke an IO_PC3
range = getStopwatch01() * 1.67; // Wert der Stoppuhr * 1,67 = Abstand in cm.

displayData(range);
//MEASURING END
}


void displayData(uint16_t r)
{

writeString_P("Abstand:");
writeInteger(r, DEC);
writeString_P(" cm\n");

sound(170,40,0);
}

//END OF ULTRASONIC FUNCTIONS ################################

int main(void)
{
initRP6Control();

setLEDs(0b111111);
mSleep(500);
setLEDs(0b000000);

initSERVO(SERVO1);

startStopwatch2();
startStopwatch01();

while(true)
{
if (getStopwatch2() > 2000) { // Alle 2000ms Servo drehen

if (pos == 0) {
servo1_position = 0;
}
if (pos == 1) {
servo1_position = MIDDLE_POSITION;
}
if (pos == 2) {
servo1_position = RIGHT_TOUCH;
}
if (pos == 3) {
servo1_position = MIDDLE_POSITION;
}

pos++;
if (pos > 3) {pos = 0;}

setStopwatch2(0);

task_US_SW();
}
task_SERVO();

mSleep(3);

}
return 0;
}

Und hier sind meine Einstellungen für den Servo aus der RP6ControlServoLib.h:


#define LEFT_TOUCH 56
#define RIGHT_TOUCH 42
#define MIDDLE_POSITION (RIGHT_TOUCH / 2)
#define PULSE_REPETITION 17
#define F_TIMER1 50000

[inka]

hallo Jordi,

wie sieht die "RP6ControlUltrasonic.c" und "*.h" denn aus?

btw, der code macht sich viel besser, wenn er in den "code" tags eingegeben wird (erweiterete ansicht, mittlere iconreihe, drittes von rechts)

[Jordi]

Hallo inka,

Die RP6ControlUltrasonic.c existiert gar nicht, ich habe die beiden Methoden in der Hauptdatei implementiert. War da etwas schlampig, werde die Methoden noch auslagern.

In der Headerdatei sind eigentlich nur die Methoden definiert. In der Orginaldatei waren vorher noch #defines für die Nutzung mit Timer1.

RP6ControlUltrasonic.h:


// Defines

// Prototypes

// RP6ControllerUltrasonicTest.c
void task_US_SW(void);
void displayData(unsigned int r);

[inka]

Hi Jordi,



Die RP6ControlUltrasonic.c existiert gar nicht, ich habe die beiden Methoden in der Hauptdatei implementiert.

also würde es bei übernahme Deines codes reichen die zeile mit dem include zu entfernen, denke ich. Ich habe in dem thread den ich neulich hier erwähnt habe eine blockierende lösung des auslesens von HC-SR04 gehabt, und wollte Deien code auch ausprobieren...


War da etwas schlampig, werde die Methoden noch auslagern.

ist mir gar nicht aufgefallen :-) und war gar nicht gemeint, manchmal sind die funktionen im programm selbst sogar übersichtlicher...

[Jordi]

Hallo inka,

ich muss mal ganz blöd fragen: Was meinst Du mit 'blockierend'? Ich vermute mal dass mein Code auch 'blockiert', da der Code nur weiter ausgeführt wird wenn der Sensor das Echo erkennt (was bisher immer passiert ist, da die Sensorreichweite bisher nie ausgereizt wurde). Ich denke ich sollte noch die Stopwatch01 überwachen und bei Zeitüberschreitung die nächste Messung auslösen.

Funktioniert der Code bei Dir?

Grüsse, Jordi

[inka]

hi Jordi,


ich muss mal ganz blöd fragen: Was meinst Du mit 'blockierend'?

ab hier (https://www.roboternetz.de/community/threads/65894-HC-SR04-m32/page2?p=610427&viewfull=1#post610427) haben wir uns mit der blockierneden und nicht-blockierenden ausführungen der mess-schleifen beschäftigt. Gehört nun Dein code zu den blockierenden? Es geht mir hier lediglich darum den prinzipiellen unterschied zu verstehen und am funktionierendem code testen zu können - die nicht blockierende ausführung habe ich nicht in einen code packen können. Dass der RP6 bei der blockierenden ausführung der US-messung mal z.b. kurz seine fahrt unterbrechen muss, stört mich nicht weiter...


Funktioniert der Code bei Dir?

ja, teilweise. Die entfernungsabfrage funktioniert nun, nachdem ich es teilweise umschreiben musste (da ich für den HC-SR04 zwei pins nehmen muss - echo und trig), der servo noch nicht, das liegt vermutlich an der anderen initailisierung und dem betreiben der servos an der multi-IO karte. Kommt noch, jetzt bin ich erstmal für eine woche weg...

[inka]

hi Jordi,

ich habe nun mit dem code etwas "herumgespielt", es funktioniert bei mir (messung und servo auch):


#include "RP6ControlLib.h"
#include "RP6I2CmasterTWI.h"
#include "RP6Control_MultiIOLib.h"
#include "RP6Control_I2CMasterLib.h"
#include "RP6Control_MultiIO.h"
#include "RP6Control_LFSBumperLib.h"
#include "RP6Control_OrientationLib.h"
#include "RP6Stopwatch0Lib.h"
#include "RP6ControlServoLib.h"
#include "standard.h"


#define I2C_RP6_BASE_ADR 10

uint16_t servopos, range;

//ULTRASONIC FUNCTIONS #######################################

void task_US_SW()
{
DDRC |= IO_PC6; // IO_PC6 Ausgang
PORTC |= (1<<PORTC6); // IO_PC6 high IO_PC6;
_delay_us(10); // 10uS warten
PORTC &= (0<<PORTC6); // IO_PC6 low ~IO_PC6;

DDRC &= ~IO_PC5; // IO_PC5 Eingang
while(!(PINC & IO_PC5)); // Warten bis steigende Flanke an IO_PC5
setStopwatch01(0);
while(PINC & IO_PC5); // Warten bis fallende Flanke an IO_PC5
range = getStopwatch01() * 1.67; // Wert der Stoppuhr * 1,67 = Abstand in cm.
}



void displayData()
{

writeString_P("Abstand:");
writeInteger(range, DEC);
writeString_P(" cm\n");

//sound(170,40,0);
}

//END OF ULTRASONIC FUNCTIONS ################################

int main(void)
{

initRP6Control();
multiio_init();
initLCD();

setLEDs(0b111111);
mSleep(500);
setLEDs(0b000000);

I2CTWI_initMaster(100);
I2CTWI_setTransmissionErrorHandler(I2C_transmissio nError);

showScreenLCD(" RP6Control M32", " HC-SR04 plus servo"," jordi","PC5 und PC6");
mSleep(1500);
clearLCD();

setServoPower(1);
PCA9685_init(50);
servopos = (SERVO1_LT);

startStopwatch01();

while(true)
{

PCA9685_set(1, servopos);

servopos += 5;
mSleep(2000);

if (servopos > (SERVO1_RT)) servopos = (SERVO1_LT);

task_US_SW();

displayData();

}
return 0;
}

ich habe den code nach meinem verständnis angepasst, weil ich ein paar sachen in Deinem code nicht verstanden habe:

- die funktion displayData(range) innerhalb der messfunktion - warum?

- die variablen r und range - warum zwei, warum innerhalb der funktion deklariert/definiert?

die messwerte, die ich bekomme sind nachvollziehbar, das einzige was mich noch stört ist das verhalten meines servos: es kommen erstmal 4 messwerte bevor die erste servobewegung stattfindet. Verstehe ich nicht, liegt aber nicht an Deinem, sondern an meinem code ...

[TrainMen]

Hi inka,
eine paar Gedanken, eine Pause in der Hauptschleife ist nicht so vorteilhaft, da Pausiert ja denn wirklich alles. Ist es denn in Deinem Projekt wirklich nötig alle 5 Servoschritte eine Messung vorzunehmen ? Wie soll das denn später funktionieren wenn der BOT sich bewegt ?
Wäre es nicht besser nur an bestimmten Positionen links, rechts und mitte die Messung durchzuführen ?


Dass der RP6 bei der blockierenden ausführung der US-messung mal z.b. kurz seine fahrt unterbrechen muss, stört mich nicht weiter...

Wieviel cm bewegt sich denn der Bot bis zur nächsten Messung ?

Wenn ich das richtig sehe, dreht Dein Servo langsam nach rechts bis er > RT erreicht hat und dann in einem Rutsch zurück nach LT. Das sieht doof aus. Warum läßt Du ihn nicht auch wieder Schrittweise nach LT drehen ? Ich habe auch eine Funktion in der Lib geschrieben wo alle meine Servos eine Grundposition einnehmen. Diese rufe ich immer vor der Hauptschleife auf (Display Zeit) und habe bei Start immer die gleiche Position.

[inka]

Hi TrainMen,

danke für die tipps, nicht alles was ich hier poste ist bis zum letzten detail durchdacht und fertig, das manches doof aussieht mag ja sein, es wäre aber für mich hilfreicher gewesen auf meine frage einzugehen...

trotzdem danke...

[Jordi]

ich habe den code nach meinem verständnis angepasst, weil ich ein paar sachen in Deinem code nicht verstanden habe:

- die funktion displayData(range) innerhalb der messfunktion - warum?

- die variablen r und range - warum zwei, warum innerhalb der funktion deklariert/definiert?

die messwerte, die ich bekomme sind nachvollziehbar, das einzige was mich noch stört ist das verhalten meines servos: es kommen erstmal 4 messwerte bevor die erste servobewegung stattfindet. Verstehe ich nicht, liegt aber nicht an Deinem, sondern an meinem code ...

Hallo inka,

danke für Deine Fragen. Warum ich das so programmiert habe kann ich Dir gar nicht beantworten...war nur glücklich dass Servo und Sensor endlich wie gewünscht zusammengearbeitet haben. Durch Deine Fragen habe ich mich aber nochmal mit dem Code auseinandergesetzt und muss zugeben dass ich nicht gerade elegant programmiert habe.

Grüsse, Jordi

Nachtrag: Ich bekomme vor der ersten Servo-Bewegung 2 US-Messungen...stört mich aber im Moment (noch) nicht...setze mich derzeit mit der Kommunikation zwischen den µCs auseinander...

[Unregistriert]

Hallo
wo kommt denn der Wert von 1,67 bei der Berechnung her ?

[inka]

also ohne Jordi jetzt vorgreifen zu wollen würde ich sagen aus der schallgeschwindigkeit

334 m/s - geteilt durch die zwei wege - geteilt durch 100 (cm) = 1,67...

edit: wobei 343 m/s bei 20°C besser passen würde...

[Jordi]

Hallo,

den Faktor habe ich nicht berechnet, sondern empirisch mit einem Zollstock und einem Hindernis in 10, 20, 30, 40 und 50cm bestimmt. (Grund: Denkfaulheit ;-) )

Genauer: Sollwert war immer der Abstand, Istwert war der Durchschnitt aus je zehn Messungen (Ausgabe im Terminal). Daraus wurde für jeden Abstand der Korrekturfaktor bestimmt (Tabellenkalkulation von Libreoffice). Die Faktoren von 20 bis 50cm waren mit 1,66667 identisch, daher habe ich den Wert 1,67 genommen. Bei 10cm war der Faktor 1,47.

Ob die Ungenauigkeit bei ganz kurzen Distanzen am US-Sensor oder meinem Code oder an Schlampigkeit beim Anlegen des Zollstocks liegt kann ich aber nicht beantworten.

Grüsse, Jordi

@inka: Danke für die Formel. ;-)