remoteControl.py

#!/usr/bin/env python
# -*- coding: utf-8 -*-
 
# notwendige Module einbinden
from time import sleep
import packedCom
# zum Tasten auslesen
import sys
import termios
import tty
import select
 
 
class remoteControlMotor(packedCom.packedCom):
    def __init__(self):
        """ Initialisieren der Klasse. """
 
        
        self.left  = 0
        self.right = 0
 
        
        packedCom.packedCom.__init__(self, 0xa6, "bb")
        # packed descriptor is 0xa6
        # left  is of type int8_t
        # right is of type int8_t
 
    def getAsString(self):
        """ Konvertiert die aktuellen Daten in einen String """
 
        # schreibe alle Daten in eine Liste
        # (Reihenfolge wie bei der Initialisierung)
        data = [ self.left, self.right ]
        # rufe die getAsString-Funktion der Basisklasse
        return super(remoteControlMotor, self).getAsString(data)
 
class remoteControlTwist(packedCom.packedCom):
    def __init__(self):
        """ Initialisieren der Klasse. """
 
        
        self.linear  = 0
        self.angular = 0
 
        
        packedCom.packedCom.__init__(self, 0xa5, "hh")
        # packed descriptor is 0xa5
        # left  is of type int16_t
        # right is of type int16_t
 
    def getAsString(self):
        """ Konvertiert die aktuellen Daten in einen String """
 
        # schreibe alle Daten in eine Liste
        # (Reihenfolge wie bei der Initialisierung)
        data = [ self.linear, self.angular ]
        # rufe die getAsString-Funktion der Basisklasse
        return super(remoteControlTwist, self).getAsString(data)
 
 
"""
Programm zum Testen der Fernsteuer-Funktionen.
 
Zugehöriges Mikrocontroller Programm ist examples/remoteControl.c
Wird dieses Programm auf dem Roboter ausgeführt, kann über das Python
Skript eine Kommunikation etabliert werden.
 
Dieses Python Skript benötigt das vereinfachte packedCom-Modul unter
  tools/python/packedCom.py
 
Das Programm kann mit Strg-C abgebrochen werden.
"""
 
# Comport öffnen
#   Standarddevice ist Kabelgebundene USB Verbindung zum Roboter
#   (/dev/tucbot/all_usb)
#
#   Zum öffnen der XBee-Verbindung muss dieses Skript mit dem entsprechenden
#   Parameter geöffnet werden, für den Roboter Pete wäre dies zum Beispiel:
#     $ python customData.py -r pete
comport = packedCom.comPort()
 
# Initialisierung um Tastaturbefehle abzufangen
#   Alte Einstellungen sichern
stdin_oldattr = termios.tcgetattr(sys.stdin)
#   Neue Einstellungen setzen
tty.setcbreak(sys.stdin)
 
# Steuerung per Nummernblock oder W-A-S-D
#   w,8  : gerade aus fahren
#   s,2  : zurück     fahren
#
#   a,4  : nach links  drehen
#   d,6  : nach rechts drehen
#
#   q,7  : leicht nach links  korrigieren
#   e,9  : leicht nach rechts korrigieren
#
#   " ",5: stehen bleiben
#   sonst: stehen bleiben
#
# Mit Strg-C abbrechen!
 
# Instantierung
twist = remoteControlTwist()
motor = remoteControlMotor()
 
try:
    while 1:
        # Kurz warten
        sleep(0.1)
 
        # Taste auslesen
        if (sys.stdin in select.select([sys.stdin], [], [], 0)[0]):
            # ein Zeichen einlesen
            c = sys.stdin.read(1)
        else:
            # keine Zeichen vorhanden --> Schleife von vorne beginnen
            continue
 
        # Esc-Taste
        if (c == chr(27)):
            break
 
        # Steuerung
        if (c in ['w',  '8']):
            twist.linear  = 200
            twist.angular =   0
            motor.right = 50
            motor.left  = 50
        elif (c in ['s',  '2']):
            twist.linear  = -100
            twist.angular =    0
            motor.right = -30
            motor.left  = -30
 
        elif (c in ['a',  '4']):
            twist.linear  =   0
            twist.angular =  30
            motor.right =  30
            motor.left  = -30
        elif (c in ['d',  '6']):
            twist.linear  =   0
            twist.angular = -30
            motor.right = -30
            motor.left  =  30
 
        elif (c in ['q',  '7']):
            twist.angular+= 5
            motor.right-= 5
            motor.left += 5
        elif (c in ['e',  '9']):
            twist.angular-= 5
            motor.right+= 5
            motor.left -= 5
 
        elif (c in [' ',  '5']):
            twist.linear  = 0
            twist.angular = 0
            motor.right = 0
            motor.left  = 0
        else:
            print "ungültiges Zeichen"
            twist.linear  = 0
            twist.angular = 0
            motor.right = 0
            motor.left  = 0
 
        comport.write(twist.getAsString())
        comport.write(motor.getAsString())
        print "motor(", motor.left, ", ", motor.right, ");", \
           "\t twist(", twist.linear, ", ", twist.angular, ");"
 
finally:
    # Alte Einstellungen der Tastatur wiederherstellen
    termios.tcsetattr(sys.stdin, termios.TCSANOW, stdin_oldattr)