MRT_Studienarbeit - This program control a 5 axis arm robot from lynx…

Users » msimmerl » Code » MRT_Studienarbeit

Simmerl Manuel / Mbed 2 deprecated MRT_Studienarbeit

This program control a 5 axis arm robot from lynx motion

main.cpp@0:b6608b36efd7, 2011-02-15 (annotated)

Committer:: msimmerl
Date:: Tue Feb 15 07:49:25 2011 +0000
Revision:: 0:b6608b36efd7

Who changed what in which revision?

User	Revision	Line number	New contents of line
msimmerl	0:b6608b36efd7	1	#include <mbed.h>
msimmerl	0:b6608b36efd7	2	#include <scmRTOS.h>
msimmerl	0:b6608b36efd7	3	#include "main.h"
msimmerl	0:b6608b36efd7	4
msimmerl	0:b6608b36efd7	5	//Serielle Schnittstelle
msimmerl	0:b6608b36efd7	6	Serial pc(USBTX, USBRX);
msimmerl	0:b6608b36efd7	7
msimmerl	0:b6608b36efd7	8	//LCD
msimmerl	0:b6608b36efd7	9	TextLCD lcd(p14, p16, p17, p15, p19, p20); // rs, e, d4, d5, d6, d7
msimmerl	0:b6608b36efd7	10	// Defines LCD bus to write data.
msimmerl	0:b6608b36efd7	11	BusOut Lcd_pins(p17, p18, p19, p20); // d4, d5, d6, d7
msimmerl	0:b6608b36efd7	12
msimmerl	0:b6608b36efd7	13	DigitalOut rs_pin(p14); // LCD pin rs (register select.)
msimmerl	0:b6608b36efd7	14	DigitalOut e_pin(p16); // LCD pin e (enable).
msimmerl	0:b6608b36efd7	15
msimmerl	0:b6608b36efd7	16	//Servos
msimmerl	0:b6608b36efd7	17	PwmOut arm_hinten(p21);
msimmerl	0:b6608b36efd7	18	PwmOut arm_vorne(p22);
msimmerl	0:b6608b36efd7	19	PwmOut drehen(p23);
msimmerl	0:b6608b36efd7	20	PwmOut klaue(p24);
msimmerl	0:b6608b36efd7	21	PwmOut heben1(p25);
msimmerl	0:b6608b36efd7	22	PwmOut heben2(p26);
msimmerl	0:b6608b36efd7	23
msimmerl	0:b6608b36efd7	24	//IO
msimmerl	0:b6608b36efd7	25	//DigitalOut myled1(LED1);
msimmerl	0:b6608b36efd7	26	//DigitalOut myled2(LED2);
msimmerl	0:b6608b36efd7	27	DigitalIn key_start(p13);
msimmerl	0:b6608b36efd7	28	DigitalIn key_menu_up(p11);
msimmerl	0:b6608b36efd7	29	DigitalIn key_menu_down(p12);
msimmerl	0:b6608b36efd7	30	DigitalIn key_stop(p10);
msimmerl	0:b6608b36efd7	31
msimmerl	0:b6608b36efd7	32	//Prozesse
msimmerl	0:b6608b36efd7	33	typedef OS::process<OS::pr0, 1000> T_Roboter;
msimmerl	0:b6608b36efd7	34	typedef OS::process<OS::pr1, 1000> T_IO;
msimmerl	0:b6608b36efd7	35
msimmerl	0:b6608b36efd7	36	T_Roboter pRoboter;
msimmerl	0:b6608b36efd7	37	T_IO pIO;
msimmerl	0:b6608b36efd7	38
msimmerl	0:b6608b36efd7	39	void Programm_1(void);
msimmerl	0:b6608b36efd7	40	void Programm_2(void);
msimmerl	0:b6608b36efd7	41	void InitPosition(void);
msimmerl	0:b6608b36efd7	42	void InitServos(void);
msimmerl	0:b6608b36efd7	43
msimmerl	0:b6608b36efd7	44	float posKlaue = 0.0015;
msimmerl	0:b6608b36efd7	45	float posArmVorne = 0.0015;
msimmerl	0:b6608b36efd7	46	float posArmHinten = 0.0015;
msimmerl	0:b6608b36efd7	47	float posHeben = 0.0015;
msimmerl	0:b6608b36efd7	48	float posDrehen = 0.00185;
msimmerl	0:b6608b36efd7	49
msimmerl	0:b6608b36efd7	50	long long count = 0;
msimmerl	0:b6608b36efd7	51	int action = 0;
msimmerl	0:b6608b36efd7	52	int running = 0;
msimmerl	0:b6608b36efd7	53
msimmerl	0:b6608b36efd7	54	int menu_counter = 0;
msimmerl	0:b6608b36efd7	55
msimmerl	0:b6608b36efd7	56	char input;
msimmerl	0:b6608b36efd7	57
msimmerl	0:b6608b36efd7	58	bool isStop = false;
msimmerl	0:b6608b36efd7	59	bool isInit = false;
msimmerl	0:b6608b36efd7	60
msimmerl	0:b6608b36efd7	61	bool keyPressed = false;
msimmerl	0:b6608b36efd7	62	int keyCounter = 0;
msimmerl	0:b6608b36efd7	63
msimmerl	0:b6608b36efd7	64	int main() {
msimmerl	0:b6608b36efd7	65	pc.baud(115200);
msimmerl	0:b6608b36efd7	66	printf("\r\nInitialisierung...\r\n");
msimmerl	0:b6608b36efd7	67	lcd.cls();
msimmerl	0:b6608b36efd7	68	lcd.printf("Lade...");
msimmerl	0:b6608b36efd7	69	InitServos();
msimmerl	0:b6608b36efd7	70	InitPosition();
msimmerl	0:b6608b36efd7	71	wait(2);
msimmerl	0:b6608b36efd7	72	lcd.locate(0, 0);
msimmerl	0:b6608b36efd7	73	lcd.printf("Lade...fertig");
msimmerl	0:b6608b36efd7	74	wait(2);
msimmerl	0:b6608b36efd7	75	lcd.locate(0, 0);
msimmerl	0:b6608b36efd7	76	lcd.printf("Programm waehlen");
msimmerl	0:b6608b36efd7	77	lcd.locate(0, 1);
msimmerl	0:b6608b36efd7	78	lcd.printf(texte[0]);
msimmerl	0:b6608b36efd7	79	OS::Run();
msimmerl	0:b6608b36efd7	80	}
msimmerl	0:b6608b36efd7	81
msimmerl	0:b6608b36efd7	82	template<> OS_PROCESS void T_Roboter::Exec() {
msimmerl	0:b6608b36efd7	83	while(1) {
msimmerl	0:b6608b36efd7	84	if(running == 1 && menu_counter == 1){
msimmerl	0:b6608b36efd7	85	Programm_1();
msimmerl	0:b6608b36efd7	86	running = 0;
msimmerl	0:b6608b36efd7	87	}else if(running == 1 && menu_counter == 2){
msimmerl	0:b6608b36efd7	88	Programm_2();
msimmerl	0:b6608b36efd7	89	running = 0;
msimmerl	0:b6608b36efd7	90	}
msimmerl	0:b6608b36efd7	91	Sleep(100);
msimmerl	0:b6608b36efd7	92	}
msimmerl	0:b6608b36efd7	93	}
msimmerl	0:b6608b36efd7	94
msimmerl	0:b6608b36efd7	95	template<> OS_PROCESS void T_IO::Exec() {
msimmerl	0:b6608b36efd7	96	while(1) {
msimmerl	0:b6608b36efd7	97	//pc.printf("Hello World\r\n");
msimmerl	0:b6608b36efd7	98	/*if(start == 1 && running == 0){
msimmerl	0:b6608b36efd7	99	running = 1;
msimmerl	0:b6608b36efd7	100	lcd.cls();
msimmerl	0:b6608b36efd7	101	lcd.locate(0,0);
msimmerl	0:b6608b36efd7	102	lcd.printf("running...");
msimmerl	0:b6608b36efd7	103	}
msimmerl	0:b6608b36efd7	104	if(start == 1){
msimmerl	0:b6608b36efd7	105	pc.printf("Hallo Welt!\r\n");
msimmerl	0:b6608b36efd7	106	}*/
msimmerl	0:b6608b36efd7	107	Sleep(1000);
msimmerl	0:b6608b36efd7	108	}
msimmerl	0:b6608b36efd7	109	}
msimmerl	0:b6608b36efd7	110
msimmerl	0:b6608b36efd7	111
msimmerl	0:b6608b36efd7	112	//---------------------------------------------------------------------------
msimmerl	0:b6608b36efd7	113	void OS::SystemTimerUserHook() {
msimmerl	0:b6608b36efd7	114	++count;
msimmerl	0:b6608b36efd7	115	if (count % 150 == 0) {
msimmerl	0:b6608b36efd7	116	if(key_start == 1 && running == 0 && menu_counter == 1 && !keyPressed){
msimmerl	0:b6608b36efd7	117	running = 1;
msimmerl	0:b6608b36efd7	118	lcd.cls();
msimmerl	0:b6608b36efd7	119	lcd.locate(0,0);
msimmerl	0:b6608b36efd7	120	lcd.printf("Running...P1");
msimmerl	0:b6608b36efd7	121	lcd.locate(0, 1);
msimmerl	0:b6608b36efd7	122	lcd.printf("Menu ->Press key");
msimmerl	0:b6608b36efd7	123	}else if(key_start == 1 && running == 0 && menu_counter == 2 && !keyPressed) {
msimmerl	0:b6608b36efd7	124	running = 1;
msimmerl	0:b6608b36efd7	125	lcd.cls();
msimmerl	0:b6608b36efd7	126	lcd.locate(0,0);
msimmerl	0:b6608b36efd7	127	lcd.printf("Running...P2");
msimmerl	0:b6608b36efd7	128	lcd.locate(0, 1);
msimmerl	0:b6608b36efd7	129	lcd.printf("Menu ->Press key");
msimmerl	0:b6608b36efd7	130	}else if(key_start == 1 && running == 1 && menu_counter == 0 && !keyPressed) {
msimmerl	0:b6608b36efd7	131	switch(menu_counter){
msimmerl	0:b6608b36efd7	132	case 0: isInit = true; break;
msimmerl	0:b6608b36efd7	133	}
msimmerl	0:b6608b36efd7	134	}
msimmerl	0:b6608b36efd7	135
msimmerl	0:b6608b36efd7	136	if(key_menu_up == 1 && menu_counter < 3 && !keyPressed) {
msimmerl	0:b6608b36efd7	137	menu_counter++;
msimmerl	0:b6608b36efd7	138	lcd.locate(0, 1);
msimmerl	0:b6608b36efd7	139	lcd.printf(texte[menu_counter]);
msimmerl	0:b6608b36efd7	140	}
msimmerl	0:b6608b36efd7	141
msimmerl	0:b6608b36efd7	142	if(key_menu_down == 1 && menu_counter > 0 && !keyPressed){
msimmerl	0:b6608b36efd7	143	menu_counter--;
msimmerl	0:b6608b36efd7	144	lcd.locate(0, 1);
msimmerl	0:b6608b36efd7	145	lcd.printf(texte[menu_counter]);
msimmerl	0:b6608b36efd7	146	}
msimmerl	0:b6608b36efd7	147
msimmerl	0:b6608b36efd7	148	if(key_stop == 1 && running == 1 && menu_counter == 3 && !keyPressed){
msimmerl	0:b6608b36efd7	149	isInit = true;
msimmerl	0:b6608b36efd7	150	}
msimmerl	0:b6608b36efd7	151
msimmerl	0:b6608b36efd7	152	if(keyPressed){
msimmerl	0:b6608b36efd7	153	if(keyCounter < 4){
msimmerl	0:b6608b36efd7	154	keyCounter++;
msimmerl	0:b6608b36efd7	155	}else{
msimmerl	0:b6608b36efd7	156	keyCounter = 0;
msimmerl	0:b6608b36efd7	157	keyPressed = false;
msimmerl	0:b6608b36efd7	158	}
msimmerl	0:b6608b36efd7	159	}
msimmerl	0:b6608b36efd7	160	if(pc.readable()){
msimmerl	0:b6608b36efd7	161	input = pc.getc();
msimmerl	0:b6608b36efd7	162	switch(input) {
msimmerl	0:b6608b36efd7	163	case 'a': posKlaue = posKlaue + 0.00001; klaue.pulsewidth(posKlaue); pc.printf("Klaue pulsewidth: %lf\r\n", posKlaue); break;
msimmerl	0:b6608b36efd7	164	case 'd': posKlaue = posKlaue - 0.00001; klaue.pulsewidth(posKlaue); pc.printf("Klaue pulsewidth: %lf\r\n", posKlaue); break;
msimmerl	0:b6608b36efd7	165	case 'w': posArmVorne = posArmVorne + 0.00001; arm_vorne.pulsewidth(posArmVorne); pc.printf("Arm vorne pulsewidth: %lf\r\n", posArmVorne); break;
msimmerl	0:b6608b36efd7	166	case 's': posArmVorne = posArmVorne - 0.00001; arm_vorne.pulsewidth(posArmVorne); pc.printf("Arm vorne pulsewidth: %lf\r\n", posArmVorne); break;
msimmerl	0:b6608b36efd7	167	case 'i': posArmHinten = posArmHinten + 0.00001; arm_hinten.pulsewidth(posArmHinten); pc.printf("Arm hinten pulsewidth: %lf\r\n", posArmHinten); break;
msimmerl	0:b6608b36efd7	168	case 'k': posArmHinten = posArmHinten - 0.00001; arm_hinten.pulsewidth(posArmHinten); pc.printf("Arm hinten pulsewidth: %lf\r\n", posArmHinten); break;
msimmerl	0:b6608b36efd7	169	case 'j': posHeben = posHeben + 0.00001; heben1.pulsewidth(posHeben); heben2.pulsewidth(posHeben); pc.printf("Heben pulsewidth: %lf\r\n", posHeben); break;
msimmerl	0:b6608b36efd7	170	case 'l': posHeben = posHeben - 0.00001; heben1.pulsewidth(posHeben); heben2.pulsewidth(posHeben); pc.printf("Heben pulsewidth: %lf\r\n", posHeben); break;
msimmerl	0:b6608b36efd7	171	case 'v': posDrehen = posDrehen + 0.00001; drehen.pulsewidth(posDrehen); drehen.pulsewidth(posDrehen); pc.printf("Drehen pulsewidth: %lf\r\n", posDrehen); break;
msimmerl	0:b6608b36efd7	172	case 'b': posDrehen = posDrehen - 0.00001; drehen.pulsewidth(posDrehen); drehen.pulsewidth(posDrehen); pc.printf("Drehen pulsewidth: %lf\r\n", posDrehen); break;
msimmerl	0:b6608b36efd7	173	default: pc.printf("Nicht definiertes Zeichen: %i\r\n", input);
msimmerl	0:b6608b36efd7	174	}
msimmerl	0:b6608b36efd7	175	}
msimmerl	0:b6608b36efd7	176	count = 0;
msimmerl	0:b6608b36efd7	177	}
msimmerl	0:b6608b36efd7	178	}
msimmerl	0:b6608b36efd7	179
msimmerl	0:b6608b36efd7	180	//---------------------------------------------------------------------------
msimmerl	0:b6608b36efd7	181	void OS::IdleProcessUserHook() {
msimmerl	0:b6608b36efd7	182	__WFI();
msimmerl	0:b6608b36efd7	183	}
msimmerl	0:b6608b36efd7	184	void Programm_2(void) {
msimmerl	0:b6608b36efd7	185	int i = 0;
msimmerl	0:b6608b36efd7	186	pc.printf("\r\n");
msimmerl	0:b6608b36efd7	187	for(i = 0; i < 10; i++){
msimmerl	0:b6608b36efd7	188	pc.printf("%i Durchgang\r\n", (i + 1));
msimmerl	0:b6608b36efd7	189
msimmerl	0:b6608b36efd7	190	//Hole Teil 1
msimmerl	0:b6608b36efd7	191	posDrehen = Drive(drehen, posDrehen, 0.00095, SPEED_FAST);
msimmerl	0:b6608b36efd7	192	posKlaue = Drive(klaue, posKlaue, 0.001920, SPEED_FAST);
msimmerl	0:b6608b36efd7	193	posArmHinten = Drive(arm_hinten, posArmHinten, 0.001800, SPEED_FAST);
msimmerl	0:b6608b36efd7	194	posArmVorne = Drive(arm_vorne, posArmVorne, 0.002290, SPEED_FAST);
msimmerl	0:b6608b36efd7	195	posHeben = Drive(heben1, heben2, posHeben, 0.000990, SPEED_FAST);
msimmerl	0:b6608b36efd7	196	posKlaue = Drive(klaue, posKlaue, 0.001210, SPEED_FAST);
msimmerl	0:b6608b36efd7	197
msimmerl	0:b6608b36efd7	198	if(isInit){
msimmerl	0:b6608b36efd7	199	isInit = false;
msimmerl	0:b6608b36efd7	200	Fahre_zu_Init_Pos();
msimmerl	0:b6608b36efd7	201	running = 0;
msimmerl	0:b6608b36efd7	202	break;
msimmerl	0:b6608b36efd7	203	}
msimmerl	0:b6608b36efd7	204
msimmerl	0:b6608b36efd7	205	//Fahre mit Teil 1 zu Pos 2
msimmerl	0:b6608b36efd7	206	posHeben = Drive(heben1, heben2, posHeben, 0.001430, SPEED_FAST);
msimmerl	0:b6608b36efd7	207	posDrehen = Drive(drehen, posDrehen, 0.002760, SPEED_FAST);
msimmerl	0:b6608b36efd7	208	posHeben = Drive(heben1, heben2, posHeben, 0.000990, SPEED_FAST);
msimmerl	0:b6608b36efd7	209	posKlaue = Drive(klaue, posKlaue, 0.001920, SPEED_FAST);
msimmerl	0:b6608b36efd7	210	posHeben = Drive(heben1, heben2, posHeben, 0.001430, SPEED_FAST);
msimmerl	0:b6608b36efd7	211
msimmerl	0:b6608b36efd7	212	if(isInit){
msimmerl	0:b6608b36efd7	213	isInit = false;
msimmerl	0:b6608b36efd7	214	Fahre_zu_Init_Pos();
msimmerl	0:b6608b36efd7	215	running = 0;
msimmerl	0:b6608b36efd7	216	break;
msimmerl	0:b6608b36efd7	217	}
msimmerl	0:b6608b36efd7	218
msimmerl	0:b6608b36efd7	219	//Hole Teil 2
msimmerl	0:b6608b36efd7	220	posDrehen = Drive(drehen, posDrehen, 0.00111, SPEED_FAST); //0.00095
msimmerl	0:b6608b36efd7	221	posKlaue = Drive(klaue, posKlaue, 0.001920, SPEED_FAST);
msimmerl	0:b6608b36efd7	222	posArmHinten = Drive(arm_hinten, posArmHinten, 0.001800, SPEED_FAST); //Alt 0.001700
msimmerl	0:b6608b36efd7	223	posArmVorne = Drive(arm_vorne, posArmVorne, 0.002290, SPEED_FAST); //Alt 0.002150
msimmerl	0:b6608b36efd7	224	posHeben = Drive(heben1, heben2, posHeben, 0.000990, SPEED_FAST); //Alt 0.000990
msimmerl	0:b6608b36efd7	225	posKlaue = Drive(klaue, posKlaue, 0.001190, SPEED_FAST);
msimmerl	0:b6608b36efd7	226
msimmerl	0:b6608b36efd7	227	if(isInit){
msimmerl	0:b6608b36efd7	228	isInit = false;
msimmerl	0:b6608b36efd7	229	Fahre_zu_Init_Pos();
msimmerl	0:b6608b36efd7	230	running = 0;
msimmerl	0:b6608b36efd7	231	break;
msimmerl	0:b6608b36efd7	232	}
msimmerl	0:b6608b36efd7	233
msimmerl	0:b6608b36efd7	234	//Fahre mit Teil 2 zu Pos 2
msimmerl	0:b6608b36efd7	235	posHeben = Drive(heben1, heben2, posHeben, 0.001430, SPEED_FAST);
msimmerl	0:b6608b36efd7	236	posDrehen = Drive(drehen, posDrehen, 0.00260, SPEED_FAST); //0.00185
msimmerl	0:b6608b36efd7	237	posHeben = Drive(heben1, heben2, posHeben, 0.000990, SPEED_FAST);
msimmerl	0:b6608b36efd7	238	posKlaue = Drive(klaue, posKlaue, 0.001920, SPEED_FAST); //0.0015
msimmerl	0:b6608b36efd7	239	posHeben = Drive(heben1, heben2, posHeben, 0.001430, SPEED_FAST);
msimmerl	0:b6608b36efd7	240
msimmerl	0:b6608b36efd7	241	if(isInit){
msimmerl	0:b6608b36efd7	242	isInit = false;
msimmerl	0:b6608b36efd7	243	Fahre_zu_Init_Pos();
msimmerl	0:b6608b36efd7	244	running = 0;
msimmerl	0:b6608b36efd7	245	break;
msimmerl	0:b6608b36efd7	246	}
msimmerl	0:b6608b36efd7	247
msimmerl	0:b6608b36efd7	248	//Hole Teil 3
msimmerl	0:b6608b36efd7	249	posDrehen = Drive(drehen, posDrehen, 0.00127, SPEED_FAST); //0.00095
msimmerl	0:b6608b36efd7	250	posKlaue = Drive(klaue, posKlaue, 0.001920, SPEED_FAST);
msimmerl	0:b6608b36efd7	251	posArmHinten = Drive(arm_hinten, posArmHinten, 0.001800, SPEED_FAST); //Alt 0.001700
msimmerl	0:b6608b36efd7	252	posArmVorne = Drive(arm_vorne, posArmVorne, 0.002290, SPEED_FAST); //Alt 0.002150
msimmerl	0:b6608b36efd7	253	posHeben = Drive(heben1, heben2, posHeben, 0.000990, SPEED_FAST); //Alt 0.000990
msimmerl	0:b6608b36efd7	254	posKlaue = Drive(klaue, posKlaue, 0.001190, SPEED_FAST);
msimmerl	0:b6608b36efd7	255
msimmerl	0:b6608b36efd7	256	if(isInit){
msimmerl	0:b6608b36efd7	257	isInit = false;
msimmerl	0:b6608b36efd7	258	Fahre_zu_Init_Pos();
msimmerl	0:b6608b36efd7	259	running = 0;
msimmerl	0:b6608b36efd7	260	break;
msimmerl	0:b6608b36efd7	261	}
msimmerl	0:b6608b36efd7	262
msimmerl	0:b6608b36efd7	263	//Fahre mit Teil 3 zu Pos 2
msimmerl	0:b6608b36efd7	264	posHeben = Drive(heben1, heben2, posHeben, 0.001430, SPEED_FAST);
msimmerl	0:b6608b36efd7	265	posDrehen = Drive(drehen, posDrehen, 0.00244, SPEED_FAST); //0.00185
msimmerl	0:b6608b36efd7	266	posHeben = Drive(heben1, heben2, posHeben, 0.000990, SPEED_FAST);
msimmerl	0:b6608b36efd7	267	posKlaue = Drive(klaue, posKlaue, 0.001920, SPEED_FAST); //0.0015
msimmerl	0:b6608b36efd7	268	posHeben = Drive(heben1, heben2, posHeben, 0.001430, SPEED_FAST);
msimmerl	0:b6608b36efd7	269
msimmerl	0:b6608b36efd7	270	//Fahre mitte
msimmerl	0:b6608b36efd7	271	posDrehen = Drive(drehen, posDrehen, 0.00185, SPEED_FAST); //0.00185
msimmerl	0:b6608b36efd7	272
msimmerl	0:b6608b36efd7	273	if(isInit){
msimmerl	0:b6608b36efd7	274	isInit = false;
msimmerl	0:b6608b36efd7	275	Fahre_zu_Init_Pos();
msimmerl	0:b6608b36efd7	276	running = 0;
msimmerl	0:b6608b36efd7	277	break;
msimmerl	0:b6608b36efd7	278	}
msimmerl	0:b6608b36efd7	279
msimmerl	0:b6608b36efd7	280	//Hole Teil 2
msimmerl	0:b6608b36efd7	281	posDrehen = Drive(drehen, posDrehen, 0.00260, SPEED_FAST);
msimmerl	0:b6608b36efd7	282	posKlaue = Drive(klaue, posKlaue, 0.001920, SPEED_FAST);
msimmerl	0:b6608b36efd7	283	posArmHinten = Drive(arm_hinten, posArmHinten, 0.001800, SPEED_FAST);
msimmerl	0:b6608b36efd7	284	posArmVorne = Drive(arm_vorne, posArmVorne, 0.002290, SPEED_FAST);
msimmerl	0:b6608b36efd7	285	posHeben = Drive(heben1, heben2, posHeben, 0.000990, SPEED_FAST);
msimmerl	0:b6608b36efd7	286	posKlaue = Drive(klaue, posKlaue, 0.001190, SPEED_FAST);
msimmerl	0:b6608b36efd7	287
msimmerl	0:b6608b36efd7	288	if(isInit){
msimmerl	0:b6608b36efd7	289	isInit = false;
msimmerl	0:b6608b36efd7	290	Fahre_zu_Init_Pos();
msimmerl	0:b6608b36efd7	291	running = 0;
msimmerl	0:b6608b36efd7	292	break;
msimmerl	0:b6608b36efd7	293	}
msimmerl	0:b6608b36efd7	294
msimmerl	0:b6608b36efd7	295	//Fahre zu Pos 3
msimmerl	0:b6608b36efd7	296	posHeben = Drive(heben1, heben2, posHeben, 0.001430, SPEED_FAST);
msimmerl	0:b6608b36efd7	297	posDrehen = Drive(drehen, posDrehen, 0.00127, SPEED_FAST);
msimmerl	0:b6608b36efd7	298	posArmHinten = Drive(arm_hinten, posArmHinten, 0.001800, SPEED_FAST);
msimmerl	0:b6608b36efd7	299	posArmVorne = Drive(arm_vorne, posArmVorne, 0.002290, SPEED_FAST);
msimmerl	0:b6608b36efd7	300	posHeben = Drive(heben1, heben2, posHeben, 0.000990, SPEED_FAST);
msimmerl	0:b6608b36efd7	301	posKlaue = Drive(klaue, posKlaue, 0.001920, SPEED_FAST);
msimmerl	0:b6608b36efd7	302
msimmerl	0:b6608b36efd7	303	if(isInit){
msimmerl	0:b6608b36efd7	304	isInit = false;
msimmerl	0:b6608b36efd7	305	Fahre_zu_Init_Pos();
msimmerl	0:b6608b36efd7	306	running = 0;
msimmerl	0:b6608b36efd7	307	break;
msimmerl	0:b6608b36efd7	308	}
msimmerl	0:b6608b36efd7	309
msimmerl	0:b6608b36efd7	310	//Hole Teil 3
msimmerl	0:b6608b36efd7	311	posHeben = Drive(heben1, heben2, posHeben, 0.001430, SPEED_FAST);
msimmerl	0:b6608b36efd7	312	posDrehen = Drive(drehen, posDrehen, 0.00244, SPEED_FAST);
msimmerl	0:b6608b36efd7	313	posArmHinten = Drive(arm_hinten, posArmHinten, 0.001800, SPEED_FAST);
msimmerl	0:b6608b36efd7	314	posArmVorne = Drive(arm_vorne, posArmVorne, 0.002290, SPEED_FAST);
msimmerl	0:b6608b36efd7	315	posHeben = Drive(heben1, heben2, posHeben, 0.000990, SPEED_FAST);
msimmerl	0:b6608b36efd7	316	posKlaue = Drive(klaue, posKlaue, 0.001190, SPEED_FAST);
msimmerl	0:b6608b36efd7	317
msimmerl	0:b6608b36efd7	318	if(isInit){
msimmerl	0:b6608b36efd7	319	isInit = false;
msimmerl	0:b6608b36efd7	320	Fahre_zu_Init_Pos();
msimmerl	0:b6608b36efd7	321	running = 0;
msimmerl	0:b6608b36efd7	322	break;
msimmerl	0:b6608b36efd7	323	}
msimmerl	0:b6608b36efd7	324
msimmerl	0:b6608b36efd7	325	//Fahre zu Pos 1
msimmerl	0:b6608b36efd7	326	posHeben = Drive(heben1, heben2, posHeben, 0.001430, SPEED_FAST);
msimmerl	0:b6608b36efd7	327	posDrehen = Drive(drehen, posDrehen, 0.00095, SPEED_FAST);
msimmerl	0:b6608b36efd7	328	posArmHinten = Drive(arm_hinten, posArmHinten, 0.001800, SPEED_FAST);
msimmerl	0:b6608b36efd7	329	posArmVorne = Drive(arm_vorne, posArmVorne, 0.002290, SPEED_FAST);
msimmerl	0:b6608b36efd7	330	posHeben = Drive(heben1, heben2, posHeben, 0.000990, SPEED_FAST);
msimmerl	0:b6608b36efd7	331	posKlaue = Drive(klaue, posKlaue, 0.001920, SPEED_FAST);
msimmerl	0:b6608b36efd7	332
msimmerl	0:b6608b36efd7	333	if(isInit){
msimmerl	0:b6608b36efd7	334	isInit = false;
msimmerl	0:b6608b36efd7	335	Fahre_zu_Init_Pos();
msimmerl	0:b6608b36efd7	336	running = 0;
msimmerl	0:b6608b36efd7	337	break;
msimmerl	0:b6608b36efd7	338	}
msimmerl	0:b6608b36efd7	339
msimmerl	0:b6608b36efd7	340	//Hole Teil 1
msimmerl	0:b6608b36efd7	341	posHeben = Drive(heben1, heben2, posHeben, 0.001430, SPEED_FAST);
msimmerl	0:b6608b36efd7	342	posDrehen = Drive(drehen, posDrehen, 0.002760, SPEED_FAST);
msimmerl	0:b6608b36efd7	343	posArmHinten = Drive(arm_hinten, posArmHinten, 0.001800, SPEED_FAST);
msimmerl	0:b6608b36efd7	344	posArmVorne = Drive(arm_vorne, posArmVorne, 0.002290, SPEED_FAST);
msimmerl	0:b6608b36efd7	345	posHeben = Drive(heben1, heben2, posHeben, 0.000990, SPEED_FAST);
msimmerl	0:b6608b36efd7	346	posKlaue = Drive(klaue, posKlaue, 0.001190, SPEED_FAST);
msimmerl	0:b6608b36efd7	347
msimmerl	0:b6608b36efd7	348	if(isInit){
msimmerl	0:b6608b36efd7	349	isInit = false;
msimmerl	0:b6608b36efd7	350	Fahre_zu_Init_Pos();
msimmerl	0:b6608b36efd7	351	running = 0;
msimmerl	0:b6608b36efd7	352	break;
msimmerl	0:b6608b36efd7	353	}
msimmerl	0:b6608b36efd7	354
msimmerl	0:b6608b36efd7	355	//Fahre zu Pos 1
msimmerl	0:b6608b36efd7	356	posHeben = Drive(heben1, heben2, posHeben, 0.001430, SPEED_FAST);
msimmerl	0:b6608b36efd7	357	posDrehen = Drive(drehen, posDrehen, 0.00111, SPEED_FAST);
msimmerl	0:b6608b36efd7	358	posArmHinten = Drive(arm_hinten, posArmHinten, 0.001800, SPEED_FAST);
msimmerl	0:b6608b36efd7	359	posArmVorne = Drive(arm_vorne, posArmVorne, 0.002290, SPEED_FAST);
msimmerl	0:b6608b36efd7	360	posHeben = Drive(heben1, heben2, posHeben, 0.000990, SPEED_FAST);
msimmerl	0:b6608b36efd7	361	posKlaue = Drive(klaue, posKlaue, 0.001920, SPEED_FAST);
msimmerl	0:b6608b36efd7	362
msimmerl	0:b6608b36efd7	363	Fahre_zu_Init_Pos();
msimmerl	0:b6608b36efd7	364	}
msimmerl	0:b6608b36efd7	365	lcd.locate(0, 0);
msimmerl	0:b6608b36efd7	366	lcd.printf("Programm waehlen");
msimmerl	0:b6608b36efd7	367	lcd.locate(0, 1);
msimmerl	0:b6608b36efd7	368	lcd.printf(texte[menu_counter]);
msimmerl	0:b6608b36efd7	369	}
msimmerl	0:b6608b36efd7	370	void Programm_1(void) {
msimmerl	0:b6608b36efd7	371	int i = 0;
msimmerl	0:b6608b36efd7	372	pc.printf("\r\n");
msimmerl	0:b6608b36efd7	373	for(i = 0; i < 10; i++){
msimmerl	0:b6608b36efd7	374	pc.printf("%i Durchgang\r\n", (i + 1));
msimmerl	0:b6608b36efd7	375
msimmerl	0:b6608b36efd7	376	//Hole Teil 1
msimmerl	0:b6608b36efd7	377	posDrehen = Drive(drehen, posDrehen, 0.00095, SPEED_MEDIUM); //Nur zum Testen: Original: 0.00095
msimmerl	0:b6608b36efd7	378	posKlaue = Drive(klaue, posKlaue, 0.001920, SPEED_MEDIUM);
msimmerl	0:b6608b36efd7	379	posArmHinten = Drive(arm_hinten, posArmHinten, 0.001800, SPEED_MEDIUM); //Alt 0.0017
msimmerl	0:b6608b36efd7	380	posArmVorne = Drive(arm_vorne, posArmVorne, 0.002290, SPEED_MEDIUM); //Alt 0.002150
msimmerl	0:b6608b36efd7	381	posHeben = Drive(heben1, heben2, posHeben, 0.000990, SPEED_MEDIUM); //Alt 0.00099
msimmerl	0:b6608b36efd7	382	posKlaue = Drive(klaue, posKlaue, 0.001210, SPEED_MEDIUM);
msimmerl	0:b6608b36efd7	383
msimmerl	0:b6608b36efd7	384	if(isInit){
msimmerl	0:b6608b36efd7	385	isInit = false;
msimmerl	0:b6608b36efd7	386	Fahre_zu_Init_Pos();
msimmerl	0:b6608b36efd7	387	running = 0;
msimmerl	0:b6608b36efd7	388	break;
msimmerl	0:b6608b36efd7	389	}
msimmerl	0:b6608b36efd7	390
msimmerl	0:b6608b36efd7	391	//Fahre mit Teil 1 zu Pos 2
msimmerl	0:b6608b36efd7	392	posHeben = Drive(heben1, heben2, posHeben, 0.001430, SPEED_MEDIUM);
msimmerl	0:b6608b36efd7	393	posDrehen = Drive(drehen, posDrehen, 0.002760, SPEED_MEDIUM); //0.00185
msimmerl	0:b6608b36efd7	394	posHeben = Drive(heben1, heben2, posHeben, 0.000990, SPEED_MEDIUM);
msimmerl	0:b6608b36efd7	395	posKlaue = Drive(klaue, posKlaue, 0.001920, SPEED_MEDIUM); //0.0015
msimmerl	0:b6608b36efd7	396	posHeben = Drive(heben1, heben2, posHeben, 0.001430, SPEED_MEDIUM);
msimmerl	0:b6608b36efd7	397	posDrehen = Drive(drehen, posDrehen, 0.00185, SPEED_MEDIUM);
msimmerl	0:b6608b36efd7	398
msimmerl	0:b6608b36efd7	399	if(isInit){
msimmerl	0:b6608b36efd7	400	isInit = false;
msimmerl	0:b6608b36efd7	401	Fahre_zu_Init_Pos();
msimmerl	0:b6608b36efd7	402	running = 0;
msimmerl	0:b6608b36efd7	403	break;
msimmerl	0:b6608b36efd7	404	}
msimmerl	0:b6608b36efd7	405
msimmerl	0:b6608b36efd7	406	//Hole Teil 2
msimmerl	0:b6608b36efd7	407	posDrehen = Drive(drehen, posDrehen, 0.00111, SPEED_MEDIUM); //0.00095
msimmerl	0:b6608b36efd7	408	posKlaue = Drive(klaue, posKlaue, 0.001920, SPEED_MEDIUM);
msimmerl	0:b6608b36efd7	409	posArmHinten = Drive(arm_hinten, posArmHinten, 0.001800, SPEED_MEDIUM); //Alt 0.001700
msimmerl	0:b6608b36efd7	410	posArmVorne = Drive(arm_vorne, posArmVorne, 0.002290, SPEED_MEDIUM); //Alt 0.002150
msimmerl	0:b6608b36efd7	411	posHeben = Drive(heben1, heben2, posHeben, 0.000990, SPEED_MEDIUM); //Alt 0.000990
msimmerl	0:b6608b36efd7	412	posKlaue = Drive(klaue, posKlaue, 0.001190, SPEED_MEDIUM);
msimmerl	0:b6608b36efd7	413
msimmerl	0:b6608b36efd7	414	if(isInit){
msimmerl	0:b6608b36efd7	415	isInit = false;
msimmerl	0:b6608b36efd7	416	Fahre_zu_Init_Pos();
msimmerl	0:b6608b36efd7	417	running = 0;
msimmerl	0:b6608b36efd7	418	break;
msimmerl	0:b6608b36efd7	419	}
msimmerl	0:b6608b36efd7	420
msimmerl	0:b6608b36efd7	421	//Fahre mit Teil 2 zu Pos 2
msimmerl	0:b6608b36efd7	422	posHeben = Drive(heben1, heben2, posHeben, 0.001430, SPEED_MEDIUM);
msimmerl	0:b6608b36efd7	423	posDrehen = Drive(drehen, posDrehen, 0.002760, SPEED_MEDIUM); //0.00185
msimmerl	0:b6608b36efd7	424	posArmHinten = Drive(arm_hinten, posArmHinten, 0.00180, SPEED_MEDIUM);
msimmerl	0:b6608b36efd7	425	posHeben = Drive(heben1, heben2, posHeben, 0.00132, SPEED_SLOW);
msimmerl	0:b6608b36efd7	426	posArmVorne = Drive(arm_vorne, posArmVorne, 0.001970, SPEED_SLOW);
msimmerl	0:b6608b36efd7	427
msimmerl	0:b6608b36efd7	428	if(isInit){
msimmerl	0:b6608b36efd7	429	isInit = false;
msimmerl	0:b6608b36efd7	430	Fahre_zu_Init_Pos();
msimmerl	0:b6608b36efd7	431	running = 0;
msimmerl	0:b6608b36efd7	432	break;
msimmerl	0:b6608b36efd7	433	}
msimmerl	0:b6608b36efd7	434
msimmerl	0:b6608b36efd7	435	//Hole Teil 3
msimmerl	0:b6608b36efd7	436	posKlaue = Drive(klaue, posKlaue, 0.001920, SPEED_MEDIUM);
msimmerl	0:b6608b36efd7	437	posHeben = Drive(heben1, heben2, posHeben, 0.00145, SPEED_MEDIUM);
msimmerl	0:b6608b36efd7	438	posDrehen = Drive(drehen, posDrehen, 0.00127, SPEED_MEDIUM);
msimmerl	0:b6608b36efd7	439	posArmHinten = Drive(arm_hinten, posArmHinten, 0.001800, SPEED_MEDIUM); //Alt 0.001700
msimmerl	0:b6608b36efd7	440	posArmVorne = Drive(arm_vorne, posArmVorne, 0.002290, SPEED_MEDIUM); //Alt 0.002150
msimmerl	0:b6608b36efd7	441	posHeben = Drive(heben1, heben2, posHeben, 0.000990, SPEED_MEDIUM); //Alt 0.000990
msimmerl	0:b6608b36efd7	442	posKlaue = Drive(klaue, posKlaue, 0.001190, SPEED_MEDIUM);
msimmerl	0:b6608b36efd7	443
msimmerl	0:b6608b36efd7	444	if(isInit){
msimmerl	0:b6608b36efd7	445	isInit = false;
msimmerl	0:b6608b36efd7	446	Fahre_zu_Init_Pos();
msimmerl	0:b6608b36efd7	447	running = 0;
msimmerl	0:b6608b36efd7	448	break;
msimmerl	0:b6608b36efd7	449	}
msimmerl	0:b6608b36efd7	450
msimmerl	0:b6608b36efd7	451	//Fahre mit Teil 3 zu Pos 2
msimmerl	0:b6608b36efd7	452	posHeben = Drive(heben1, heben2, posHeben, 0.00150, SPEED_MEDIUM);
msimmerl	0:b6608b36efd7	453	posArmVorne = Drive(arm_vorne, posArmVorne, 0.00136, SPEED_MEDIUM);
msimmerl	0:b6608b36efd7	454	posArmHinten = Drive(arm_hinten, posArmHinten, 0.0014, SPEED_MEDIUM); //Alt 0.001700
msimmerl	0:b6608b36efd7	455	posDrehen = Drive(drehen, posDrehen, 0.002760, SPEED_MEDIUM);
msimmerl	0:b6608b36efd7	456	posHeben = Drive(heben1, heben2, posHeben, 0.001450, SPEED_SLOW);
msimmerl	0:b6608b36efd7	457	posArmVorne = Drive(arm_vorne, posArmVorne, 0.00153, SPEED_SLOW);
msimmerl	0:b6608b36efd7	458	posArmHinten = Drive(arm_hinten, posArmHinten, 0.00154, SPEED_SLOW);
msimmerl	0:b6608b36efd7	459	posKlaue = Drive(klaue, posKlaue, 0.001920, SPEED_MEDIUM);
msimmerl	0:b6608b36efd7	460
msimmerl	0:b6608b36efd7	461	if(isInit){
msimmerl	0:b6608b36efd7	462	isInit = false;
msimmerl	0:b6608b36efd7	463	Fahre_zu_Init_Pos();
msimmerl	0:b6608b36efd7	464	running = 0;
msimmerl	0:b6608b36efd7	465	break;
msimmerl	0:b6608b36efd7	466	}
msimmerl	0:b6608b36efd7	467	//Drehe zur Ausgangsposition
msimmerl	0:b6608b36efd7	468	posArmVorne = Drive(arm_vorne, posArmVorne, 0.00182, SPEED_MEDIUM);
msimmerl	0:b6608b36efd7	469	posDrehen = Drive(drehen, posDrehen, 0.00185, SPEED_MEDIUM);
msimmerl	0:b6608b36efd7	470
msimmerl	0:b6608b36efd7	471	//Hole Teil 3
msimmerl	0:b6608b36efd7	472	posDrehen = Drive(drehen, posDrehen, 0.002760, SPEED_MEDIUM);
msimmerl	0:b6608b36efd7	473	posArmVorne = Drive(arm_vorne, posArmVorne, 0.00153, SPEED_SLOW);
msimmerl	0:b6608b36efd7	474	posKlaue = Drive(klaue, posKlaue, 0.001190, SPEED_MEDIUM);
msimmerl	0:b6608b36efd7	475	posArmVorne = Drive(arm_vorne, posArmVorne, 0.00182, SPEED_SLOW);
msimmerl	0:b6608b36efd7	476
msimmerl	0:b6608b36efd7	477	if(isInit){
msimmerl	0:b6608b36efd7	478	isInit = false;
msimmerl	0:b6608b36efd7	479	Fahre_zu_Init_Pos();
msimmerl	0:b6608b36efd7	480	running = 0;
msimmerl	0:b6608b36efd7	481	break;
msimmerl	0:b6608b36efd7	482	}
msimmerl	0:b6608b36efd7	483
msimmerl	0:b6608b36efd7	484	//Fahre mit Teil 3 zu Pos 1
msimmerl	0:b6608b36efd7	485	posDrehen = Drive(drehen, posDrehen, 0.00127, SPEED_MEDIUM);
msimmerl	0:b6608b36efd7	486	posArmHinten = Drive(arm_hinten, posArmHinten, 0.001800, SPEED_SLOW); //Alt 0.001700
msimmerl	0:b6608b36efd7	487	posArmVorne = Drive(arm_vorne, posArmVorne, 0.002290, SPEED_SLOW); //Alt 0.002150
msimmerl	0:b6608b36efd7	488	posHeben = Drive(heben1, heben2, posHeben, 0.000990, SPEED_SLOW); //Alt 0.000990
msimmerl	0:b6608b36efd7	489	posKlaue = Drive(klaue, posKlaue, 0.001920, SPEED_MEDIUM);
msimmerl	0:b6608b36efd7	490
msimmerl	0:b6608b36efd7	491	if(isInit){
msimmerl	0:b6608b36efd7	492	isInit = false;
msimmerl	0:b6608b36efd7	493	Fahre_zu_Init_Pos();
msimmerl	0:b6608b36efd7	494	running = 0;
msimmerl	0:b6608b36efd7	495	break;
msimmerl	0:b6608b36efd7	496	}
msimmerl	0:b6608b36efd7	497
msimmerl	0:b6608b36efd7	498	//Hole Teil 2
msimmerl	0:b6608b36efd7	499	posHeben = Drive(heben1, heben2, posHeben, 0.001430, SPEED_MEDIUM);
msimmerl	0:b6608b36efd7	500	posDrehen = Drive(drehen, posDrehen, 0.002760, SPEED_MEDIUM); //0.00185
msimmerl	0:b6608b36efd7	501	posArmHinten = Drive(arm_hinten, posArmHinten, 0.00180, SPEED_MEDIUM);
msimmerl	0:b6608b36efd7	502	posHeben = Drive(heben1, heben2, posHeben, 0.00132, SPEED_SLOW);
msimmerl	0:b6608b36efd7	503	posArmVorne = Drive(arm_vorne, posArmVorne, 0.001970, SPEED_SLOW);
msimmerl	0:b6608b36efd7	504	posKlaue = Drive(klaue, posKlaue, 0.001190, SPEED_MEDIUM);
msimmerl	0:b6608b36efd7	505
msimmerl	0:b6608b36efd7	506	if(isInit){
msimmerl	0:b6608b36efd7	507	isInit = false;
msimmerl	0:b6608b36efd7	508	Fahre_zu_Init_Pos();
msimmerl	0:b6608b36efd7	509	running = 0;
msimmerl	0:b6608b36efd7	510	break;
msimmerl	0:b6608b36efd7	511	}
msimmerl	0:b6608b36efd7	512
msimmerl	0:b6608b36efd7	513	//Fahre mit Teil 2 zu Pos 1
msimmerl	0:b6608b36efd7	514	posArmHinten = Drive(arm_hinten, posArmHinten, 0.001700, SPEED_MEDIUM); //Alt 0.001700
msimmerl	0:b6608b36efd7	515	posDrehen = Drive(drehen, posDrehen, 0.00111, SPEED_MEDIUM); //0.00095
msimmerl	0:b6608b36efd7	516	posArmHinten = Drive(arm_hinten, posArmHinten, 0.001800, SPEED_SLOW);
msimmerl	0:b6608b36efd7	517	posArmVorne = Drive(arm_vorne, posArmVorne, 0.002290, SPEED_SLOW); //Alt 0.002150
msimmerl	0:b6608b36efd7	518	posHeben = Drive(heben1, heben2, posHeben, 0.000990, SPEED_SLOW); //Alt 0.000990
msimmerl	0:b6608b36efd7	519	posKlaue = Drive(klaue, posKlaue, 0.001920, SPEED_MEDIUM);
msimmerl	0:b6608b36efd7	520
msimmerl	0:b6608b36efd7	521	if(isInit){
msimmerl	0:b6608b36efd7	522	isInit = false;
msimmerl	0:b6608b36efd7	523	Fahre_zu_Init_Pos();
msimmerl	0:b6608b36efd7	524	running = 0;
msimmerl	0:b6608b36efd7	525	break;
msimmerl	0:b6608b36efd7	526	}
msimmerl	0:b6608b36efd7	527
msimmerl	0:b6608b36efd7	528	//Hole Teil 1
msimmerl	0:b6608b36efd7	529	posHeben = Drive(heben1, heben2, posHeben, 0.001430, SPEED_MEDIUM);
msimmerl	0:b6608b36efd7	530	posDrehen = Drive(drehen, posDrehen, 0.002760, SPEED_MEDIUM); //0.00185
msimmerl	0:b6608b36efd7	531	posHeben = Drive(heben1, heben2, posHeben, 0.000990, SPEED_MEDIUM);
msimmerl	0:b6608b36efd7	532	posKlaue = Drive(klaue, posKlaue, 0.001190, SPEED_MEDIUM); //0.0015
msimmerl	0:b6608b36efd7	533
msimmerl	0:b6608b36efd7	534	if(isInit){
msimmerl	0:b6608b36efd7	535	isInit = false;
msimmerl	0:b6608b36efd7	536	Fahre_zu_Init_Pos();
msimmerl	0:b6608b36efd7	537	running = 0;
msimmerl	0:b6608b36efd7	538	break;
msimmerl	0:b6608b36efd7	539	}
msimmerl	0:b6608b36efd7	540
msimmerl	0:b6608b36efd7	541	//Fahre mit Teil 1 zu Pos 1
msimmerl	0:b6608b36efd7	542	posHeben = Drive(heben1, heben2, posHeben, 0.001430, SPEED_MEDIUM);
msimmerl	0:b6608b36efd7	543	posDrehen = Drive(drehen, posDrehen, 0.00095, SPEED_MEDIUM);
msimmerl	0:b6608b36efd7	544	posArmHinten = Drive(arm_hinten, posArmHinten, 0.001800, SPEED_SLOW); //Alt 0.0017
msimmerl	0:b6608b36efd7	545	posArmVorne = Drive(arm_vorne, posArmVorne, 0.002290, SPEED_SLOW); //Alt 0.002150
msimmerl	0:b6608b36efd7	546	posHeben = Drive(heben1, heben2, posHeben, 0.000990, SPEED_SLOW); //Alt 0.00099
msimmerl	0:b6608b36efd7	547	posKlaue = Drive(klaue, posKlaue, 0.001920, SPEED_MEDIUM);
msimmerl	0:b6608b36efd7	548
msimmerl	0:b6608b36efd7	549	Fahre_zu_Init_Pos();
msimmerl	0:b6608b36efd7	550
msimmerl	0:b6608b36efd7	551	}
msimmerl	0:b6608b36efd7	552
msimmerl	0:b6608b36efd7	553	lcd.locate(0, 0);
msimmerl	0:b6608b36efd7	554	lcd.printf("Programm waehlen");
msimmerl	0:b6608b36efd7	555	lcd.locate(0, 1);
msimmerl	0:b6608b36efd7	556	lcd.printf(texte[menu_counter]);
msimmerl	0:b6608b36efd7	557	}
msimmerl	0:b6608b36efd7	558
msimmerl	0:b6608b36efd7	559	void Fahre_zu_Init_Pos(){
msimmerl	0:b6608b36efd7	560	posHeben = Drive(heben1, heben2, posHeben, 0.0015, SPEED_MEDIUM);
msimmerl	0:b6608b36efd7	561	posArmHinten = Drive(arm_hinten, posArmHinten, 0.0015, SPEED_MEDIUM);
msimmerl	0:b6608b36efd7	562	posArmVorne = Drive(arm_vorne, posArmVorne, 0.0015, SPEED_MEDIUM);
msimmerl	0:b6608b36efd7	563	posDrehen = Drive(drehen, posDrehen, 0.00185, SPEED_MEDIUM);
msimmerl	0:b6608b36efd7	564	posArmHinten = Drive(arm_hinten, posArmHinten, 0.0015, SPEED_MEDIUM);
msimmerl	0:b6608b36efd7	565	posArmVorne = Drive(arm_vorne, posArmVorne, 0.0015, SPEED_MEDIUM);
msimmerl	0:b6608b36efd7	566	posHeben = Drive(heben1, heben2, posHeben, 0.0015, SPEED_MEDIUM);
msimmerl	0:b6608b36efd7	567	}
msimmerl	0:b6608b36efd7	568
msimmerl	0:b6608b36efd7	569	void InitPosition(void) {
msimmerl	0:b6608b36efd7	570	arm_vorne.pulsewidth(0.0015);
msimmerl	0:b6608b36efd7	571	arm_hinten.pulsewidth(0.0015);
msimmerl	0:b6608b36efd7	572	klaue.pulsewidth(0.0015);
msimmerl	0:b6608b36efd7	573	drehen.pulsewidth(0.00185);
msimmerl	0:b6608b36efd7	574	heben1.pulsewidth(0.0015);
msimmerl	0:b6608b36efd7	575	heben2.pulsewidth(0.0015);
msimmerl	0:b6608b36efd7	576	}
msimmerl	0:b6608b36efd7	577
msimmerl	0:b6608b36efd7	578	void InitServos(void) {
msimmerl	0:b6608b36efd7	579	arm_vorne.period(sPeriod);
msimmerl	0:b6608b36efd7	580	arm_hinten.period(sPeriod);
msimmerl	0:b6608b36efd7	581	klaue.period(sPeriod);
msimmerl	0:b6608b36efd7	582	heben1.period(sPeriod);
msimmerl	0:b6608b36efd7	583	heben2.period(sPeriod);
msimmerl	0:b6608b36efd7	584	drehen.period(sPeriod);
msimmerl	0:b6608b36efd7	585	}

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	15 Feb 2011
Imports:	4
Forks:	0
Commits:	1
Dependents:	0
Dependencies:	2
Followers:	1

main.cpp@0:b6608b36efd7, 2011-02-15 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning