Simmerl Manuel
This program control a 5 axis arm robot from lynx motion
Diff: main.cpp
- Revision:
- 0:b6608b36efd7
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Tue Feb 15 07:49:25 2011 +0000
@@ -0,0 +1,585 @@
+#include <mbed.h>
+#include <scmRTOS.h>
+#include "main.h"
+
+//Serielle Schnittstelle
+Serial pc(USBTX, USBRX);
+
+//LCD
+TextLCD lcd(p14, p16, p17, p15, p19, p20); // rs, e, d4, d5, d6, d7
+// Defines LCD bus to write data.
+BusOut Lcd_pins(p17, p18, p19, p20); // d4, d5, d6, d7
+
+DigitalOut rs_pin(p14); // LCD pin rs (register select.)
+DigitalOut e_pin(p16); // LCD pin e (enable).
+
+//Servos
+PwmOut arm_hinten(p21);
+PwmOut arm_vorne(p22);
+PwmOut drehen(p23);
+PwmOut klaue(p24);
+PwmOut heben1(p25);
+PwmOut heben2(p26);
+
+//IO
+//DigitalOut myled1(LED1);
+//DigitalOut myled2(LED2);
+DigitalIn key_start(p13);
+DigitalIn key_menu_up(p11);
+DigitalIn key_menu_down(p12);
+DigitalIn key_stop(p10);
+
+//Prozesse
+typedef OS::process<OS::pr0, 1000> T_Roboter;
+typedef OS::process<OS::pr1, 1000> T_IO;
+
+T_Roboter pRoboter;
+T_IO pIO;
+
+void Programm_1(void);
+void Programm_2(void);
+void InitPosition(void);
+void InitServos(void);
+
+float posKlaue = 0.0015;
+float posArmVorne = 0.0015;
+float posArmHinten = 0.0015;
+float posHeben = 0.0015;
+float posDrehen = 0.00185;
+
+long long count = 0;
+int action = 0;
+int running = 0;
+
+int menu_counter = 0;
+
+char input;
+
+bool isStop = false;
+bool isInit = false;
+
+bool keyPressed = false;
+int keyCounter = 0;
+
+int main() {
+ pc.baud(115200);
+ printf("\r\nInitialisierung...\r\n");
+ lcd.cls();
+ lcd.printf("Lade...");
+ InitServos();
+ InitPosition();
+ wait(2);
+ lcd.locate(0, 0);
+ lcd.printf("Lade...fertig");
+ wait(2);
+ lcd.locate(0, 0);
+ lcd.printf("Programm waehlen");
+ lcd.locate(0, 1);
+ lcd.printf(texte[0]);
+ OS::Run();
+}
+
+template<> OS_PROCESS void T_Roboter::Exec() {
+ while(1) {
+ if(running == 1 && menu_counter == 1){
+ Programm_1();
+ running = 0;
+ }else if(running == 1 && menu_counter == 2){
+ Programm_2();
+ running = 0;
+ }
+ Sleep(100);
+ }
+}
+
+template<> OS_PROCESS void T_IO::Exec() {
+ while(1) {
+ //pc.printf("Hello World\r\n");
+ /*if(start == 1 && running == 0){
+ running = 1;
+ lcd.cls();
+ lcd.locate(0,0);
+ lcd.printf("running...");
+ }
+ if(start == 1){
+ pc.printf("Hallo Welt!\r\n");
+ }*/
+ Sleep(1000);
+ }
+}
+
+
+//---------------------------------------------------------------------------
+void OS::SystemTimerUserHook() {
+ ++count;
+ if (count % 150 == 0) {
+ if(key_start == 1 && running == 0 && menu_counter == 1 && !keyPressed){
+ running = 1;
+ lcd.cls();
+ lcd.locate(0,0);
+ lcd.printf("Running...P1");
+ lcd.locate(0, 1);
+ lcd.printf("Menu ->Press key");
+ }else if(key_start == 1 && running == 0 && menu_counter == 2 && !keyPressed) {
+ running = 1;
+ lcd.cls();
+ lcd.locate(0,0);
+ lcd.printf("Running...P2");
+ lcd.locate(0, 1);
+ lcd.printf("Menu ->Press key");
+ }else if(key_start == 1 && running == 1 && menu_counter == 0 && !keyPressed) {
+ switch(menu_counter){
+ case 0: isInit = true; break;
+ }
+ }
+
+ if(key_menu_up == 1 && menu_counter < 3 && !keyPressed) {
+ menu_counter++;
+ lcd.locate(0, 1);
+ lcd.printf(texte[menu_counter]);
+ }
+
+ if(key_menu_down == 1 && menu_counter > 0 && !keyPressed){
+ menu_counter--;
+ lcd.locate(0, 1);
+ lcd.printf(texte[menu_counter]);
+ }
+
+ if(key_stop == 1 && running == 1 && menu_counter == 3 && !keyPressed){
+ isInit = true;
+ }
+
+ if(keyPressed){
+ if(keyCounter < 4){
+ keyCounter++;
+ }else{
+ keyCounter = 0;
+ keyPressed = false;
+ }
+ }
+ if(pc.readable()){
+ input = pc.getc();
+ switch(input) {
+ case 'a': posKlaue = posKlaue + 0.00001; klaue.pulsewidth(posKlaue); pc.printf("Klaue pulsewidth: %lf\r\n", posKlaue); break;
+ case 'd': posKlaue = posKlaue - 0.00001; klaue.pulsewidth(posKlaue); pc.printf("Klaue pulsewidth: %lf\r\n", posKlaue); break;
+ case 'w': posArmVorne = posArmVorne + 0.00001; arm_vorne.pulsewidth(posArmVorne); pc.printf("Arm vorne pulsewidth: %lf\r\n", posArmVorne); break;
+ case 's': posArmVorne = posArmVorne - 0.00001; arm_vorne.pulsewidth(posArmVorne); pc.printf("Arm vorne pulsewidth: %lf\r\n", posArmVorne); break;
+ case 'i': posArmHinten = posArmHinten + 0.00001; arm_hinten.pulsewidth(posArmHinten); pc.printf("Arm hinten pulsewidth: %lf\r\n", posArmHinten); break;
+ case 'k': posArmHinten = posArmHinten - 0.00001; arm_hinten.pulsewidth(posArmHinten); pc.printf("Arm hinten pulsewidth: %lf\r\n", posArmHinten); break;
+ case 'j': posHeben = posHeben + 0.00001; heben1.pulsewidth(posHeben); heben2.pulsewidth(posHeben); pc.printf("Heben pulsewidth: %lf\r\n", posHeben); break;
+ case 'l': posHeben = posHeben - 0.00001; heben1.pulsewidth(posHeben); heben2.pulsewidth(posHeben); pc.printf("Heben pulsewidth: %lf\r\n", posHeben); break;
+ case 'v': posDrehen = posDrehen + 0.00001; drehen.pulsewidth(posDrehen); drehen.pulsewidth(posDrehen); pc.printf("Drehen pulsewidth: %lf\r\n", posDrehen); break;
+ case 'b': posDrehen = posDrehen - 0.00001; drehen.pulsewidth(posDrehen); drehen.pulsewidth(posDrehen); pc.printf("Drehen pulsewidth: %lf\r\n", posDrehen); break;
+ default: pc.printf("Nicht definiertes Zeichen: %i\r\n", input);
+ }
+ }
+ count = 0;
+ }
+}
+
+//---------------------------------------------------------------------------
+void OS::IdleProcessUserHook() {
+ __WFI();
+}
+void Programm_2(void) {
+ int i = 0;
+ pc.printf("\r\n");
+ for(i = 0; i < 10; i++){
+ pc.printf("%i Durchgang\r\n", (i + 1));
+
+ //Hole Teil 1
+ posDrehen = Drive(drehen, posDrehen, 0.00095, SPEED_FAST);
+ posKlaue = Drive(klaue, posKlaue, 0.001920, SPEED_FAST);
+ posArmHinten = Drive(arm_hinten, posArmHinten, 0.001800, SPEED_FAST);
+ posArmVorne = Drive(arm_vorne, posArmVorne, 0.002290, SPEED_FAST);
+ posHeben = Drive(heben1, heben2, posHeben, 0.000990, SPEED_FAST);
+ posKlaue = Drive(klaue, posKlaue, 0.001210, SPEED_FAST);
+
+ if(isInit){
+ isInit = false;
+ Fahre_zu_Init_Pos();
+ running = 0;
+ break;
+ }
+
+ //Fahre mit Teil 1 zu Pos 2
+ posHeben = Drive(heben1, heben2, posHeben, 0.001430, SPEED_FAST);
+ posDrehen = Drive(drehen, posDrehen, 0.002760, SPEED_FAST);
+ posHeben = Drive(heben1, heben2, posHeben, 0.000990, SPEED_FAST);
+ posKlaue = Drive(klaue, posKlaue, 0.001920, SPEED_FAST);
+ posHeben = Drive(heben1, heben2, posHeben, 0.001430, SPEED_FAST);
+
+ if(isInit){
+ isInit = false;
+ Fahre_zu_Init_Pos();
+ running = 0;
+ break;
+ }
+
+ //Hole Teil 2
+ posDrehen = Drive(drehen, posDrehen, 0.00111, SPEED_FAST); //0.00095
+ posKlaue = Drive(klaue, posKlaue, 0.001920, SPEED_FAST);
+ posArmHinten = Drive(arm_hinten, posArmHinten, 0.001800, SPEED_FAST); //Alt 0.001700
+ posArmVorne = Drive(arm_vorne, posArmVorne, 0.002290, SPEED_FAST); //Alt 0.002150
+ posHeben = Drive(heben1, heben2, posHeben, 0.000990, SPEED_FAST); //Alt 0.000990
+ posKlaue = Drive(klaue, posKlaue, 0.001190, SPEED_FAST);
+
+ if(isInit){
+ isInit = false;
+ Fahre_zu_Init_Pos();
+ running = 0;
+ break;
+ }
+
+ //Fahre mit Teil 2 zu Pos 2
+ posHeben = Drive(heben1, heben2, posHeben, 0.001430, SPEED_FAST);
+ posDrehen = Drive(drehen, posDrehen, 0.00260, SPEED_FAST); //0.00185
+ posHeben = Drive(heben1, heben2, posHeben, 0.000990, SPEED_FAST);
+ posKlaue = Drive(klaue, posKlaue, 0.001920, SPEED_FAST); //0.0015
+ posHeben = Drive(heben1, heben2, posHeben, 0.001430, SPEED_FAST);
+
+ if(isInit){
+ isInit = false;
+ Fahre_zu_Init_Pos();
+ running = 0;
+ break;
+ }
+
+ //Hole Teil 3
+ posDrehen = Drive(drehen, posDrehen, 0.00127, SPEED_FAST); //0.00095
+ posKlaue = Drive(klaue, posKlaue, 0.001920, SPEED_FAST);
+ posArmHinten = Drive(arm_hinten, posArmHinten, 0.001800, SPEED_FAST); //Alt 0.001700
+ posArmVorne = Drive(arm_vorne, posArmVorne, 0.002290, SPEED_FAST); //Alt 0.002150
+ posHeben = Drive(heben1, heben2, posHeben, 0.000990, SPEED_FAST); //Alt 0.000990
+ posKlaue = Drive(klaue, posKlaue, 0.001190, SPEED_FAST);
+
+ if(isInit){
+ isInit = false;
+ Fahre_zu_Init_Pos();
+ running = 0;
+ break;
+ }
+
+ //Fahre mit Teil 3 zu Pos 2
+ posHeben = Drive(heben1, heben2, posHeben, 0.001430, SPEED_FAST);
+ posDrehen = Drive(drehen, posDrehen, 0.00244, SPEED_FAST); //0.00185
+ posHeben = Drive(heben1, heben2, posHeben, 0.000990, SPEED_FAST);
+ posKlaue = Drive(klaue, posKlaue, 0.001920, SPEED_FAST); //0.0015
+ posHeben = Drive(heben1, heben2, posHeben, 0.001430, SPEED_FAST);
+
+ //Fahre mitte
+ posDrehen = Drive(drehen, posDrehen, 0.00185, SPEED_FAST); //0.00185
+
+ if(isInit){
+ isInit = false;
+ Fahre_zu_Init_Pos();
+ running = 0;
+ break;
+ }
+
+ //Hole Teil 2
+ posDrehen = Drive(drehen, posDrehen, 0.00260, SPEED_FAST);
+ posKlaue = Drive(klaue, posKlaue, 0.001920, SPEED_FAST);
+ posArmHinten = Drive(arm_hinten, posArmHinten, 0.001800, SPEED_FAST);
+ posArmVorne = Drive(arm_vorne, posArmVorne, 0.002290, SPEED_FAST);
+ posHeben = Drive(heben1, heben2, posHeben, 0.000990, SPEED_FAST);
+ posKlaue = Drive(klaue, posKlaue, 0.001190, SPEED_FAST);
+
+ if(isInit){
+ isInit = false;
+ Fahre_zu_Init_Pos();
+ running = 0;
+ break;
+ }
+
+ //Fahre zu Pos 3
+ posHeben = Drive(heben1, heben2, posHeben, 0.001430, SPEED_FAST);
+ posDrehen = Drive(drehen, posDrehen, 0.00127, SPEED_FAST);
+ posArmHinten = Drive(arm_hinten, posArmHinten, 0.001800, SPEED_FAST);
+ posArmVorne = Drive(arm_vorne, posArmVorne, 0.002290, SPEED_FAST);
+ posHeben = Drive(heben1, heben2, posHeben, 0.000990, SPEED_FAST);
+ posKlaue = Drive(klaue, posKlaue, 0.001920, SPEED_FAST);
+
+ if(isInit){
+ isInit = false;
+ Fahre_zu_Init_Pos();
+ running = 0;
+ break;
+ }
+
+ //Hole Teil 3
+ posHeben = Drive(heben1, heben2, posHeben, 0.001430, SPEED_FAST);
+ posDrehen = Drive(drehen, posDrehen, 0.00244, SPEED_FAST);
+ posArmHinten = Drive(arm_hinten, posArmHinten, 0.001800, SPEED_FAST);
+ posArmVorne = Drive(arm_vorne, posArmVorne, 0.002290, SPEED_FAST);
+ posHeben = Drive(heben1, heben2, posHeben, 0.000990, SPEED_FAST);
+ posKlaue = Drive(klaue, posKlaue, 0.001190, SPEED_FAST);
+
+ if(isInit){
+ isInit = false;
+ Fahre_zu_Init_Pos();
+ running = 0;
+ break;
+ }
+
+ //Fahre zu Pos 1
+ posHeben = Drive(heben1, heben2, posHeben, 0.001430, SPEED_FAST);
+ posDrehen = Drive(drehen, posDrehen, 0.00095, SPEED_FAST);
+ posArmHinten = Drive(arm_hinten, posArmHinten, 0.001800, SPEED_FAST);
+ posArmVorne = Drive(arm_vorne, posArmVorne, 0.002290, SPEED_FAST);
+ posHeben = Drive(heben1, heben2, posHeben, 0.000990, SPEED_FAST);
+ posKlaue = Drive(klaue, posKlaue, 0.001920, SPEED_FAST);
+
+ if(isInit){
+ isInit = false;
+ Fahre_zu_Init_Pos();
+ running = 0;
+ break;
+ }
+
+ //Hole Teil 1
+ posHeben = Drive(heben1, heben2, posHeben, 0.001430, SPEED_FAST);
+ posDrehen = Drive(drehen, posDrehen, 0.002760, SPEED_FAST);
+ posArmHinten = Drive(arm_hinten, posArmHinten, 0.001800, SPEED_FAST);
+ posArmVorne = Drive(arm_vorne, posArmVorne, 0.002290, SPEED_FAST);
+ posHeben = Drive(heben1, heben2, posHeben, 0.000990, SPEED_FAST);
+ posKlaue = Drive(klaue, posKlaue, 0.001190, SPEED_FAST);
+
+ if(isInit){
+ isInit = false;
+ Fahre_zu_Init_Pos();
+ running = 0;
+ break;
+ }
+
+ //Fahre zu Pos 1
+ posHeben = Drive(heben1, heben2, posHeben, 0.001430, SPEED_FAST);
+ posDrehen = Drive(drehen, posDrehen, 0.00111, SPEED_FAST);
+ posArmHinten = Drive(arm_hinten, posArmHinten, 0.001800, SPEED_FAST);
+ posArmVorne = Drive(arm_vorne, posArmVorne, 0.002290, SPEED_FAST);
+ posHeben = Drive(heben1, heben2, posHeben, 0.000990, SPEED_FAST);
+ posKlaue = Drive(klaue, posKlaue, 0.001920, SPEED_FAST);
+
+ Fahre_zu_Init_Pos();
+ }
+ lcd.locate(0, 0);
+ lcd.printf("Programm waehlen");
+ lcd.locate(0, 1);
+ lcd.printf(texte[menu_counter]);
+}
+void Programm_1(void) {
+ int i = 0;
+ pc.printf("\r\n");
+ for(i = 0; i < 10; i++){
+ pc.printf("%i Durchgang\r\n", (i + 1));
+
+ //Hole Teil 1
+ posDrehen = Drive(drehen, posDrehen, 0.00095, SPEED_MEDIUM); //Nur zum Testen: Original: 0.00095
+ posKlaue = Drive(klaue, posKlaue, 0.001920, SPEED_MEDIUM);
+ posArmHinten = Drive(arm_hinten, posArmHinten, 0.001800, SPEED_MEDIUM); //Alt 0.0017
+ posArmVorne = Drive(arm_vorne, posArmVorne, 0.002290, SPEED_MEDIUM); //Alt 0.002150
+ posHeben = Drive(heben1, heben2, posHeben, 0.000990, SPEED_MEDIUM); //Alt 0.00099
+ posKlaue = Drive(klaue, posKlaue, 0.001210, SPEED_MEDIUM);
+
+ if(isInit){
+ isInit = false;
+ Fahre_zu_Init_Pos();
+ running = 0;
+ break;
+ }
+
+ //Fahre mit Teil 1 zu Pos 2
+ posHeben = Drive(heben1, heben2, posHeben, 0.001430, SPEED_MEDIUM);
+ posDrehen = Drive(drehen, posDrehen, 0.002760, SPEED_MEDIUM); //0.00185
+ posHeben = Drive(heben1, heben2, posHeben, 0.000990, SPEED_MEDIUM);
+ posKlaue = Drive(klaue, posKlaue, 0.001920, SPEED_MEDIUM); //0.0015
+ posHeben = Drive(heben1, heben2, posHeben, 0.001430, SPEED_MEDIUM);
+ posDrehen = Drive(drehen, posDrehen, 0.00185, SPEED_MEDIUM);
+
+ if(isInit){
+ isInit = false;
+ Fahre_zu_Init_Pos();
+ running = 0;
+ break;
+ }
+
+ //Hole Teil 2
+ posDrehen = Drive(drehen, posDrehen, 0.00111, SPEED_MEDIUM); //0.00095
+ posKlaue = Drive(klaue, posKlaue, 0.001920, SPEED_MEDIUM);
+ posArmHinten = Drive(arm_hinten, posArmHinten, 0.001800, SPEED_MEDIUM); //Alt 0.001700
+ posArmVorne = Drive(arm_vorne, posArmVorne, 0.002290, SPEED_MEDIUM); //Alt 0.002150
+ posHeben = Drive(heben1, heben2, posHeben, 0.000990, SPEED_MEDIUM); //Alt 0.000990
+ posKlaue = Drive(klaue, posKlaue, 0.001190, SPEED_MEDIUM);
+
+ if(isInit){
+ isInit = false;
+ Fahre_zu_Init_Pos();
+ running = 0;
+ break;
+ }
+
+ //Fahre mit Teil 2 zu Pos 2
+ posHeben = Drive(heben1, heben2, posHeben, 0.001430, SPEED_MEDIUM);
+ posDrehen = Drive(drehen, posDrehen, 0.002760, SPEED_MEDIUM); //0.00185
+ posArmHinten = Drive(arm_hinten, posArmHinten, 0.00180, SPEED_MEDIUM);
+ posHeben = Drive(heben1, heben2, posHeben, 0.00132, SPEED_SLOW);
+ posArmVorne = Drive(arm_vorne, posArmVorne, 0.001970, SPEED_SLOW);
+
+ if(isInit){
+ isInit = false;
+ Fahre_zu_Init_Pos();
+ running = 0;
+ break;
+ }
+
+ //Hole Teil 3
+ posKlaue = Drive(klaue, posKlaue, 0.001920, SPEED_MEDIUM);
+ posHeben = Drive(heben1, heben2, posHeben, 0.00145, SPEED_MEDIUM);
+ posDrehen = Drive(drehen, posDrehen, 0.00127, SPEED_MEDIUM);
+ posArmHinten = Drive(arm_hinten, posArmHinten, 0.001800, SPEED_MEDIUM); //Alt 0.001700
+ posArmVorne = Drive(arm_vorne, posArmVorne, 0.002290, SPEED_MEDIUM); //Alt 0.002150
+ posHeben = Drive(heben1, heben2, posHeben, 0.000990, SPEED_MEDIUM); //Alt 0.000990
+ posKlaue = Drive(klaue, posKlaue, 0.001190, SPEED_MEDIUM);
+
+ if(isInit){
+ isInit = false;
+ Fahre_zu_Init_Pos();
+ running = 0;
+ break;
+ }
+
+ //Fahre mit Teil 3 zu Pos 2
+ posHeben = Drive(heben1, heben2, posHeben, 0.00150, SPEED_MEDIUM);
+ posArmVorne = Drive(arm_vorne, posArmVorne, 0.00136, SPEED_MEDIUM);
+ posArmHinten = Drive(arm_hinten, posArmHinten, 0.0014, SPEED_MEDIUM); //Alt 0.001700
+ posDrehen = Drive(drehen, posDrehen, 0.002760, SPEED_MEDIUM);
+ posHeben = Drive(heben1, heben2, posHeben, 0.001450, SPEED_SLOW);
+ posArmVorne = Drive(arm_vorne, posArmVorne, 0.00153, SPEED_SLOW);
+ posArmHinten = Drive(arm_hinten, posArmHinten, 0.00154, SPEED_SLOW);
+ posKlaue = Drive(klaue, posKlaue, 0.001920, SPEED_MEDIUM);
+
+ if(isInit){
+ isInit = false;
+ Fahre_zu_Init_Pos();
+ running = 0;
+ break;
+ }
+ //Drehe zur Ausgangsposition
+ posArmVorne = Drive(arm_vorne, posArmVorne, 0.00182, SPEED_MEDIUM);
+ posDrehen = Drive(drehen, posDrehen, 0.00185, SPEED_MEDIUM);
+
+ //Hole Teil 3
+ posDrehen = Drive(drehen, posDrehen, 0.002760, SPEED_MEDIUM);
+ posArmVorne = Drive(arm_vorne, posArmVorne, 0.00153, SPEED_SLOW);
+ posKlaue = Drive(klaue, posKlaue, 0.001190, SPEED_MEDIUM);
+ posArmVorne = Drive(arm_vorne, posArmVorne, 0.00182, SPEED_SLOW);
+
+ if(isInit){
+ isInit = false;
+ Fahre_zu_Init_Pos();
+ running = 0;
+ break;
+ }
+
+ //Fahre mit Teil 3 zu Pos 1
+ posDrehen = Drive(drehen, posDrehen, 0.00127, SPEED_MEDIUM);
+ posArmHinten = Drive(arm_hinten, posArmHinten, 0.001800, SPEED_SLOW); //Alt 0.001700
+ posArmVorne = Drive(arm_vorne, posArmVorne, 0.002290, SPEED_SLOW); //Alt 0.002150
+ posHeben = Drive(heben1, heben2, posHeben, 0.000990, SPEED_SLOW); //Alt 0.000990
+ posKlaue = Drive(klaue, posKlaue, 0.001920, SPEED_MEDIUM);
+
+ if(isInit){
+ isInit = false;
+ Fahre_zu_Init_Pos();
+ running = 0;
+ break;
+ }
+
+ //Hole Teil 2
+ posHeben = Drive(heben1, heben2, posHeben, 0.001430, SPEED_MEDIUM);
+ posDrehen = Drive(drehen, posDrehen, 0.002760, SPEED_MEDIUM); //0.00185
+ posArmHinten = Drive(arm_hinten, posArmHinten, 0.00180, SPEED_MEDIUM);
+ posHeben = Drive(heben1, heben2, posHeben, 0.00132, SPEED_SLOW);
+ posArmVorne = Drive(arm_vorne, posArmVorne, 0.001970, SPEED_SLOW);
+ posKlaue = Drive(klaue, posKlaue, 0.001190, SPEED_MEDIUM);
+
+ if(isInit){
+ isInit = false;
+ Fahre_zu_Init_Pos();
+ running = 0;
+ break;
+ }
+
+ //Fahre mit Teil 2 zu Pos 1
+ posArmHinten = Drive(arm_hinten, posArmHinten, 0.001700, SPEED_MEDIUM); //Alt 0.001700
+ posDrehen = Drive(drehen, posDrehen, 0.00111, SPEED_MEDIUM); //0.00095
+ posArmHinten = Drive(arm_hinten, posArmHinten, 0.001800, SPEED_SLOW);
+ posArmVorne = Drive(arm_vorne, posArmVorne, 0.002290, SPEED_SLOW); //Alt 0.002150
+ posHeben = Drive(heben1, heben2, posHeben, 0.000990, SPEED_SLOW); //Alt 0.000990
+ posKlaue = Drive(klaue, posKlaue, 0.001920, SPEED_MEDIUM);
+
+ if(isInit){
+ isInit = false;
+ Fahre_zu_Init_Pos();
+ running = 0;
+ break;
+ }
+
+ //Hole Teil 1
+ posHeben = Drive(heben1, heben2, posHeben, 0.001430, SPEED_MEDIUM);
+ posDrehen = Drive(drehen, posDrehen, 0.002760, SPEED_MEDIUM); //0.00185
+ posHeben = Drive(heben1, heben2, posHeben, 0.000990, SPEED_MEDIUM);
+ posKlaue = Drive(klaue, posKlaue, 0.001190, SPEED_MEDIUM); //0.0015
+
+ if(isInit){
+ isInit = false;
+ Fahre_zu_Init_Pos();
+ running = 0;
+ break;
+ }
+
+ //Fahre mit Teil 1 zu Pos 1
+ posHeben = Drive(heben1, heben2, posHeben, 0.001430, SPEED_MEDIUM);
+ posDrehen = Drive(drehen, posDrehen, 0.00095, SPEED_MEDIUM);
+ posArmHinten = Drive(arm_hinten, posArmHinten, 0.001800, SPEED_SLOW); //Alt 0.0017
+ posArmVorne = Drive(arm_vorne, posArmVorne, 0.002290, SPEED_SLOW); //Alt 0.002150
+ posHeben = Drive(heben1, heben2, posHeben, 0.000990, SPEED_SLOW); //Alt 0.00099
+ posKlaue = Drive(klaue, posKlaue, 0.001920, SPEED_MEDIUM);
+
+ Fahre_zu_Init_Pos();
+
+ }
+
+ lcd.locate(0, 0);
+ lcd.printf("Programm waehlen");
+ lcd.locate(0, 1);
+ lcd.printf(texte[menu_counter]);
+}
+
+void Fahre_zu_Init_Pos(){
+ posHeben = Drive(heben1, heben2, posHeben, 0.0015, SPEED_MEDIUM);
+ posArmHinten = Drive(arm_hinten, posArmHinten, 0.0015, SPEED_MEDIUM);
+ posArmVorne = Drive(arm_vorne, posArmVorne, 0.0015, SPEED_MEDIUM);
+ posDrehen = Drive(drehen, posDrehen, 0.00185, SPEED_MEDIUM);
+ posArmHinten = Drive(arm_hinten, posArmHinten, 0.0015, SPEED_MEDIUM);
+ posArmVorne = Drive(arm_vorne, posArmVorne, 0.0015, SPEED_MEDIUM);
+ posHeben = Drive(heben1, heben2, posHeben, 0.0015, SPEED_MEDIUM);
+}
+
+void InitPosition(void) {
+ arm_vorne.pulsewidth(0.0015);
+ arm_hinten.pulsewidth(0.0015);
+ klaue.pulsewidth(0.0015);
+ drehen.pulsewidth(0.00185);
+ heben1.pulsewidth(0.0015);
+ heben2.pulsewidth(0.0015);
+}
+
+void InitServos(void) {
+ arm_vorne.period(sPeriod);
+ arm_hinten.period(sPeriod);
+ klaue.period(sPeriod);
+ heben1.period(sPeriod);
+ heben2.period(sPeriod);
+ drehen.period(sPeriod);
+}
\ No newline at end of file