Simmerl Manuel
/
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 | } |