Code:
'===============================================================================
'BAGGER Steuerung
'
'RC Eingang 1 an Pin 3 (PB1, PCINT9) Stick 1 (Aileron)
'RC Eingang 2 an Pin 6 (PA7, PCINT7) Stick 2 (Elevator)
'RC Eingang 3 an Pin 5 (PB2, INT0) Betriebsmode
'Poti 1a an Pin 10 (PA3, ADC3)
'Poti 1b an Pin 9 (PA4, ADC4)
'Poti 2a an Pin 8 (PA5, ADC5)
'Poti 2b an Pin 7 (PA6, ADC6)
'Servo 1 an Pin 2 (PB0) Baggerarmservo
'Servo 2 an Pin 13 (PA0) Baggerarmservo
'Servo 3 an Pin 12 (PA1) Kettenantrieb
'Servo 4 an Pin 11 (PA2) Kettenantrieb
'
'===============================================================================
$regfile = "attiny84.dat"
$crystal = 8000000 'FuseBit CKDIV8 deaktivieren
$hwstack = 50
$swstack = 50
$framesize = 50
'-------------------------------------------------------------------------------------------------
'Timer konfigurieren
'-------------------------------------------------------------------------------------------------
Config Timer1 = Timer , Prescale = 8 'Timer für Einlesen RC Signale
Start Timer1
Config Timer0 = Timer , Prescale = 64 'Timer für Servoausgabe, Wert 125 entspricht 1ms, Wert 250 entspricht 2ms
Enable Timer0
On Timer0 Servoausgabe
'-------------------------------------------------------------------------------------------------
'Poti-Eingänge konfigurieren
'-------------------------------------------------------------------------------------------------
Config Adc = Single , Prescaler = Auto
Start Adc
'-------------------------------------------------------------------------------------------------------------
'Variablen definieren
'-------------------------------------------------------------------------------------------------------------
Const Jitterausgleich = 8 'Wert in µs um den sich gemessene Impulslaenge aendern darf/muss, bevor Servo gestellt wird
'Nur in Vielfachen von 8 veraendern (8, 16, 32, ..)
'Variablen fürs RC Einlesen
Dim Rc_signal_1_start As Word
Dim Rc_signal_2_start As Word
Dim Rc_signal_3_start As Word
Dim Rc_signal_1_stop As Word
Dim Rc_signal_2_stop As Word
Dim Rc_signal_3_stop As Word
Dim Rc_signal_1_stop_flag As Byte
Dim Rc_signal_2_stop_flag As Byte
Dim Rc_signal_3_stop_flag As Byte
Dim Impulslaenge_1 As Word
Dim Impulslaenge_2 As Word
Dim Impulslaenge_3 As Word
Dim Impulslaenge_1_alt As Word
Dim Impulslaenge_2_alt As Word
Dim Impulslaenge_3_alt As Word
Dim Impulslaengenaenderung As Integer 'wird nur einmal gebraucht, da nur Hilfswariable
'Variablen für Servowegsbegrenzung
Dim Poti_1a As Word
Dim Poti_1b As Word
Dim Poti_2a As Word
Dim Poti_2b As Word
Dim Limit_1a As Byte
Dim Limit_1b As Byte
Dim Limit_2a As Byte
Dim Limit_2b As Byte
'Variablen für Berechnungen
Dim Berechnung_1 As Word
Dim Berechnung_2 As Word
Dim Berechnung_3 As Word
Dim Berechnung_4 As Word
Dim Berechnung_1a As Word
Dim Berechnung_2a As Word
Dim Berechnung_3a As Word
Dim Berechnung_4a As Word
Dim I As Word
Dim Servospeed As Word
'Variable für Deltamischer
Dim A1 As Integer
Dim A2 As Integer
Dim B1 As Integer
Dim B2 As Integer
'Variablen für Servoausgabe
Dim Kanal As Byte
Dim Servoausgabe_1 As Byte
Dim Servoausgabe_2 As Byte
Dim Servoausgabe_3 As Byte
Dim Servoausgabe_4 As Byte
'-------------------------------------------------------------------------------------------------
'Einigen Variablen Werte zuweisen
'-------------------------------------------------------------------------------------------------
Kanal = 1
Berechnung_1a = 187 'alle Servos erst mal auf Mittelstellung
Berechnung_2a = 187
Berechnung_3a = 187
Berechnung_4a = 187
'-------------------------------------------------------------------------------------------------------------
'Ein- und Ausgang festlegen
'-------------------------------------------------------------------------------------------------------------
Ddra = &B00000111 'PA0 - PA2 werden Ausgänge, restlicher PortA bleibt Eingang
Ddrb = &B00000001 'PB0 wird Ausgang, restlicher PortB bleibt Eingang
'-------------------------------------------------------------------------------------------------
'Interrupt-Service-Routinen konfigurieren und freigeben
'-------------------------------------------------------------------------------------------------
'Info:
'Alle Porta Pinchangeinterrupts sind "PCINT0" zugeordnet.
'Alle Portb Pinchangeinterrupts sind "PCINT1" zugeordnet.
Pcmsk1.pcint9 = 1 'beim Flankenwechsel an PB1/PCINT9 (RC Eingang 1) Pinchangeinterrupt1 auslösen und in die Subroutine springen
Enable Pcint1 'Pinchangeinterrupt1 (1 weil auf PortB) zulassen
On Pcint1 Rc_eingang_1 Nosave 'Register werden manuel in der ISR gesichert
Pcmsk0.pcint7 = 1 'beim Flankenwechsel an PA7/PCINT6 (RC Eingang 2) Pinchangeinterrupt0 auslösen und in die Subroutine springen
Enable Pcint0 'Pinchangeinterrupt0 (0 weil auf PortA) zulassen
On Pcint0 Rc_eingang_2 Nosave 'Register werden manuel in der ISR gesichert
Config Int0 = Change 'beim Flankenwechsel an PB2/INT0 (RC Eingang 3) Int0 auslösen und in die Subroutine springen
Enable Int0
On Int0 Rc_eingang_3 Nosave 'Register werden manuel in der ISR gesichert
Enable Interrupts
'======================================================
'Hauptprogramm
'======================================================
Do
'RC Impulslänge berechnen, auf 8-Bit umrechnen und Grenzwerte festlegen
If Rc_signal_1_stop_flag = 1 Then 'Bearbeitung nur, wenn ISR Pulsende gefunden hat
Disable Pcint1 'Mögliche Korruption durch ISR vorbeugen
Impulslaenge_1 = Rc_signal_1_stop - Rc_signal_1_start
Enable Pcint1
Rc_signal_1_stop_flag = 0
Impulslaengenaenderung = Impulslaenge_1_alt - Impulslaenge_1 'SW-Hysterese
If Abs(impulslaengenaenderung) >= Jitterausgleich Then
Impulslaenge_1_alt = Impulslaenge_1
Berechnung_1 = Impulslaenge_1
Shift Berechnung_1 , Right , 3 'Division von Berechnung_1 durch 8
If Berechnung_1 > 255 Then Berechnung_1 = 255
If Berechnung_1 < 120 Then Berechnung_1 = 120
End If
End If
If Rc_signal_2_stop_flag = 1 Then 'Bearbeitung nur, wenn ISR Pulsende gefunden hat
Disable Pcint0 'Mögliche Korruption durch ISR vorbeugen
Impulslaenge_2 = Rc_signal_2_stop - Rc_signal_2_start
Enable Pcint0
Rc_signal_2_stop_flag = 0
Impulslaengenaenderung = Impulslaenge_2_alt - Impulslaenge_2 'SW-Hysterese
If Abs(impulslaengenaenderung) >= Jitterausgleich Then
Impulslaenge_2_alt = Impulslaenge_2
Berechnung_2 = Impulslaenge_2
Shift Berechnung_2 , Right , 3 'Division von Berechnung_1 durch 8
If Berechnung_2 > 255 Then Berechnung_2 = 255
If Berechnung_2 < 120 Then Berechnung_2 = 120
End If
End If
If Rc_signal_3_stop_flag = 1 Then 'Bearbeitung nur, wenn ISR Pulsende gefunden hat
Disable Int0 'Mögliche Korruption durch ISR vorbeugen
Impulslaenge_3 = Rc_signal_3_stop - Rc_signal_3_start
Enable Int0
Rc_signal_3_stop_flag = 0
Impulslaengenaenderung = Impulslaenge_3_alt - Impulslaenge_3 'SW-Hysterese
If Abs(impulslaengenaenderung) >= Jitterausgleich Then
Impulslaenge_3_alt = Impulslaenge_3
Berechnung_3 = Impulslaenge_3
Shift Berechnung_3 , Right , 3 'Division von Berechnung_1 durch 8
If Berechnung_3 > 255 Then Berechnung_3 = 255
If Berechnung_3 < 120 Then Berechnung_3 = 120
End If
End If
'--------------------------------------------------
'Start der größten IF Bedingung (Betriebsmodewahl)
'FAHRBETRIEB
'--------------------------------------------------
If Berechnung_3 < 187 Then 'falls der Betriebswahlkanal unter Mittelstellung ist, ist Fahrbetrieb
'(
'Hier wird durchgeschleift, NUR für Testzwecke!
Berechnung_3a = Berechnung_1 'Werte vom ersten RC Kanal gehen zu Fahrservo 1
Berechnung_4a = Berechnung_2 'Werte vom zweiten RC Kanal gehen zu Fahrservo 2
'Ende durchschleifen
')
'Hier wird gemischt (Deltamischer)
A1 = Berechnung_1 - 187 ' Mittelstellung nun bei 0, Auschläge von -67 bis +67
A2 = Berechnung_2 - 187
A1 = A1 / 2 ' Werte halbieren (Mischanteile)
'A2 = A2 / 2
B1 = A1 + A2
B2 = A1 - A2
B1 = 185 + B1 ' Unterschiedliche Ausgangswerte wegen Nullpunktjustierung!
B2 = 188 + B2
Berechnung_3a = B1
Berechnung_4a = B2
'Ende der Mischer Berechnung
If Berechnung_3a < 120 Then 'Werte begrenzen
Berechnung_3a = 120
End If
If Berechnung_3a > 255 Then
Berechnung_3a = 255
End If
If Berechnung_4a < 120 Then
Berechnung_4a = 120
End If
If Berechnung_4a > 255 Then
Berechnung_4a = 255
End If
'--------------------------------------------------
'BAGGERETRIEB
'--------------------------------------------------
Else 'falls Betriebswahlschalter über Mittelstellung ist, ist Baggerbetrieb
Berechnung_3a = 187 ' Fahrservos in Mittelstellung
Berechnung_4a = 187 ' Fahrservos in Mittelstellung
'Berechnung der Servogeschwindigkeit und Servostellung
'Erstes Baggerarmservo
If Berechnung_1 < 182 Then 'Info: Todband (Mittelstellung) liegt zwischen 182 und 193
Servospeed = Berechnung_1 - 120 'ergibt Werte zwischen 0 (schnell) und 61 (langsam)
Servospeed = Servospeed * 100 'Werte hochscalieren (Faktor 30 bis 100, höherer Faktor = langsamere minimal Speed)
For I = 1 To Servospeed 'Werte von 0 (schnell) bis max 6100 (langsam) sinnvoll
Next I
Decr Berechnung_1a
End If
If Berechnung_1 > 193 Then
Servospeed = 255 - Berechnung_1
Servospeed = Servospeed * 100
For I = 1 To Servospeed
Next I
Incr Berechnung_1a
End If
'Zweites Baggerarmservo
If Berechnung_2 < 182 Then 'Info: Todband (Mittelstellung) liegt zwischen 182 und 193
Servospeed = Berechnung_2 - 120 'ergibt Werte zwischen 0 (schnell) und 61 (langsam)
Servospeed = Servospeed * 100 'Werte hochscalieren (Faktor 30 bis 100, höherer Faktor = langsamere minimal Speed)
For I = 1 To Servospeed 'Werte von 0 (schnell) bis max 6100 (langsam) sinnvoll
Next I
Decr Berechnung_2a
End If
If Berechnung_2 > 193 Then
Servospeed = 255 - Berechnung_2
Servospeed = Servospeed * 100
For I = 1 To Servospeed
Next I
Incr Berechnung_2a
End If
'Potis abfragen und Werte für Servolimits berechnen
Poti_1a = Getadc(3)
Poti_1a = Poti_1a / 16 '1024 / 16 = 62,5 ergibt ca 25% Wegbegrenzung
Limit_1a = 255 - Poti_1a
Poti_1b = Getadc(4)
Poti_1b = Poti_1b / 16
Limit_1b = 120 + Poti_1b
Poti_2a = Getadc(5)
Poti_2a = Poti_2a / 16
Limit_2a = 255 - Poti_2a
Poti_2b = Getadc(6)
Poti_2b = Poti_2b / 16
Limit_2b = 120 + Poti_2b
'RC Signal auf Limitwerte begrenzen (nur Baggerarmservos)
If Berechnung_1a > Limit_1a Then 'zu hohe Werte abfangen
Berechnung_1a = Limit_1a
End If
If Berechnung_1a < Limit_1b Then 'zu kleine Werte abfangen
Berechnung_1a = Limit_1b
End If
If Berechnung_2a > Limit_2a Then
Berechnung_2a = Limit_2a
End If
If Berechnung_2a < Limit_2b Then
Berechnung_2a = Limit_2b
End If
' Ende der größten IF Bedingung
End If
'Finale Berechnungen an Servoausgabe übergeben, Servoausgabe_x zum direkten Laden in TCNT0 vorbereiten (Cmd Load umgangen)
Servoausgabe_1 = 256 - Berechnung_1a 'Baggerarmservo 1,
Servoausgabe_2 = 256 - Berechnung_2a 'Baggerarmservo 2
Servoausgabe_3 = 256 - Berechnung_3a 'Fahrservo oder ESC 1
Servoausgabe_4 = 256 - Berechnung_4a 'Fahrservo oder ESC 2
Loop
'======================================================
'ISR
'======================================================
Rc_eingang_1:
$asm
push r17 'Register auf Stack sichern
sbis pinb , 1 'Skip next Instr if PINBx = 1
rjmp Puls_ende1 'Springe Puls_ende
in r17 , tcnt1l 'Timer1 low Byte holen
sts {Rc_signal_1_start} , r17 'Speichere Timer1 low Byte nach Rc_signal_x_start low Byte
in r17 , tcnt1h 'Timer1 high Byte holen
sts {Rc_signal_1_start} + 1 , r17 'Speichere Timer1 high Byte nach Rc_signal_x_start high Byte
rjmp ende1 'Springe zum Ende
Puls_ende1:
in r17 , tcnt1l 'Timer1 low Byte holen
sts {Rc_signal_1_stop} , r17 'Speichere Timer1 low Byte nach Rc_signal_x_stop low Byte
in r17 , tcnt1h 'Timer1 high Byte holen
sts {Rc_signal_1_stop} + 1 , r17 'Speichere Timer1 high Byte nach Rc_signal_x_stop high Byte
ldi r17 , 1
sts {Rc_signal_1_stop_flag} , r17 'Setze Flag zur Bearbeitung von Impulslaenge in Hauptschleife
Ende1:
pop r17 'Register vom Stack zurückholen
$end Asm
Return
Rc_eingang_2:
$asm
push r17 'Register auf Stack sichern
sbis pinA , 7 'Skip next Instr if PINBx = 1
rjmp Puls_ende2 'Springe Puls_ende
in r17 , tcnt1l 'Timer1 low Byte holen
sts {Rc_signal_2_start} , r17 'Speichere Timer1 low Byte nach Rc_signal_x_start low Byte
in r17 , tcnt1h 'Timer1 high Byte holen
sts {Rc_signal_2_start} + 1 , r17 'Speichere Timer1 high Byte nach Rc_signal_x_start high Byte
rjmp ende2 'Springe zum Ende
Puls_ende2:
in r17 , tcnt1l 'Timer1 low Byte holen
sts {Rc_signal_2_stop} , r17 'Speichere Timer1 low Byte nach Rc_signal_x_stop low Byte
in r17 , tcnt1h 'Timer1 high Byte holen
sts {Rc_signal_2_stop} + 1 , r17 'Speichere Timer1 high Byte nach Rc_signal_x_stop high Byte
ldi r17 , 1
sts {Rc_signal_2_stop_flag} , r17 'Setze Flag zur Bearbeitung von Impulslaenge in Hauptschleife
Ende2:
pop r17 'Register vom Stack zurückholen
$end Asm
Return
Rc_eingang_3:
$asm
push r17 'Register auf Stack sichern
sbis pinB , 2 'Skip next Instr if PINBx = 1
rjmp Puls_ende3 'Springe Puls_ende
in r17 , tcnt1l 'Timer1 low Byte holen
sts {Rc_signal_3_start} , r17 'Speichere Timer1 low Byte nach Rc_signal_x_start low Byte
in r17 , tcnt1h 'Timer1 high Byte holen
sts {Rc_signal_3_start} + 1 , r17 'Speichere Timer1 high Byte nach Rc_signal_x_start high Byte
rjmp ende3 'Springe zum Ende
Puls_ende3:
in r17 , tcnt1l 'Timer1 low Byte holen
sts {Rc_signal_3_stop} , r17 'Speichere Timer1 low Byte nach Rc_signal_x_stop low Byte
in r17 , tcnt1h 'Timer1 high Byte holen
sts {Rc_signal_3_stop} + 1 , r17 'Speichere Timer1 high Byte nach Rc_signal_x_stop high Byte
ldi r17 , 1
sts {Rc_signal_3_stop_flag} , r17 'Setze Flag zur Bearbeitung von Impulslaenge in Hauptschleife
Ende3:
pop r17 'Register vom Stack zurückholen
$end Asm
Return
Servoausgabe:
$asm
push r16 'Register auf Stack sichern
in r16,sreg 'Statusregister holen und halten
push r17 'Register auf Stack sichern
lds r17 , {kanal} 'hole Kanalnummer
cpi r17 , 1 'check Kanal und ...
brne LABEL_KANAL_2 '... "wenn nicht gleich" verzweige zum nächsten Kanal
sbic portB , 0 'Skip next instr. wenn PORTA.0 = 0 ist
rjmp label_1 'Springe zum LOW-setzen des Servosignals
sts {kanal} , r17 'Sichere Kanalnummer
lds r17 , {Servoausgabe_1} 'Hole aufbereiteten Pulslängenwert für Timer0
Out Tcnt0 , R17 'Setze Timer0 mit Pulslängenwert
sbi portB , 0 'Setze Servosignal HIGH
rjmp Ende_isr 'Springe zum Ende der ISR
Label_1:
cbi portB , 0 'Setze Servosignal nach LOW
inc r17 'Erhöhe Kanalnummer
Label_kanal_2: 'Bearbeitung von Kanal 2
cpi r17 , 2 'check Kanal und ...
brne LABEL_KANAL_3 '... "wenn nicht gleich" verzweige zum nächsten Kanal
sbic porta , 0 'Skip next instr. wenn PORTA.1 = 0 ist
rjmp label_2 'Springe zum LOW-setzen des Servosignals
sts {kanal} , r17 'Sichere Kanalnummer
lds r17 , {Servoausgabe_2} 'Hole aufbereiteten Pulslängenwert für Timer0
Out Tcnt0 , R17 'Setze Timer0 mit Pulslängenwert
sbi porta , 0 'Setze Servosignal HIGH
rjmp Ende_isr 'Springe zum Ende der ISR
Label_2:
cbi porta , 0 'Setze Servosignal nach LOW
inc r17 'Erhöhe Kanalnummer
Label_kanal_3: 'Bearbeitung von Kanal 3
cpi r17 , 3 'check Kanal und ...
brne LABEL_KANAL_4 '... "wenn nicht gleich" verzweige zum nächsten Kanal
sbic porta , 1 'Skip next instr. wenn PORTA.2 = 0 ist
rjmp label_3 'Springe zum LOW-setzen des Servosignals
sts {kanal} , r17 'Sichere Kanalnummer
lds r17 , {Servoausgabe_3} 'Hole aufbereiteten Pulslängenwert für Timer0
Out Tcnt0 , R17 'Setze Timer0 mit Pulslängenwert
sbi porta , 1 'Setze Servosignal HIGH
rjmp Ende_isr 'Springe zum Ende der ISR
Label_3:
cbi porta , 1 'Setze Servosignal nach LOW
inc r17 'Erhöhe Kanalnummer
Label_kanal_4: 'Bearbeitung von Kanal 4
cpi r17 , 4 'check Kanal und ...
brne LABEL_KANAL_5 '... "wenn nicht gleich" verzweige zum nächsten Kanal
sbic porta , 2 'Skip next instr. wenn PORTA.3 = 0 ist
rjmp label_4 'Springe zum LOW-setzen des Servosignals
sts {kanal} , r17 'Sichere Kanalnummer
lds r17 , {Servoausgabe_4} 'Hole aufbereiteten Pulslängenwert für Timer0
Out Tcnt0 , R17 'Setze Timer0 mit Pulslängenwert
sbi porta , 2 'Setze Servosignal HIGH
rjmp Ende_isr 'Springe zum Ende der ISR
Label_4:
cbi porta , 2 'Setze Servosignal nach LOW
inc r17 'Erhöhe Kanalnummer
'Pausenauffüllung
'Pausenanfang
inc r17
sts {kanal} , r17 'Sichere Kanalnummer
ldi r17 , &B00000101 'CS00 und CS02 für prescaler 1024 in Timer0 vorbereiten
Out Tccr0b , R17 'Timer0 läuft mit 128µs Auflösung
ldi r17 , 162 'Wert für ca 12ms bis zum nächsten OVF (162=256-94) bei prescaler 1024
Out Tcnt0 , R17
rjmp Ende_isr 'Springe zum Ende der ISR
'Pausenende
Label_kanal_5: 'Bearbeitung von Kanal 5 (Pausenende)
ldi r17 , &B00000011 'CS00 und CS01 für prescaler 64 in Timer0 vorbereiten
Out Tccr0b , R17 'Timer0 läuft wieder mit 8µs Auflösung
ldi r17 , 1 'Kanalnummer auf 1 setzen
sts {kanal} , r17 'Sichere Kanalnummer
Ende_isr:
pop r17 'Register vom Stack zurückholen
Out Sreg , R16 'Statusregister zurückspeichern
pop r16 'Register vom Stack zurückholen
$end Asm
Return
Zitieren
Lesezeichen