Simmerl Manuel
/
MRT_Studienarbeit
This program control a 5 axis arm robot from lynx motion
main.cpp
- Committer:
- msimmerl
- Date:
- 2011-02-15
- Revision:
- 0:b6608b36efd7
File content as of revision 0:b6608b36efd7:
#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); }