#!/usr/bin/env python

import sys, time
import carcomm 

if __name__ == "__main__":

    try:
        # voorlopig vast
        port = "/dev/ttyUSB0"
        baudrate = 28800
        parity = 'N'
        rtscts = False
        xonxoff = False
        echo = False
        convert_outgoing = carcomm.CONVERT_CRLF
        repr_mode = 0

        carcon = carcomm.CarControl(
            port,
            baudrate,
            parity,
            rtscts,
            xonxoff,
            echo,
            convert_outgoing,
            repr_mode,
        )
        time.sleep(2)  # de nexus robot is niet zo snel..
        carcon.sendpacket(20,0)
        carcon.getinfopacket()
	carcon.sendpacket(20,22)



	speed = 400
        for x in range(0, 50):
#            carcon.setspeed(0.5, 0)
             carcon.sendwheelscommand(speed, speed, speed, speed)
#            carcon.getinfopacket()
             time.sleep(0.05)
	carcon.sendwheelscommand(0, 0, 0, 0)
#       carcon.setspeed( 0, 0)

	time.sleep(1)  # de nexus robot is niet zo snel..

        for x in range(0, 50):
#            carcon.setspeed(0.5, 0)
             carcon.sendwheelscommand(-speed, -speed, -speed, -speed)
#            carcon.getinfopacket()
             time.sleep(0.05)
	carcon.sendwheelscommand(0, 0, 0, 0)
#       carcon.setspeed( 0, 0)



    except carcomm.serial.SerialException as e:
        sys.stderr.write("could not open port %r: %s\n" % (port, e))
        sys.exit(1)