qv12.asm
1 | ;===============================================================================
| 2 | ;
| 3 | ; Testprogramm zur Steuerung von 13 Servos am 8535 mit 4Mhz per Interupt
| 4 | ; Servos 1-8 an PortC, Servos 9-12 an PortD,0-3
| 5 | ; Servos 9-12 haben bereits eigene Register (R12-R15).
| 6 | ; Zusätzlich an PortD,4 ein Servo zum Schwenken der IR Sensoren GP2D12.
| 7 | ; Servo-Stellungswerte werden im SRAM in 0x0060 bis 0x006C zwischengespeichert.
| 8 | ; Messwerte der IR Sensoren landen Im SRAM unter 0x0080-0x009F fuer IR1 und
| 9 | ; 0x00A0-0x00BF fuer IR2.
| 10 | ; Beinhub wird im SRAM in 0x0070
| 11 | ; am Anfang des Programmes abgelegt und soll in spaeteren Versionen
| 12 | ; im Main-Bereich des Programmes errechnet oder ueber I2C oder SPI
| 13 | ; von einem anderen Controller übertragen werden.
| 14 | ; Schrittlaenge wird im Main Bereich ermittelt und im SRAM in 0x006E
| 15 | ; und 0x006F abgelegt.
| 16 | ;
| 17 | ;-------------------------------------------------------------------------------
| 18 | ;
| 19 | ; Mit diesem Code sollte der Roboter "Quo Vadis" erstmals in der Lage
| 20 | ; sein zu laufen. Hinderniserkennung und Kurven sollen erstmals in einer
| 21 | ; vorlaeufigen Variante funktionieren.
| 22 | ; W.Geenen 1.7.03
| 23 | ;
| 24 | ;===============================================================================
| 25 |
| 26 | .include "8535def.inc"
| 27 |
| 28 | .DEF temp = R17
| 29 | .DEF temp2 = R18
| 30 | .Def Posi = R20
| 31 | .DEF Beweg = R19
| 32 |
| 33 | .DEF Zaehl01 = R0 ;Zaehlschleife Hauptprogramm
| 34 | .DEF Schrittlaengel = R0
| 35 | .DEF Zaehl02 = R1
| 36 | .DEF Schrittlaenger = R1
| 37 | .DEF Zaehl03 = R2 ;Zaehlschleife Interupt
| 38 | .DEF Zaehl04 = R3
| 39 |
| 40 |
| 41 | .DEF RSvr1 = R4 ; = Servo vorne rechts 1
| 42 | .DEF RSvr2 = R5 ; = Servo vorne rechts 2
| 43 | .DEF RSvl1 = R6 ; = Servo vorne links 1
| 44 | .DEF RSvl2 = R7 ; = Servo vorne links 2
| 45 | .DEF RSmr1 = R8 ; = Servo mitte rechts 1
| 46 | .DEF RSmr2 = R9 ; = Servo mitte rechts 2
| 47 | .DEF RSml1 = R10 ; = Servo mitte links 1
| 48 | .DEF RSml2 = R11 ; = Servo mitte links 2
| 49 | .DEF RShr1 = R12 ; = Servo hinten rechts 1
| 50 | .DEF RShr2 = R13 ; = Servo hinten rechts 2
| 51 | .DEF RShl1 = R14 ; = Servo hinten links 1
| 52 | .DEF RShl2 = R15 ; = Servo hinten links 2
| 53 | .DEF Kopf = R16 ; = Servo zum Schwenken der GP2D12
| 54 |
| 55 | .DEF Wert01 = R21 ;Sensorwerte
| 56 | .DEF Wert02 = R22
| 57 |
| 58 | .DEF Sammelw = R24
| 59 |
| 60 | .EQU SSvr1 = 0x0060 ; = Servo vorne rechts 1
| 61 | .EQU SSvr2 = 0x0061 ; = Servo vorne rechts 2
| 62 | .EQU SSvl1 = 0x0062 ; = Servo vorne links 1
| 63 | .EQU SSvl2 = 0x0063 ; = Servo vorne links 2
| 64 | .EQU SSmr1 = 0x0064 ; = Servo mitte rechts 1
| 65 | .EQU SSmr2 = 0x0065 ; = Servo mitte rechts 2
| 66 | .EQU SSml1 = 0x0066 ; = Servo mitte links 1
| 67 | .EQU SSml2 = 0x0067 ; = Servo mitte links 2
| 68 | .EQU SShr1 = 0x0068 ; = Servo hinten rechts 1
| 69 | .EQU SShr2 = 0x0069 ; = Servo hinten rechts 2
| 70 | .EQU SShl1 = 0x006A ; = Servo hinten links 1
| 71 | .EQU SShl2 = 0x006B ; = Servo hinten links 2
| 72 | .EQU SKopf = 0x006C ; = Speicherplatz für Position Kopf
| 73 | .EQU Grund = 0x006D ; = Grundwerte
| 74 | .EQU Schrittl = 0x006E ; = Schrittweite links
| 75 | .EQU Schrittr = 0x006F ; = Schrittweite rechts
| 76 | .EQU Hub = 0x0070 ; = Beinhub
| 77 | .EQU SPosi = 0x0071
| 78 | .EQU IR1 = 0x0080 ;Messwerte Sensor 1
| 79 | .EQU IR2 = 0x00A0 ;Messwerte SEnsor 2
| 80 |
| 81 | ;Konstanten
| 82 | .EQU Sichern1 = 0x00C0
| 83 | .EQU Sichern2 = 0x00E0
| 84 | .EQU Kanal = 0
| 85 |
| 86 |
| 87 |
| 88 | .CSEG
| 89 | .ORG 0000
| 90 | RJMP Start ;Interupt Routinen überspringen
| 91 | .ORG $008
| 92 | RJMP Timer1 ;Zweiter Timer Interupt
| 93 |
| 94 | .ORG $009
| 95 | RJMP Timer0 ;erster Interupt
| 96 |
| 97 | Start:
| 98 |
| 99 | ;Stack einrichten
| 100 |
| 101 | LDI temp, HIGH(RAMEND) ;oberes Byte
| 102 | OUT SPH, temp ;an Stapelzeiger
| 103 | LDI temp, LOW(RAMEND) ;unteres Byte
| 104 | OUT SPL, temp ;an Stapelzeiger
| 105 |
| 106 | ;Sensor Speicher einrichten
| 107 | LDI YH, HIGH(IR1)
| 108 | LDI YL, LOW(IR1)
| 109 |
| 110 | LDI XH, HIGH(Sichern1)
| 111 | LDI XL, LOW(Sichern1)
| 112 |
| 113 |
| 114 | ;AD Wandler einrichten
| 115 |
| 116 | ldi temp,0b00000111 ;Vorteiler = ganz wichtig
| 117 | out ADCSR,temp ;bei 4MHz auf 64 oder 128 stellen
| 118 |
| 119 | cbi ADCSR,ADIE ;Int = AUS, muß aber nicht unbedingt
| 120 |
| 121 | sbi ADCSR,ADEN ;AD generell einschalten
| 122 |
| 123 | Free_Run:
| 124 | sbi ADCSR, ADFR ;Free Run einschalten
| 125 |
| 126 | sbi ADCSR, ADSC ;Start Wandlung
| 127 |
| 128 |
| 129 | ;Grundwerte in den Speicher
| 130 |
| 131 | LDI temp, 127
| 132 | STS SSvr1, temp
| 133 | STS SSvr2, temp
| 134 | STS SSvl1, temp
| 135 | STS SSvl2, temp
| 136 | STS SSmr1, temp
| 137 | STS SSmr2, temp
| 138 | STS SSml1, temp
| 139 | STS SSml2, temp
| 140 | STS SShr1, temp
| 141 | STS SShr2, temp
| 142 | STS SShl1, temp
| 143 | STS SShl2, temp
| 144 | STS Grund, temp
| 145 | LDI temp, 45
| 146 | STS Schrittl, temp
| 147 | STS Schrittr, temp
| 148 | LDI temp, 35
| 149 | STS Hub, temp
| 150 | LDI temp, 0b00000111
| 151 | STS SPosi, temp ;Stellungsroutine verzoegern
| 152 |
| 153 |
| 154 |
| 155 | ;Interupt einrichten
| 156 | ;Counter0
| 157 |
| 158 | LDI temp, 167
| 159 | OUt TCNT0, temp ;Interuptzaehler Voreinstellung
| 160 | LDI temp, 0b00000101 ;Timer0 Takt/1024
| 161 | OUT TCCR0, temp
| 162 | LDI temp, 0b00000101
| 163 | OUT TIMSK, temp ;Interupt aktivieren
| 164 |
| 165 | ;Counter1
| 166 |
| 167 | LDI temp, 253
| 168 | OUT TCNT1H, temp ;Interuptzaehler Voreinstellung
| 169 | LDI temp, 0x00
| 170 | OUT TCNT1L, temp
| 171 | LDI temp, 0b00000101 ;Timer1 Takt/1024
| 172 | OUT TCCR1B, temp
| 173 |
| 174 | Sei ;Interupt-enable Bit setzen
| 175 |
| 176 |
| 177 | LDI temp, 0xFF ;Port B Ausgang
| 178 | Out DDRB, temp
| 179 |
| 180 | LDI temp, 0xFF ;Port D Ausgang
| 181 | Out DDRD, temp
| 182 |
| 183 | LDI temp, 0xFF ;Port C Ausgang
| 184 | Out DDRC, temp
| 185 |
| 186 | ;-------------------------------<Main>------------------------------------------
| 187 |
| 188 | vergleichen:
| 189 | LDS Kopf, SKopf
| 190 | CPI Kopf, 0 ;Ist der Kopf am linken Ende?
| 191 | BREQ sichern
| 192 | CPI Kopf, 248 ;oder am Rechten?
| 193 | BREQ sichern
| 194 |
| 195 | Speicherplatz_laden:
| 196 | LDI XL, Sichern1 ;Speicherplatz laden
| 197 | ADIW XL, 12 ;die ersten Speicherplaetze erstmal weglassen
| 198 | auswerten:
| 199 |
| 200 | ;links innen hoechsten Messwert ermitteln
| 201 |
| 202 | LD Wert01, X+ ;1.Wert laden
| 203 | links_m:
| 204 | CPI XL, 0x00D0 ;wenn Adresse 0x00D0 erreicht, zur Routine fuer
| 205 | BREQ auswerten2 ;rechts innen springen
| 206 | CPI Wert01, 50
| 207 | BRLO links_m ;springe zurueck, wenn kleiner als 50
| 208 | MOV temp, Wert01 ;in temp kopieren
| 209 | LD Wert01, X+
| 210 | tauschen:
| 211 | CP temp, Wert01 ;vergleichen
| 212 | BRLO auslassen ;wenn Wert01 niedriger, dann zurueck
| 213 | MOV temp, Wert01 ;sonst, Wert01 in temp
| 214 | auslassen:
| 215 | rjmp links_m
| 216 |
| 217 | auswerten2:
| 218 |
| 219 | ;rechts innen hoechsten Messwert ermitteln
| 220 |
| 221 | LD Wert01, X+ ;1.Wert laden
| 222 | rechts_m:
| 223 | CPI XL, 0x00D4 ;wenn Adresse 0x00D4 erreicht, zur Routine fuer
| 224 | BREQ r_oder_l ; alle inneren Daten vergleichen, weiter
| 225 | CPI Wert01, 50
| 226 | BRLO rechts_m ;springe zurueck, wenn kleiner als 50
| 227 | MOV temp2, Wert01 ;in temp2 kopieren
| 228 | LD Wert01, X+
| 229 | tauschen2:
| 230 | CP temp, Wert01 ;vergleichen
| 231 | BRLO auslassen2 ;wenn Wert01 niedriger, dann zurueck
| 232 | MOV temp2, Wert01 ;sonst, Wert01 in temp
| 233 | auslassen2:
| 234 | rjmp rechts_m
| 235 |
| 236 | ;links und rechts vergleichen
| 237 | r_oder_l:
| 238 | CP temp, temp2
| 239 | BRSH linksrum
| 240 |
| 241 | rechtsrum:
| 242 | ror temp ;temp durch 4 teilen
| 243 | ror temp
| 244 | STS Schrittr, temp ;temp in Schrittr speichern
| 245 |
| 246 | rjmp vergleichen
| 247 |
| 248 | linksrum:
| 249 | ror temp2 ;temp2 durch 4 teilen
| 250 | ror temp2
| 251 | STS Schrittl, temp2 ;temp2 in Schrittl speichern
| 252 |
| 253 | rjmp vergleichen
| 254 |
| 255 | ;markieren: ist erstmal ausgeklammert
| 256 | ; LDI Wert01, 0xFF
| 257 | ; ST X, Wert01 ;als nahes Hindernis markieren
| 258 | rjmp auswerten
| 259 |
| 260 | sichern:
| 261 | LD temp, Y+ ;Alle Sensorwerte nochmal vor Überschreiben sichern
| 262 | ST X+, temp ;vor Überschreiben sichern
| 263 | CPI XL, 0x00FF
| 264 | BRNE sichern
| 265 | LDI YL, 0x0080
| 266 | LDI XL, 0x00C0
| 267 | rjmp Speicherplatz_laden
| 268 |
| 269 |
| 270 | ;-------------------------------</Main>-----------------------------------------
| 271 |
| 272 | ;111111111111111 Hier wird der erste Interupt ausgeführt!! 1111111111111111111
| 273 |
| 274 | Timer0:
| 275 | Push temp ;Registerinhalt auf den Stapel
| 276 | In temp,SREG ;Statusregister einlesen
| 277 | Push temp ;Statusregister auf den Stapel
| 278 |
| 279 | LDI temp, 167
| 280 | OUT TCNT0, temp ;Interuptzaehler Voreinstellung
| 281 |
| 282 | LDI temp,0xFF
| 283 | Out PortC, temp ;Servoports high setzen
| 284 | LDI temp,0x1F
| 285 | Out PortD, temp
| 286 |
| 287 | ;IR1 auslesen
| 288 |
| 289 | ldi temp, 0
| 290 | out ADMUX,temp ;Kanal auswählen
| 291 |
| 292 | ;AD_busy: sbis ADCSR, ADIF ;Flag da?, busy?
| 293 | ; rjmp AD_busy ;springt raus, wenn fertig
| 294 |
| 295 | Lesen: in temp, ADCL
| 296 | in temp2, ADCH
| 297 |
| 298 | ;auf 8bit verkürzen
| 299 |
| 300 | Auf256: ror temp2 ;xxxxxxxN c=N
| 301 | ror temp ;Nnnnnnnn c=n
| 302 | ror temp2 ;xxxxxxxx c=N
| 303 | ror temp ;NNnnnnnn c=n
| 304 |
| 305 | lsr Kopf
| 306 | lsr Kopf
| 307 | lsr Kopf ;Stellungswert durch 8 teilen
| 308 |
| 309 | LDI YL, 0x80
| 310 | ADD YL, Kopf
| 311 |
| 312 | ST Y, temp ;Messwert an entsprechender Stelle speichern
| 313 |
| 314 | ;IR2 auslesen
| 315 |
| 316 | ldi temp, 1
| 317 | out ADMUX,temp ;Kanal auswählen
| 318 |
| 319 | ;AD_busy: sbis ADCSR, ADIF ;Flag da?, busy?
| 320 | ; rjmp AD_busy ;springt raus, wenn fertig
| 321 |
| 322 | Lesen2: in temp, ADCL
| 323 | in temp2, ADCH
| 324 |
| 325 | ;auf 8bit verkürzen
| 326 | ror temp2 ;xxxxxxxN c=N
| 327 | ror temp ;Nnnnnnnn c=n
| 328 | ror temp2 ;xxxxxxxx c=N
| 329 | ror temp ;NNnnnnnn c=n
| 330 |
| 331 | LDI YL, 0xA0
| 332 | ADD YL, Kopf
| 333 |
| 334 | ST Y, temp ;Messwert an entsprechender Stelle speichern
| 335 |
| 336 |
| 337 |
| 338 | Kopf_drehen: ;Servostellung fuer Sensoren neu berechnen
| 339 | LDI temp, 4
| 340 | LDS Kopf, SKopf
| 341 | LDS temp2, SPosi
| 342 | ANDI temp2, 0b00010000 ;Richtungsbit Kopf auslesen
| 343 | CPI temp2, 0b00010000
| 344 | BREQ links
| 345 | rechts: ADD Kopf, temp ;Servo eins nach rechts
| 346 | CPI Kopf, 248 ;Pruefen ob ende
| 347 | BREQ bit_setzen ;wenn ja, bit setzen
| 348 | rjmp Kopf_wert_speichern
| 349 | links: SUB Kopf, temp ;Servo eins nach links
| 350 | CPI Kopf, 0 ;Pruefen ob ende
| 351 | BREQ bit_loeschen ;wenn ja, bit loeschen
| 352 | rjmp Kopf_wert_speichern
| 353 |
| 354 | bit_setzen:
| 355 | LDS temp2, SPosi
| 356 | ORI temp2, 0b00010000 ;bit setzen
| 357 | rjmp weiter
| 358 | bit_loeschen:
| 359 | LDS temp2, SPosi
| 360 | ANDI temp2, 0b11101111 ;bit loeschen
| 361 |
| 362 | weiter:
| 363 | STS SPosi, temp2 ;und speichern
| 364 | Kopf_wert_speichern:
| 365 | STS SKopf, Kopf ;Kopf-Stellung speichern
| 366 |
| 367 |
| 368 |
| 369 | ;weiter mit Servoroutine
| 370 |
| 371 | LDI temp,0xD0
| 372 | MOV Zaehl03, temp
| 373 | LDI temp,0x03
| 374 | msek: MOV Zaehl04, temp ;eine Millisekunde warten
| 375 |
| 376 |
| 377 | msek1: DEC Zaehl04
| 378 | Brne msek1
| 379 | DEC Zaehl03
| 380 | Brne msek
| 381 |
| 382 | LDS RSvr1, SSvr1
| 383 | LDS RSvr2, SSvr2
| 384 | LDS RSvl1, SSvl1
| 385 | LDS RSvl2, SSvl2 ;Speicherwerte in Servo Register laden
| 386 | LDS RSmr1, SSmr1
| 387 | LDS RSmr2, SSmr2
| 388 | LDS RSml1, SSml1
| 389 | LDS RSml2, SSml2
| 390 | LDS RShr1, SShr1
| 391 | LDS RShr2, SShr2
| 392 | LDS RShl1, SShl1
| 393 | LDS RShl2, SShl2
| 394 | LDS Kopf, SKopf
| 395 |
| 396 | CLR Zaehl03
| 397 | Rueck1:
| 398 |
| 399 | CP Zaehl03, RSvr1
| 400 | BREQ S1aus
| 401 | aus1: CP Zaehl03, RSvr2
| 402 | BREQ S2aus
| 403 | aus2: CP Zaehl03, RSvl1
| 404 | BREQ S3aus
| 405 | aus3: CP Zaehl03, RSvl2
| 406 | BREQ S4aus
| 407 | aus4: CP Zaehl03, RSmr1
| 408 | BREQ S5aus
| 409 | aus5: CP Zaehl03, RSmr2
| 410 | BREQ S6aus
| 411 | aus6: CP Zaehl03, RSml1
| 412 | BREQ S7aus
| 413 | aus7: CP Zaehl03, RSml2
| 414 | BREQ S8aus
| 415 | aus8: CP Zaehl03, RShr1
| 416 | BREQ S9aus
| 417 | aus9: CP Zaehl03, RShr2
| 418 | BREQ S10aus
| 419 | aus10: CP Zaehl03, RShl1
| 420 | BREQ S11aus
| 421 | aus11: CP Zaehl03, RShl2
| 422 | BREQ S12aus
| 423 | aus12: CP Zaehl03, Kopf
| 424 | BREQ S13aus
| 425 | aus13:
| 426 | INC Zaehl03
| 427 | BRNE Rueck1
| 428 |
| 429 | RJMP Servoroutine_erledigt
| 430 |
| 431 |
| 432 | S1aus: CBI PortC, 0
| 433 | RJMP aus1
| 434 | S2aus: CBI PortC, 1
| 435 | RJMP aus2
| 436 | S3aus: CBI PortC, 2
| 437 | RJMP aus3
| 438 | S4aus: CBI PortC, 3
| 439 | RJMP aus4
| 440 | S5aus: CBI PortC, 4
| 441 | RJMP aus5
| 442 | S6aus: CBI PortC, 5
| 443 | RJMP aus6
| 444 | S7aus: CBI PortC, 6
| 445 | RJMP aus7
| 446 | S8aus: CBI PortC, 7
| 447 | RJMP aus8
| 448 | S9aus: CBI PortD, 0
| 449 | RJMP aus9
| 450 | S10aus: CBI PortD, 1
| 451 | RJMP aus10
| 452 | S11aus: CBI PortD, 2
| 453 | RJMP aus11
| 454 | S12aus: CBI PortD, 3
| 455 | RJMP aus12
| 456 | S13aus: CBI PortD, 4
| 457 | RJMP aus13
| 458 |
| 459 |
| 460 |
| 461 | Servoroutine_erledigt:
| 462 |
| 463 |
| 464 |
| 465 | ;-------------------------Ruecksprung vorbereiten-------------------------------
| 466 |
| 467 | Pop temp ;alten Inhalt von SREG vom Stapel holen
| 468 | OUT SREG, temp ;Statusregister wieder herstellen
| 469 | Pop temp ;alten Inhalt von temp wieder herstellen
| 470 | reti
| 471 |
| 472 | ;22222222222222222 hier wird der zweite Interupt ausgeführt 222222222222222222
| 473 |
| 474 | Timer1:
| 475 | Push temp ;Registerinhalt auf den Stapel
| 476 | In temp,SREG ;Statusregister einlesen
| 477 | Push temp ;Statusreister auf den Stapel
| 478 |
| 479 | LDI temp, 253
| 480 | OUT TCNT1H, temp ;Interuptzaehler Voreinstellung
| 481 | LDI temp, 0x00
| 482 | OUT TCNT1L, temp
| 483 |
| 484 | LDS Beweg, SPosi
| 485 | ANDI Beweg, 0b00000111 ;Nur Beweg-Bits verwenden
| 486 | CPI Beweg, 0
| 487 | Breq Stufe1
| 488 | CPI Beweg, 1 ;Schritt 4
| 489 | Breq Stufe2
| 490 | CPI Beweg, 2 ;Schritt 3
| 491 | Breq Stufe3
| 492 | CPI Beweg, 3 ;Schritt 2
| 493 | Breq Stufe4 ;Schritt 1
| 494 | Rjmp Stellungs_routine_erledigt
| 495 |
| 496 | Stufe2: Rjmp Stuf2
| 497 | Stufe3: Rjmp Stuf3
| 498 | Stufe4: Rjmp Stuf4
| 499 |
| 500 |
| 501 | Stufe1: LDI Beweg, 4
| 502 | LDI temp, 127 ;rechtes Bein vor
| 503 | LDS Posi, Schrittr ;linkes Bein zurueck
| 504 | ADD temp, Posi
| 505 | STS SSvr1, temp
| 506 | LDI temp, 127
| 507 | LDS Posi, Schrittr
| 508 | SUB temp, Posi
| 509 | STS SSvr2, temp
| 510 | LDI temp, 127
| 511 | LDS Posi, Schrittl
| 512 | ADD temp, Posi
| 513 | STS SSvl1, temp
| 514 | LDI temp, 127
| 515 | LDS Posi, Schrittl
| 516 | SUB temp, Posi
| 517 | STS SSvl2, temp
| 518 | LDI temp, 127
| 519 | LDS Posi, Schrittr
| 520 | SUB temp, Posi
| 521 | STS SSmr1, temp
| 522 | LDI temp, 127
| 523 | LDS Posi, Schrittr
| 524 | ADD temp, Posi
| 525 | STS SSmr2, temp
| 526 | LDI temp, 127
| 527 | LDS Posi, Schrittl
| 528 | SUB temp, Posi
| 529 | STS SSml1, temp
| 530 | LDI temp, 127
| 531 | LDS Posi, Schrittl
| 532 | ADD temp, Posi
| 533 | STS SSml2, temp
| 534 | LDI temp, 127 ;rechtes Bein vor
| 535 | LDS Posi, Schrittr ;linkes Bein zurueck
| 536 | ADD temp, Posi
| 537 | STS SShr1, temp
| 538 | LDI temp, 127
| 539 | LDS Posi, Schrittr
| 540 | SUB temp, Posi
| 541 | STS SShr2, temp
| 542 | LDI temp, 127
| 543 | LDS Posi, Schrittl
| 544 | ADD temp, Posi
| 545 | STS SShl1, temp
| 546 | LDI temp, 127
| 547 | LDS Posi, Schrittl
| 548 | SUB temp, Posi
| 549 | STS SShl2, temp
| 550 | Rjmp Stellungs_routine_erledigt
| 551 | Stuf2:
| 552 | LDI temp, 127 ;rechtes Bein senken
| 553 | LDS Posi, Hub ;linkes Bein heben
| 554 | ADD temp, Posi
| 555 | STS SSvr1, temp
| 556 | LDI temp, 127
| 557 | LDS Posi, Hub
| 558 | ADD temp, Posi
| 559 | STS SSvr2, temp
| 560 | LDI temp, 127
| 561 | LDS Posi, Hub
| 562 | ADD temp, Posi
| 563 | STS SSvl1, temp
| 564 | LDI temp, 127
| 565 | LDS Posi, Hub
| 566 | ADD temp, Posi
| 567 | STS SSvl2, temp
| 568 | LDI temp, 127
| 569 | LDS Posi, Hub
| 570 | SUB temp, Posi
| 571 | STS SSmr1, temp
| 572 | LDI temp, 127
| 573 | LDS Posi, Hub
| 574 | SUB temp, Posi
| 575 | STS SSmr2, temp
| 576 | LDI temp, 127
| 577 | LDS Posi, Hub
| 578 | SUB temp, Posi
| 579 | STS SSml1, temp
| 580 | LDI temp, 127
| 581 | LDS Posi, Hub
| 582 | SUB temp, Posi
| 583 | STS SSml2, temp
| 584 | LDI temp, 127 ;rechtes Bein senken
| 585 | LDS Posi, Hub ;linkes Bein heben
| 586 | ADD temp, Posi
| 587 | STS SShr1, temp
| 588 | LDI temp, 127
| 589 | LDS Posi, Hub
| 590 | ADD temp, Posi
| 591 | STS SShr2, temp
| 592 | LDI temp, 127
| 593 | LDS Posi, Hub
| 594 | ADD temp, Posi
| 595 | STS SShl1, temp
| 596 | LDI temp, 127
| 597 | LDS Posi, Hub
| 598 | ADD temp, Posi
| 599 | STS SShl2, temp
| 600 | Rjmp Stellungs_routine_erledigt
| 601 | Stuf3:
| 602 | LDI temp, 127 ;rechtes Bein zurueck
| 603 | LDS Posi, Schrittr ;linkes Bein vor
| 604 | SUB temp, Posi
| 605 | STS SSvr1, temp
| 606 | LDI temp, 127
| 607 | LDS Posi, Schrittr
| 608 | ADD temp, Posi
| 609 | STS SSvr2, temp
| 610 | LDI temp, 127
| 611 | LDS Posi, Schrittl
| 612 | SUB temp, Posi
| 613 | STS SSvl1, temp
| 614 | LDI temp, 127
| 615 | LDS Posi, Schrittl
| 616 | ADD temp, Posi
| 617 | STS SSvl2, temp
| 618 | LDI temp, 127
| 619 | LDS Posi, Schrittr
| 620 | ADD temp, Posi
| 621 | STS SSmr1, temp
| 622 | LDI temp, 127
| 623 | LDS Posi, Schrittr
| 624 | SUB temp, Posi
| 625 | STS SSmr2, temp
| 626 | LDI temp, 127
| 627 | LDS Posi, Schrittl
| 628 | ADD temp, Posi
| 629 | STS SSml1, temp
| 630 | LDI temp, 127
| 631 | LDS Posi, Schrittl
| 632 | SUB temp, Posi
| 633 | STS SSml2, temp
| 634 | LDI temp, 127 ;rechtes Bein zurueck
| 635 | LDS Posi, Schrittr ;linkes Bein vor
| 636 | SUB temp, Posi
| 637 | STS SShr1, temp
| 638 | LDI temp, 127
| 639 | LDS Posi, Schrittr
| 640 | ADD temp, Posi
| 641 | STS SShr2, temp
| 642 | LDI temp, 127
| 643 | LDS Posi, Schrittl
| 644 | SUB temp, Posi
| 645 | STS SShl1, temp
| 646 | LDI temp, 127
| 647 | LDS Posi, Schrittl
| 648 | ADD temp, Posi
| 649 | STS SShl2, temp
| 650 | Rjmp Stellungs_routine_erledigt
| 651 | Stuf4:
| 652 | LDI temp, 127
| 653 | LDS Posi, Hub
| 654 | SUB temp, Posi
| 655 | STS SSvr1, temp
| 656 | LDI temp, 127
| 657 | LDS Posi, Hub
| 658 | SUB temp, Posi
| 659 | STS SSvr2, temp
| 660 | LDI temp, 127
| 661 | LDS Posi, Hub
| 662 | SUB temp, Posi ;rechts Bein senken
| 663 | STS SSvl1, temp ;links Bein heben
| 664 | LDI temp, 127
| 665 | LDS Posi, Hub
| 666 | SUB temp, Posi
| 667 | STS SSvl2, temp
| 668 | LDI temp, 127
| 669 | LDS Posi, Hub
| 670 | ADD temp, Posi
| 671 | STS SSmr1, temp
| 672 | LDI temp, 127
| 673 | LDS Posi, Hub
| 674 | ADD temp, Posi
| 675 | STS SSmr2, temp
| 676 | LDI temp, 127
| 677 | LDS Posi, Hub
| 678 | ADD temp, Posi
| 679 | STS SSml1, temp
| 680 | LDI temp, 127
| 681 | LDS Posi, Hub
| 682 | ADD temp, Posi
| 683 | STS SSml2, temp
| 684 | LDI temp, 127
| 685 | LDS Posi, Hub
| 686 | SUB temp, Posi
| 687 | STS SShr1, temp
| 688 | LDI temp, 127
| 689 | LDS Posi, Hub
| 690 | SUB temp, Posi
| 691 | STS SShr2, temp
| 692 | LDI temp, 127
| 693 | LDS Posi, Hub
| 694 | SUB temp, Posi ;rechts Bein senken
| 695 | STS SShl1, temp ;links Bein heben
| 696 | LDI temp, 127
| 697 | LDS Posi, Hub
| 698 | SUB temp, Posi
| 699 | STS SShl2, temp
| 700 | Rjmp Stellungs_routine_erledigt
| 701 |
| 702 |
| 703 |
| 704 |
| 705 | Stellungs_Routine_erledigt:
| 706 | DEC Beweg
| 707 | LDS temp, SPosi
| 708 | ANDI temp, 0b11111000
| 709 | OR temp, Beweg ;SPosi-Wert wieder herstellen
| 710 | STS SPosi, temp ;Positionszaehler wieder auf 4 setzen
| 711 |
| 712 | ;--------------------------Ruecksprung vorbereiten------------------------------
| 713 |
| 714 | Pop temp ;alten Inhalt von SREG vom Stapel holen
| 715 | OUT SREG, temp ;Statusregister wieder herstellen
| 716 | Pop temp ;alten Inhalt von temp wieder herstellen
| 717 | reti
|
|