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

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);
}
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

File content as of revision 0:b6608b36efd7:

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning
