====== Steuerung der Motoren ======
Wir werden alle Längenangaben in cm vornehmen (das ist auch die Masseinheit für den Distanzsensor).

Die Motoren haben einen Winkelsensor, der in 1-Grad Schritten die Position vom Motor angibt. Mit diesem Sensor ist eine einigermassen präzise Navigation möglich.

<WRAP todo>
Messen Sie den Roboter aus und notieren Sie sich folgende Grössen (in cm):
  * Raddurchmesser.
  * Abstand der Räder (für die Berechnung von Kurven).
Berechnen Sie daraus folgende Grössen:
  * Radumfang.
  * Distanz in cm, die das Rad mit 1° Rotation zurücklegt.
  * Anzahl Grad Rotation, die ein Rad zurücklegen muss, um den Roboter um 90° zu drehen (angenommen das andere Rad steht still).

<code python viertelkreis.py>
from ev3robot import *

# Roboter initialisieren

robot = LegoRobot()
links = Motor(MotorPort.A)    
robot.addPart(links)
links.rotateTo(100, True)    # Hier die korrekte Gradzahl eintragen!

robot.exit()                  # Programm korrekt beenden
</code>
{{:lehrkraefte:blc:informatik:glf20:robotik:pasted:20210503-091521.png?320}}
</WRAP>


==== Wichtigste Gear-Funktionen ====
<WRAP info>
  * Voraussetzung ist, dass die Variable ''gear'' auch ein ''Gear''-Objekt ist.
  * **Nicht-blockierend** heisst, das Programm läuft sofort weiter. Die Motoren drehen einfach weiter in dieser Einstellung, bis eine Änderung vorgenommen wird.
  * **Blockierend** heisst, das Programm wartet an dieser Stelle die gegebene Anzahl **Millisekunden** und kann während dieser Zeit nicht auf Sensoreingaben reagieren. Die Motoren **stoppen nach** einen blockierenden Befehl wieder.

^ nicht-blockierend ^ blockierend ^ Beschreibung ^
| gear.backward() | gear.backward(ms) | fährt rückwärts |
| gear.forward()  | gear.forward(ms) | fährt forwärts |
| gear.left() | gear.left(ms)	| dreht links  |
| gear.right() | gear.right(ms)	| dreht rechts  |
| gear.leftArc(radius)	| gear.leftArc(radius , ms)	| fährt auf einem Linksbogen (negative Radien bewirken eine Rückwärtsbewegung) |
| gear.rightArc(radius)	| gear.rightArc(radius , ms) | Rechtsbogen |
| gear.setSpeed(speed) | | setzt die Geschwindigkeit (Werte zwischen 0 und ungefähr 70 sind überhaupt sinnvoll.) |
| gear.stop() |	| stoppt das Fahrwerk (nötig nach nicht-blockierenden Befehlen |
| getLeftMotorCount() | | Gibt die Position des linken Motors in Grad zurück | 
| resetLeftMotorCount() | | Setzt den Gradzähler des linken Motors auf 0° zurück | 

Vollständige Dokumentation der Funktionen: http://tigerjython.ch/index.php?inhalt_links=navigation.inc.php&inhalt_mitte=robotik/robotikdoc.html
</WRAP>


<WRAP todo>
  * Der Wert ''v'' in ''gear.setSpeed(v)'' ist ein Prozentangabe und bezieht sich auf die Maximalgeschwindigkeit, wobei nur bis 70% sinnvoll sind (darüber kommt es mehr und mehr zu Ungenauigkeiten). Ziel ist es, den Umrechnungsfaktor von dieser Prozentangabe in cm/s zu erhalten.
    * Testen Sie dazu das folgende Programm ''speedtest.py''.
    * Ermitteln Sie den Umrechnungsfaktor.


<code python speedtest.py>
from ev3robot import *

######################
##  Initialisierung ##
######################

# Roboter initialisieren
robot = LegoRobot()

# Raeder hinzufuegen
gear = Gear()
robot.addPart(gear)

###########################################
## B E G I N   D E S   P R O G R A M M S ##
###########################################
v = 40                            # Werte bis ca. 70 sind sinnvoll
t = 2                             # Zeit, während der gemessen wird.
print("v=%d" % v)                 # Anzeige auf dem Computer im Ausgabefenster
gear.setSpeed(v)                  # Geschwindigkeit setzen
gear.forward()                    # Fahren...
Tools.delay(1000)                 # Geschwindigkeit stabilisieren, 1 Sekunde warten
start = gear.getLeftMotorCount()  # Aktuelle Winkel-Position in Grad in die Variable start speichern.
Tools.delay(t*1000)               # t Sekunden warten (während die Räder weiter drehen)
distanz = gear.getLeftMotorCount()-start    # Differenz zur Startposition berechnen (in Grad)
print("Geschwindigkeit in Grad/sec %f" % (distanz/t))
gear.stop()
robot.exit()                  # Programm korrekt beenden
</code>

</WRAP>

=== Zusatzaufgabe für Musiker (und alle Ton-Begeisterte) ===
Testen Sie folgendes Program (drehen Sie dazu den Roboter auf den Kopf, damit die Räder frei drehen können. Was hören Sie und warum?
<code python speedtest.py>
from ev3robot import *

######################
##  Initialisierung ##
######################

# Roboter initialisieren
robot = LegoRobot()

# Raeder hinzufuegen
gear = Gear()
robot.addPart(gear)

###########################################
## B E G I N   D E S   P R O G R A M M S ##
###########################################

for v in range(10,90, 10):
    print("\n\n\n\nv=%d" % v)
    gear.setSpeed(v)                 # Werte bis ca. 70 sind sinnvoll
    gear.forward()                    # Fahren...
    Tools.delay(2000)                 # Geschwindigkeit stabilisieren, 1 Sekunde warten
   
gear.stop()
robot.exit()                  # Programm korrekt beenden
</code>
===== Sauber anfahren und bremsen =====
<WRAP todo>
  * Schreiben Sie ein Programm, das den Roboter mit einer gegebenen Geschwindigkeit ''myspeed'' "genau" einen Meter geradeaus fahren lässt und dann stoppt.
    * Berechnen Sie dazu aus der Geschwindigkeit die benötigte Zeit.
    * Wie genau fährt der Roboter (Richtung und Distanz)?
<code python>
# INITIALISIERUNG FEHLT HIER NOCH
v = 50  # Geschwindkeit in setSpeed
d = 100 # Distanz
zeit = ????   # Aus v und d die benötigte Zeit berechnen

gear.setSpeed(v)         # 
gear.forward(int(zeit*1000))  # Millisekunden als Ganzzahl

gear.stop()

robot.exit()                  # Programm korrekt beenden
</code>
  * Schreiben Sie das Programm wie folgt um:
<code python>
# INITIALISIERUNG FEHLT HIER NOCH
einMeter = 1234  # Die korrekte Anzahl Grad für 1 m  ausrechnen und eintragen!
gear.resetLeftMotorCount()
mySpeed = 60   # Egal welche Geschwindigkeit
gear.setSpeed(mySpeed)
gear.forward()
while gear.getLeftMotorCount()<einMeter:
   pass # Tu einfach nix
gear.stop()
robot.exit()
</code>
  * Die Geschwindgkeit vor der while-Schlaufe soll auf 5 gesetzt werden.
  * Die Geschwindigkeit soll auf den ersten 10cm langsam auf ''myspeed'' hochgefahren werden.
  * Finden Sie dazu eine lineare Funktion $v(d)=m \cdot d + q$, die zur Distanz $d$ (in Grad = ''gear.getLeftMotorCount()'') die Geschwindkeit $v$ berechnet, und zwar so dass für $d=0$ der Wert $v=5$ herauskommt und für das $d$, das der Distanz 10 cm entspricht, soll der Wert der Variablen ''mySpeed'' herauskommen.
  * Setzen mit dieser Funktion in der while-Schlaufe die Geschwindigkeit, wenn ''gear.getLeftMotorCount()'' kleiner als 40 cm ist (Masseinheiten beachten). Setzen Sie die berechnete Geschwindigkeit in die int-Funktion, z.B. ''gear.setSpeed(int(meinegeschwindigkeit))''.
  * Testen Sie Ihren Code.
  * Machen Sie das umgekehrt Gleiche am Ende auf den letzten 40 cm

**Zusatz für Mathematik-Begeisterte**: Die Beschleunigung (Änderung der Geschwindigkeit pro Zeit) ist mit der obigen Methode nicht linear. Welcher Typ Funktion müsste $v(s)$ sein, damit die Beschleunigung konstant wäre ($s$ ist hier die zurückgelegte Strecke)?
<hidden Lösungsansatz>
Bei konstanter Beschleunigung $a$ sehen ist $v(t)=a\cdot t$ und $s(t) = \frac{1}{2}a\cdot t^2$. Wir nehmen der Einfachheit halber an, dass $v(0)=0$ und $s(0)=0$. Ansonsten verkomplizieren sich die Formeln etwas, das Ergebnis ist aber qualitativ das gleiche.

Aus $s(t)=\frac{1}{2}a\cdot t^2$ erhält man $t=\sqrt{\frac{2s}{a}}$, eingesetzt in $v(t)$ erhält man $v(s) = a \cdot \sqrt{\frac{2s}{a}} = \sqrt{2s \cdot a}$.

D.h. $v(s)$ müsste eine Wurzelfunktion sein (und nicht eine lineare Funktion).
</hidden>
</WRAP>

==== Intervall umrechnen, ein für alle mal ====
<WRAP todo>
Wir suchen die lineare Funktion $f$, die das Intervall $[a,b]$ auf das Intervall $[c,d]$ abbildet, d.h. $f(a)=c$ und $f(b)=d$.

Gehen Sie dazu wie folgt vor:
  * Intervall verschieben: Bestimmen eine Verschiebung (d.h. eine Funktion $f_1(x)$, die eine Konstante addiert) so, dass $a$ zu Null wird, d.h. $f_1(a)=0$.
  * Intervall skalieren auf $[0,1]$: Bestimmen Sie eine Multiplikation (Funktion $(f_2(x)$) so, $f_2(f_1(b)) = 1$ (und damit natürlich weiterhin $f_2(f_1(a))=0$).
  * Intervall skalieren auf die Länge von $[c,d]$: Bestimmen Sie eine Multiplikation (Funktion $(f_3(x)$) so, $f_3(1) = $ Länge von $[c,d]$.
  * Intervall verschieben: Bestimmen eine Verschiebung (Funktion $f_4(x)$) so, dass $f_4(0)=c$.
  * Die gesuchte Funktion ist $f(x) = f_4(f_3(f_2(f_1(x))))$.

{{lehrkraefte:blc:informatik:glf20:robotik:2021-04-30-note-11-44.pdf|Notizen 2pG als pdf}}

{{lehrkraefte:blc:informatik:glf20:robotik:2021-05-03-note-09-00.pdf|Notizen 2aLM als pdf}}

Programmieren Sie diese Funktion nun in Python:
<code python>
# Bildet x mit lineare Funktion ab, so dass das Intervall [a,b] auf [c,d] abgebildet wird
def linear(a,b,c,d,x):
    # Obige Funktionen auf x anwenden
    # Resultat zurückgeben
    return x  
</code>
Diese Funktion kann dann im Programm verwendet werden, z.B.
<code python>
if gear.getLeftMotorCount()<zehncm:
    gear.setSpeed(int(linear(0,zehncm, 5, myspeed, gear.getLeftMotorCount())))
</code>

<hidden Lösungsvorschlag>
<code python>
def linear(a,b,c,d,x):
    return (x-a)/(b-a)*(d-c)+c  


einMeter = 100/0.048  # Die korrekte Anzahl Grad für 1 m  ausrechnen und eintragen!
anfahren = 0.4*einMeter  # Strecke bis wohin beschleunigt wird
bremsen = 0.6*einMeter   # Strecke ab der gebremst wird
gear.resetLeftMotorCount()   # Strecke auf 0 Grad setzen
mySpeed = 60   # Egal welche Geschwindigkeit

while gear.getLeftMotorCount()<einMeter:
    if gear.getLeftMotorCount()<anfahren:
        v = linear(0,anfahren, 5, mySpeed, gear.getLeftMotorCount()) # Formel mit gear.getLeftMotorCount()
        print(int(v))
        gear.setSpeed(int(v))
        gear.forward()

</code>
</hidden>
  * Bauen Sie die Funktion ''linear(a,b,c,d,x)'' in Ihr Programm ein, so dass der Roboter auf den ersten 40 cm sauber anfährt und auf den letzten 40 cm wieder bremst.
  * Programieren Sie eine Funktion ''fahrgut(distanz)'', die den Roboter die ''distanz'' geradeaus fahren lässt, wobei auf den ersten 10 cm sauber anfahren und auf den lezten 10 cm wieder sauber bremst.
<code python>
def fahrgut(distanz):
    global gear   # Sonst ist die Variable gear in der Funktion nicht zugänglich
    # Hier den Programmteil einfügen

# Benutzung der Funktion    
fahrgut(50)  # 0.5 m sauber fahren.
</code>

**Zusatz für Mathematik-Begeisterte**
  * Bestimmen Sie $v(s)$ für die gleichmässig beschleunigte Bewegung mit $v(t)=at+v_0$ ($v_0>0$ ist die Anfangsgeschwindigkeit) und $s(t)=\frac{1}{2}at^2+v_0t$, wobei $a$ so gewählt werden soll, dass $v(s_1)=v_1$, wobei $s_1$ die Strecke sein soll, auf der die Beschleunigung stattfindet und $v_1$ die Geschwindigkeit am Ende der Strecke.

<hidden Lösung>
$$ 
s(t)=\frac{1}{2}at^2+v_0t \quad \Leftrightarrow \quad t = \frac{-v_0 + \sqrt{v_0^2+2as}}{a}
$$
Die negative Lösung wird hier verworfen, weil wir nur an der positiven Zeit interessiert sind.
Eingesetzt in $v(t)$ erhält man
$$
v(s) = \sqrt{v_0^2+2as}
$$
Löst man $v(s_1)=v_1$ nach $a$ auf erhält man
$$
a = \frac{v_1^2-v_0^2}{2s_1}
$$
und damit die gesuchte Funktion
$$
v(s) = \sqrt{v_0^2 + \frac{v_1^2-v_0^2}{s_1} \cdot s}
$$

Diese kann nun in Python programmiert werden:
<code python>
# Anfangsgeschwindigkeit, Endgeschwindigkeit, Gesamtstrecke, aktuelle Strecke
def vs(v0, v1, s1, s):
  return (v0*v0 + (v1*v1-v0*v0)/s1*s)**0.5
</code>
</hidden>
</WRAP>