1:
2:
3:

digitalWrite(Servopin, HIGH);
delayMicroseconds(Pulslänge - 5);
digitalWrite(Servopin, LOW);

1:
2:
3:
4:
5:
6:
7:
8:
9:
10:
11:
12:
13:
14:
15:
16:
17:
18:
19:
20:
21:
22:
23:
24:
25:
26:
27:
28:
29:
30:
31:
32:
33:
34:
35:
36:
37:
38:
39:
40:
41:
42:
43:
44:
45:
46:
47:
48:
49:
50:
51:
52:
53:
54:
55:
56:
57:
58:
59:
60:
61:
62:
63:
64:
65:
66:
67:
68:
69:
70:
71:
72:
73:
74:
75:
76:
77:
78:
79:
80:
81:
82:
83:
84:
85:
86:
87:
88:
89:
90:
91:
92:
93:
94:
95:
96:
97:
98:
99:
100:
101:
102:
103:
104:
105:
106:
107:
108:
109:
110:
111:
112:
113:
114:
115:
116:
117:
118:
119:
120:
121:
122:
123:
124:
125:
126:
127:
128:
129:
130:
131:
132:
133:
134:
135:
136:
137:
138:
139:
140:
141:
142:
143:
144:
145:
146:
147:
148:
149:
150:
151:
152:
153:
154:
155:
156:
157:
158:
159:
160:
161:
162:
163:
164:
165:
166:
167:
168:
169:
170:
171:
172:
173:
174:
175:
176:
177:

/*
  Unimog 1.3

  Weitere Schritte in Richtung duale Steuerung
  Einführung der Verhaltensmodi Kind, RC, Fehler
  Versuch der gemischten Lenkung
  Fahrsteuerung durch Kind mit zwei Fußtastern
*/ 

int RC_Gas_quer; //linker Steuerknüppel links-rechts
int RC_Lenkung_laengs; //rechter Steuerknüppel vor-zurück
int RC_Lenkung; //rechter Steuerknüppel links-rechts
int RC_Gas; //linker Steuerknüppel vor-zurück
int RC_Schaltung; //Schalter 5.Kanal an der Fernsteuerung
int Lenkmotor_Wert = 1500; //Servosignal-Ausgabe für kleinen Lenkmotor-Fahrtregler
int Fahrtregler_Wert = 1000; //Servosignal-Ausgabe für Haupt-Fahrtregler
int Nullpos;
int Sollpos;
int Istpos;
int Lenkrad;
long Lenkrad_Null;
int Lenkrad_Zaehler;
int Vorwaerts;
int Rueckwaerts;
byte Modus = 0;
int Moduszaehler = 0;


const byte RC_Pin = 2;
const byte Lenkmotor_Pin = 3;
const byte Fahrtregler_Pin = 4;
const byte Umpolrelais_Pin = 5;
const byte Istpos_Pin = 0;
const byte Nullpos_Pin = 1;
const byte Lenkrad_Pin = 2;
const byte Gas_Pin = 3;
const byte Schaltung_Pin = 4;
const byte Fehler = 0;
const byte RC = 1;
const byte Kind = 2;

void setup() {
  
  pinMode(RC_Pin, INPUT);
  pinMode(Lenkmotor_Pin, OUTPUT);
  pinMode(Fahrtregler_Pin, OUTPUT);
  pinMode(Umpolrelais_Pin, OUTPUT);
  
}

void loop() {

  RC_Gas_quer = pulseIn(2, LOW);
  RC_Lenkung_laengs = pulseIn(2, LOW);
  RC_Lenkung = pulseIn(2, LOW);
  RC_Gas = pulseIn(2, LOW);
  RC_Schaltung = pulseIn(2, LOW);

  Istpos = analogRead(Istpos_Pin);
  Nullpos = analogRead(Nullpos_Pin);
  Lenkrad = analogRead(Lenkrad_Pin);
  Vorwaerts = analogRead(Gas_Pin); //hier hängt aber nur ein Fußtaster dran
  Rueckwaerts = analogRead(Schaltung_Pin); //hier ein anderer Fußtaster zum Rückwärtsfahren 

  //gleitender Mittelwert und Nullwertbildung für Lenkrad-Drehmomentsensor
  Lenkrad_Null = Lenkrad_Null + Lenkrad;
  if (Lenkrad_Zaehler >= 3000) {
    Lenkrad_Null = Lenkrad_Null - (Lenkrad_Null / 3000);
  }
  else {
    Lenkrad_Zaehler++;
  }
  Lenkrad = Lenkrad - Lenkrad_Null / Lenkrad_Zaehler;
   
  //Nach 1s gültiger RC_Lenkung Neutralwerte vom Fehlermodus in den RC-Modus wechseln
  if (Modus == Fehler) {
    if (RC_Lenkung >= 1485 && RC_Lenkung <= 1515) Moduszaehler++; 
    if (Moduszaehler >= 50) {
      Moduszaehler = 0;
      Modus = RC;
    }
  }

  //Nach 10s RC_Lenkung Neutralwerte in den Kind-Modus wechseln
  if (Modus == RC) {
    if (RC_Lenkung >= 1485 && RC_Lenkung <= 1515) Moduszaehler++; 
    if (Moduszaehler >= 500) {
      Moduszaehler = 0;
      Modus = Kind;
    }
  }

  //Bei RC_Gas größer Neutral oder RC_Lenkung nicht neutral sofort in den RC-Modus
  if (Modus == Kind) {
    if (RC_Lenkung < 1485 || RC_Lenkung > 1515) Modus = RC;
    if (RC_Gas > 1150) Modus = RC;
  }

  //Verhalten im Fhelermodus
  if (Modus == Fehler) {
         Lenkmotor_Wert = 1500;
         Fahrtregler_Wert = 1000;
         digitalWrite(Umpolrelais_Pin, LOW);
  }

  //Verhalten im RC-Modus
  if (Modus == RC) {
         Fahrtregler_Wert = (RC_Gas - 1500) * 1.32 + 1500;
         Sollpos = (RC_Lenkung - 1500) / 2.35; //Sollwert draus machen
         Lenkmotor_Wert = (Sollpos - Istpos + Nullpos) * 10 + 1500;
         if (Lenkmotor_Wert > 2000) Lenkmotor_Wert = 2000;
         if (Lenkmotor_Wert < 1000) Lenkmotor_Wert = 1000;
         if (RC_Schaltung >= 1500) {
                digitalWrite(Umpolrelais_Pin, HIGH);
         }
         else{
                digitalWrite(Umpolrelais_Pin, LOW);
         }
  }

  //Verhalten im Kind-Modus
  if (Modus == Kind) {
         
         //ganze Lenkerei
         if (Lenkrad < -10) {
           Sollpos = Sollpos + 5;
           if (Sollpos > 170) Sollpos = 170;
         }
         if (Lenkrad > 10) {
           Sollpos = Sollpos -5;
           if (Sollpos < -190) Sollpos = -190;
         }
         if (Lenkrad >= -10 && Lenkrad <= 10) {
            if (Sollpos > 0) Sollpos--;
            if (Sollpos < 0) Sollpos++;
         }
         Lenkmotor_Wert = (Sollpos - Istpos + Nullpos) * 10 + 1500;
         if (Lenkmotor_Wert > 2000) Lenkmotor_Wert = 2000;
         if (Lenkmotor_Wert < 1000) Lenkmotor_Wert = 1000;  

         //ganze Fahrerei mit schlichten Fußtastern als Gaspedale
         if (Vorwaerts < 512 && Rueckwaerts > 512) {
            if (Fahrtregler_Wert < 1050) digitalWrite(Umpolrelais_Pin, LOW);
            if (Fahrtregler_Wert < 1200) {
              Fahrtregler_Wert = Fahrtregler_Wert + 3;
            }
            else {
              Fahrtregler_Wert = Fahrtregler_Wert + 8;
            }
         }
         if (Vorwaerts > 512 && Rueckwaerts < 512) {
            if (Fahrtregler_Wert < 1050) digitalWrite(Umpolrelais_Pin, HIGH);
            if (Fahrtregler_Wert < 1200) {
              Fahrtregler_Wert = Fahrtregler_Wert + 3;
            }
            else {
              Fahrtregler_Wert = Fahrtregler_Wert + 8;
            }
         }
         if (Vorwaerts < 512 && Rueckwaerts < 512) Fahrtregler_Wert = 1000;
         if (Vorwaerts > 512 && Rueckwaerts > 512) Fahrtregler_Wert = Fahrtregler_Wert -20;
         if (Fahrtregler_Wert > 2000) Fahrtregler_Wert = 2000;
         if (Fahrtregler_Wert < 1000) Fahrtregler_Wert = 1000;
  }
  digitalWrite(Lenkmotor_Pin, HIGH);
  delayMicroseconds(Lenkmotor_Wert - 5);
  digitalWrite(Lenkmotor_Pin, LOW);

  delayMicroseconds(30);
  
  digitalWrite(Fahrtregler_Pin, HIGH);
  delayMicroseconds(Fahrtregler_Wert - 5);
  digitalWrite(Fahrtregler_Pin, LOW);

}