4 Achs Roboterarm an M32

[proevofreak]

hallo leute,
hier will ich euch mein neues projekt vorstellen. nachdem ich jetzt kurz vor studienbeginn doch noch zeit zum basteln hatte, habe ich nun endlich meinen 4 Achs Roboterarm fertig gebaut, welcher über die freien IO-Ports der M32 angesteuert werden soll.



Hier ein paar bilder für die ersten eindrücke:
ps: ja den greifer hab ich mir von Radbruch abgeschaut^^.

http://img136.imageshack.us/img136/603/1000903b.th.jpg (http://img136.imageshack.us/i/1000903b.jpg/)

http://img10.imageshack.us/img10/2558/1000907s.th.jpg (http://img10.imageshack.us/i/1000907s.jpg/)


http://img23.imageshack.us/img23/452/1000906h.th.jpg (http://img23.imageshack.us/i/1000906h.jpg/)

der arm besteht aus 5 servos und einem Reflexkoppler (befindet sich am Greifer), mit welchem gegenstände im erfassungsbereich des greifers erkannt werden können.

jetzt hab ich mich mal an die programmierung gemacht. aber ich scheitere schon an der ersten hürde.
warum sind alle IO- Ports der M32 unprogrammiert high bzw. warum der M32 auf meinen an PD 5 angeschlossenen Reflexkoppler nicht?

scheinbar habe ich noch irgendeine Maßnahme bei der Initialisierung meiner IO- Ports in meinem programm vergessen. wer kann mir da kurz weiterhelfen?

mfg



#include "RP6ControlLib.h"


//Motor 1 => PD6
//Motor 2 => PC6
//Motor 3 => PC7
//Motor 4 => PC3
//Motor 5 => PC2
//Reflexkoppler => PD5

void teil_erkannt(void)
{if(PINA & IO_PD5)
{writeString_P("kein Teil erkannt\n");}

else
{writeString_P("Teil erkannt\n");}
}



int main (void)
{
initRP6Control();

DDRD &=~ IO_PD5; //PD5 (M32) als Eingang


while(true)
{teil_erkannt();

}
return 0;
}

[radbruch]

Hallo

Mächtig viel Heiskleber dran :) Ist der Arm direkt auf der Platine befestigt oder hast da eine Unterkonstruktion drunter?

Hier stimmt was nicht: if(PINA & IO_PD5) So wird Port A Bit5 abgefragt. Richtig wäre:

if(PIND & IO_PD5)

Was ist das für ein Sensor und wie ist er an PD5 angeschlossen?

Warum die Pins high sind kann ich nicht so schnell beantworten, weil ich kein M32 besitze und deshalb die Lib noch nicht entpackt habe. Vielleicht wird in initRP6Control() irgendwas gesetzt.

Die Pins PC2 bis PC5 sind die JTAG-Pins, möglicherweise muss man diese Funktion ausschalten. (Das M32 hat doch auch einen Mega32, oder?)

Grundsätzlich sollten die jungfräulichen Pins nach dem Starten den Z-Status (Eingang ohne PullUp) haben, also hochohmig sein. Vielleicht kann man mit einem hochohmigen Digitalvoltmeter trotzdem eine Spannung messen die aber bei Belastung (Pin gegen GND im 10er-kOhm Bereich) zusammenbrechen sollte.

Gruß

mic

[proevofreak]

danke radbruch, wie konnt ich das nur übersehen^^. aber wie es in der C-programmierung nun mal so ist, ein falscher Buchstabe und nichts geht^^.

ja der heißkleber ist wirklich ne allzweckwaffe. also direkt auf die platine habe ich mit heißkleber den 1.Servo geklebt, welcher dann den 2.servo dreht. wenn du beim ersten bild ganz gut hinschaust kannst du ihn ein wenig sehen. hab also keine unterkonstruktion verwendet.

der sensor ist ein cny 70 von vishay.

hier das datenblatt:

http://pdf1.alldatasheet.com/datasheet-pdf/view/26332/VISAY/CNY70.html

der anschluss ist eigentlich recht simpel
siehe hier:

http://www.mikrocontroller.net/attachment/41016/120.png

V_Ol kommt an den eingang des µc.

allerdings war bei mir das große problem dass ich keinen richtigen high-low wechsel erzeugen konnte. darum hab ich mich nach vielen test dazu entschlossen den 47K widerstand mit nem poti zu ersetzen. so kam ich dann doch noch zu meinem pegelwechsel.

der sensor erkennt gegenstände im abstand von 2-3 mm und eignet sich so hervorragend für eine hinderniserkennung in meinem greifer.
dazu kommt sein recht leichter anschluss und günstiger preis.

mfg

[proevofreak]

hallo leute, mein roboterarm ist ja schon seit längerem fertig (siehe oben). da ich in letzter zeit nicht zum programmieren kam, möchte ich mich ihn nun nach einer längeren pause programmieren.

nun stehe ich vor einem ersten problem. ich möchte eigentlich recht simpel einen eingang an der M32 programmieren (für den reflexkoppler) und habe dazu zum testen ein recht kurzes programm geschrieben.

leider sind bei mir die IO ports der M32 immer high, auch wenn ich das programm aufgespielt habe. auch mein IO_PD6, welcher ja mein eingang ist.

darum bekomme ich auch keine textausgaben.

hier mein programm:




#include "RP6ControlLib.h"

uint16_t a;

void task_teil_erkannt(void)
{if (PIND & IO_PD6)
{
a=0;
writeString_P("Kein Teil erkannt");}

else {a=1;
writeString_P("Teil erkannt");}
}

int main (void)
{initRP6Control();

DDRD &=~ IO_PD6; //IO_PD6 als Eingang




while(true)
{
task_teil_erkannt();
}
return 0;
}


hab eigentlich alles so gemacht wie in der anleitung beschrieben.
wer kann mir weiterhelfen?

mfg andi

[radbruch]

Hallo

Ich kann es im Moment nicht testen: Fehlt da nicht der Zeilenvorschub am Ende der Textausgabe? ("\n")

Gruß

mic

[proevofreak]

danke radbruch, du hattest natürlich recht. hab ich ganz übersehen.

was ich allerdings nicht verstehe ist, warum ich an meinem eingang IO_PD6 ein high signal von 5 volt messe, wenn ich den reflexkoppler abgesteckt habe und das programm laufen lasse.

gruß andi

[proevofreak]

so leute, mittlerweile arbeitet mein 5 achs roboterarm auf der basis von dirks servo lib schon richtig. jetzt hab ich nur noch ein problem in meinem programm. und zwar sollte der arm nach einem kompletten arbeitszyklus solange in seiner grundstellung bleiben, bis wieder ein teil erkennt.

leider ist das momentan noch nicht der fall, da der arm nach einem komplett zyklus selbstständig wieder von vorne anfängt mit dem greifer zu greifen und sich anschließend den kompletten arbeitszyklus selbsständigt wiederholt.

das problem dürfte an meiner stopwatch2() liegen. irgendwie startet sie immer wieder selbstständig, ohne dass ein teil erkannt wird (hab es mit der terminal meldung überprüft).

hier mein bisheriges programm:


// Uncommented Version of RP6ControlServo.c
// written by Dirk
// ------------------------------------------------------------------------------------------

#include "RP6ControlLib.h"
#include "RP6ControlServoLib.h"



//Servo 1 => PC2
//Servo 2 => PC3
//Servo 3=> PC4
//Servo 4=> PC5
//Servo 5=> PC6
//Reflexkoppler => PD6

uint8_t a;

void task_teil_erkannt(void)
{if (PIND & (1<<6))
{
a=1;
writeString_P("kein Teil erkannt\n");}

else {a=0;
writeString_P("Teil erkannt\n");

startStopwatch2();

}
}
void task_servoansteuerung(void)
{if (getStopwatch2() >100 && getStopwatch2() <6500)
{
servo1_position = 0;
writeString_P("Greifer zu\n");
}

if (getStopwatch2() > 2000 && getStopwatch2()<2200)
{
servo3_position = 50;
servo4_position = 150;
writeString_P("Arm hoch\n");
writeString_P("start stopwatch2\n");}

if (getStopwatch2() >2200 && getStopwatch2() <2400)
{servo4_position = 130;}

if (getStopwatch2() >2400 && getStopwatch2() <2600)
{servo4_position = 110;}

if (getStopwatch2() >2600 && getStopwatch2() <2800)
{servo4_position = 90;}

if (getStopwatch2() >2800 && getStopwatch2() <3000)
{servo4_position = 80;}

if (getStopwatch2() >3000 && getStopwatch2() <3200)
{servo4_position = 70;}

if (getStopwatch2() >3200 && getStopwatch2() <3400)
{servo4_position = 60;}

if (getStopwatch2() >3400 && getStopwatch2() <3600)
{servo4_position = 50;}

if (getStopwatch2() >3600 && getStopwatch2() <3800)
{servo4_position = 40;}

if (getStopwatch2() > 3800 && getStopwatch2() <5000)
{servo4_position = 30;}

if (getStopwatch2() >4500 && getStopwatch2() <9500)
{servo5_position = 0;}

if (getStopwatch2() >5000 && getStopwatch2() <8500)
{servo4_position = 0;}

if (getStopwatch2() >5500 && getStopwatch2() <7500)
{servo2_position = 50;
writeString_P("Greifer drehen\n");}

if (getStopwatch2() >6500)
{servo1_position = RIGHT_TOUCH;}

if (getStopwatch2() >7500 )
{servo2_position = 150;
}

if (getStopwatch2() >8500 && getStopwatch2() <10500)
{servo4_position = 100;}

if (getStopwatch2() >9500)
{servo5_position = MIDDLE_POSITION;}

if (getStopwatch2() >10500)
{servo4_position = 120;
servo3_position = RIGHT_TOUCH;}

if (getStopwatch2() >10900)
{servo4_position = 140;}

if (getStopwatch2() >11300 && getStopwatch2()< 11500)
{servo4_position = RIGHT_TOUCH;}

if (getStopwatch2() >11500)
{setStopwatch2(0);
writeString_P("setStopwatch2 auf 0\n");}

}

int main(void)
{
initRP6Control();
DDRD &=~ (1<<6); //Eingang für Reflexkoppler

initSERVO(SERVO1|SERVO2|SERVO3|SERVO4|SERVO5);
servo1_position = RIGHT_TOUCH;
servo2_position = 150;
servo3_position = RIGHT_TOUCH;
servo4_position = RIGHT_TOUCH;
servo5_position = MIDDLE_POSITION;


while(true)
{task_servoansteuerung();

task_teil_erkannt();


task_SERVO();

mSleep(3);
}
return 0;
}


fällt irgendjemand auf, was ich falsch programmiert habe.

p.s. demnächst stell ich ein video von meinem arm online, aber natürlich erst wenn der arm korrekt funktioniert.

mfg andi

[Dirk]

Ich sehe 2 Möglichkeiten:

1. Du könntest deine Variable a nutzen:
In task_servoansteuerung() würde die Armbewegung nur stattfinden, wenn a==1 ist.

2. Du könntest in task_teil_erkannt() in dem Teil, wo KEIN Teil erkannt wurde, die Stopwatch2 auf 0 setzen und STOPPEN. Dadurch würde in task_servoansteuerung() auch keine Bewegung stattfinden.

Probier mal beides aus.

Gruß Dirk

[proevofreak]

danke nochmal dirk für deine hilfe. leider bin ich bis heute nicht mehr zum programmieren gekommen und habs drum auch total vergessen dir zu antworten.

aber dafür funktioniert mein arm nun komplett so wie ich will, also nicht nur für einen zyklus. viel habe ich probiert, wie ich das umsetzen kann, auch deine vorschläge aus deinem letzten post habe ich ausprobiert, aber erst heute kam mir die lösung.

zur erinnerung: mein bisheriges problem war, dass das setStopwatchx(0) nur die Stopwatchx zurückgesetzt hat und diese sich wiederum danach selbstständig gestartet hat.

darum bin ich heute in der anleitung auf die funktion stopStopwatchx(); gestoßen. seit ich diese dann in mein programm mit eingebaut habe, funktioniert alles wie gewollt.

hier ist nun mein fertiges programm:
// Uncommented Version of RP6ControlServo.c

// ------------------------------------------------------------------------------------------

#include "RP6ControlLib.h"
#include "RP6ControlServoLib.h"
#include "RP6I2CmasterTWI.h"
#include "RP6Control_I2CMasterLib.h"

//Servo 1 => PC2
//Servo 2 => PC3
//Servo 3=> PC4
//Servo 4=> PC5
//Servo 5=> PC6
//Reflexkoppler => PD6

uint8_t a;




void task_teil_erkannt(void)
{if (PIND & (1<<6))
{
a=1;

writeString_P("kein Teil erkannt\n");
}

else {a=0;
writeString_P("Teil erkannt\n");
startStopwatch2();


}
}
void task_servoansteuerung(void)
{

if (getStopwatch2() >100 && getStopwatch2() <6000)
{
servo1_position = 0;
writeString_P("Greifer zu\n");
stopStopwatch3();
setStopwatch3(0);
}

if (getStopwatch2() > 2000 && getStopwatch2()<6000)
{
servo3_position = 50;
stopStopwatch3();
setStopwatch3(0);
writeString_P("Arm hoch\n");
}

if (getStopwatch2() >3000 && getStopwatch2() < 5000)

{servo2_position =0;
stopStopwatch3();
setStopwatch3(0);}

if ( getStopwatch2() >4000 && getStopwatch2() <6000)
{servo5_position = 0;
stopStopwatch3();
setStopwatch3(0);}

if ( getStopwatch2() >5000 && getStopwatch2() <6000 )
{servo2_position =150;
writeString_P("Greifer auf Grundposition zurück\n");
stopStopwatch3();
setStopwatch3(0);}

if (getStopwatch2() >6000 )
{servo3_position = RIGHT_TOUCH;
writeString_P("Stopwatch3 starten\n");
startStopwatch3();}


if ( getStopwatch3() >1000 )
{servo1_position = RIGHT_TOUCH;
writeString_P("Stopwatch3 läuft\n");
stopStopwatch2();}

if ( getStopwatch3() > 2000 && getStopwatch3() <4000)
{servo3_position =50;
writeString_P("Arm hoch 3\n");
setStopwatch2(0);
writeString_P("Stopwatch2 auf 0 \n");}

if (getStopwatch3() >3000)
{servo5_position = MIDDLE_POSITION;
setStopwatch2(0);}

if (getStopwatch3() >4000)
{servo3_position = RIGHT_TOUCH;
setStopwatch2(0);}



}


void RP6_Bewegung(void)
{if (getStopwatch3() >4200 || (a==1 && getStopwatch2() ==0 && getStopwatch3() ==0))
{changeDirection(BWD);
moveAtSpeed(50,50);
stopStopwatch2();
setStopwatch2(0);
writeString("RP6 fahren\n");}

if (a==0)
{moveAtSpeed(0,0);
stopStopwatch3();
setStopwatch3(0);
writeString("RP6 Stillstand\n");}
}







int main(void)
{
initRP6Control();

I2CTWI_initMaster(100);


DDRD &=~ (1<<6); //Eingang für Reflexkoppler

initSERVO(SERVO1|SERVO2|SERVO3|SERVO5);
servo1_position = RIGHT_TOUCH;
servo2_position = 150;
servo3_position = RIGHT_TOUCH;
servo5_position = MIDDLE_POSITION;


while(true)
{task_servoansteuerung();

task_teil_erkannt();
RP6_Bewegung();

task_SERVO();

mSleep(3);
}
return 0;
}


einziger wehrmutstropfen ist die tatsache, dass einer meiner servos, wohl aufgrund meines zu schweren armes einen getriebeschaden hat, weshalb aus meinem ursprünglichen 5- achs-arm nun ein 4- achs-arm wurde.

das versprochene video werde ich in den kommenden tagen wie versprochen online stellen.

was mich abschließend noch brennend interessieren würde, ist die frage, warum ich die stopwatches, wie in meinem fall erst mit stopStopwatchx() stoppen und anschließend mit setStopwatchx(0); rücksetzen muss, damit sie nicht von alleine wieder starten und nicht so, wie man es sonst gewohnt ist, ohne stopStopwatchx();.

vielleicht hat ja jemand aus dem forum eine erklärung dazu...

mfg andi

[proevofreak]

so nun hier zum abschluss meines 4- achs roboterarmprojekts das versprochene video:

http://www.youtube.com/watch?v=gQIsf_iKPC0

vielleicht konnte ich ja den einen oder anderen zum nachbauen animieren.

mfg
andi

[Robotuner]

Hallo
dein Roboterarm ist ja coll.
Wie schwere Sachen kann man damiteigentlich heben ?

[proevofreak]

hallo Robotuner,
mit dem meinem arm lassen nur sachen zwischen geschätzt 0- 30 gramm anheben.
das liegt aber mitunter an meinen verwendeten billigservos.
es gibt auch servos mit wesentlich höheren drehmomenten, allerdings aber auch zu einem höheren preis.

es liegt also so ziemlich an dir, wie schwere sachen du mit deinem arm heben kannst.