Hallo
Hier nochmal der Quellcode für mein Video inkl. Erklärung:
Code:
// Servo-Sensor an einem Greifer 4.3.2010 Thund3r
static int reset,durch,durch2,stop,count; //Variablen die von verschiedenen Methoden verändert werden können
void gtest(void)
{
int c;
if(reset==0) // Wird nur einma zu Beginn des Programmes aufgerufen
{
for(c=0;c<=50;c++)
{
ServoG(12);
}
for(c=0;c<=50;c++)
{
ServoG(21);
if(durch>durch2) // Ermittelt den höchsten Schwellwert
{
durch2=durch; // Speichert diesen
}
mSleep(40);
}
reset=1; // Damit es nur einmal ausgeführt wird
}
for(c=0;c<=50;c++)
{
ServoG(12); //Greifarm öffnet sich
}
setLEDs(0b0000);
mSleep(200);
for(c=0;c<=50;c++)
{
if(stop==0) // Wenn alles ok Greif zu
{
ServoG(21);
mSleep(40);
}
else {mSleep(50);} // Wenn Widerstand erkannt (s.u.) halte Position
if(durch>durch2) //Wenn Belastung höher als ermittelter Grenzwert
{
count++; // Anzahl der Grenzwertüberschreitungen
}
else{count=0;} // Wenn Grenzwert nur einmalig erreicht wurde verwerfe den Entrag
if(count==2) //Wenn Grenzwert 2 mal Überschritten wird tritt Aktion in Kraft (Anzahl variabel)
{
setLEDs(0b1111);
count=0;
stop=1; // Greifvorgang wird abgebrochen und Position gehalten s.o.
}
}
stop=0;
mSleep(200);
}
void messG() // Messmethode
{
uint16_t adc5 = readADC(ADC_5); // Messwert einlesen
clearLCD();
durch=adc5; // Übergeben damit andere Methoden damit arbeiten können
setCursorPosLCD(0, 0);
writeStringLCD("Belastung: "); // Ausgabe auf LCD Display
writeIntegerLCD(adc5, DEC);
}
void ServoG(uint8_t pos) // Zur Ansteuerung des Servos
{
DDRC |= IO_PC7; // Pin auf Ausgang
PORTC |= IO_PC7; // High
sleep(pos); // Stellzeit
PORTC &= ~IO_PC7; // Low
DDRC &= ~IO_PC7; // Pin auf Eingang
sleep(5);
messG(); // messen
sleep(180);
}
Prakitsch zum schonen der Greifmechanik, wenn der Arm etwas nicht zu fest drücken darf, um zu überprüfen ob die gewünnschte Servo Position erreicht wurde oder ob der Servo noch in Bewegung ist etc.
Gruß Thund3r
Zitieren
Lesezeichen