Hello!

Thanks a lot for the previous postings on turtle graphics. They helped me produce an ad-hoc visualization for homework 5.1 quite fast.

As the homework should already be closed by now, I'm posting the complete program, adopted from unit 5-14.

Look for comments like #turtle graphics to see where I changed it (all in the lower section).

It's very simple, but I hope you like it anyway :-)

Kind regards, Joerg

(P.S.: Grmblfx... even here... suffering from indentation issues!!!
Now, visibly, the controller loop appears to be indented too much, but this is required
for the program to work after copy-pasting it from the forum back into an editor out here.)

# -----------
# User Instructions
#
# Implement a P controller by running 100 iterations
# of robot motion. The steering angle should be set
# by the parameter tau so that:
#
# steering = -tau_p * CTE - tau_d * diff_CTE - tau_i * int_CTE
#
# where the integrated crosstrack error (int_CTE) is
# the sum of all the previous crosstrack errors.
# This term works to cancel out steering drift.
#
# Your code should print a list that looks just like
# the list shown in the video.
#
# Only modify code at the bottom!
# ------------

from math import *
import random

# ------------------------------------------------
# 
# this is the robot class
#

class robot:

    # --------
    # init: 
    #    creates robot and initializes location/orientation to 0, 0, 0
    #

    def __init__(self, length = 20.0):
        self.x = 0.0
        self.y = 0.0
        self.orientation = 0.0
        self.length = length
        self.steering_noise = 0.0
        self.distance_noise = 0.0
        self.steering_drift = 0.0

    # --------
    # set: 
    #   sets a robot coordinate
    #

    def set(self, new_x, new_y, new_orientation):

        self.x = float(new_x)
        self.y = float(new_y)
        self.orientation = float(new_orientation) % (2.0 * pi)

    # --------
    # set_noise: 
    #   sets the noise parameters
    #

    def set_noise(self, new_s_noise, new_d_noise):
        # makes it possible to change the noise parameters
        # this is often useful in particle filters
        self.steering_noise = float(new_s_noise)
        self.distance_noise = float(new_d_noise)

    # --------
    # set_steering_drift: 
    #   sets the systematical steering drift parameter
    #

    def set_steering_drift(self, drift):
        self.steering_drift = drift

    # --------
    # move: 
    #    steering = front wheel steering angle, limited by max_steering_angle
    #    distance = total distance driven, most be non-negative

    def move(self, steering, distance, 
             tolerance = 0.001, max_steering_angle = pi / 4.0):

        if steering > max_steering_angle:
            steering = max_steering_angle
        if steering < -max_steering_angle:
            steering = -max_steering_angle
        if distance < 0.0:
            distance = 0.0

        # make a new copy
        res = robot()
        res.length         = self.length
        res.steering_noise = self.steering_noise
        res.distance_noise = self.distance_noise
        res.steering_drift = self.steering_drift

        # apply noise
        steering2 = random.gauss(steering, self.steering_noise)
        distance2 = random.gauss(distance, self.distance_noise)

        # apply steering drift
        steering2 += self.steering_drift

        # Execute motion
        turn = tan(steering2) * distance2 / res.length

        if abs(turn) < tolerance:

            # approximate by straight line motion

            res.x = self.x + (distance2 * cos(self.orientation))
            res.y = self.y + (distance2 * sin(self.orientation))
            res.orientation = (self.orientation + turn) % (2.0 * pi)

        else:

            # approximate bicycle model for motion

            radius = distance2 / turn
            cx = self.x - (sin(self.orientation) * radius)
            cy = self.y + (cos(self.orientation) * radius)
            res.orientation = (self.orientation + turn) % (2.0 * pi)
            res.x = cx + (sin(res.orientation) * radius)
            res.y = cy - (cos(res.orientation) * radius)

        return res

    def __repr__(self):
        return '[x=%.5f y=%.5f orient=%.5f]'  % (self.x, self.y, self.orientation)

############## ADD / MODIFY CODE BELOW ####################

# ------------------------------------------------------------------------
#
# run - does a single control run.

#turtle graphics
from turtle import *

def run(param1, param2, param3):
    myrobot = robot()
    myrobot.set(0.0, 1.0, 0.0)
    speed = 1.0 # motion distance is equal to speed (we assume time = 1)
    N = 300
    myrobot.set_steering_drift(10.0 / 180.0 * pi) # 10 degree bias, this will be added in by the move function, you do not need to add it below!
    #
    # Enter code here
    #

    int_crosstrack_error = 0.0
    crosstrack_error = myrobot.y

    penup() #turtle graphics
    setx(myrobot.x)
    sety(myrobot.y*100)
    setheading(myrobot.orientation * 180/pi)
    pendown()

    for i in range(N):
                diff_crosstrack_error = myrobot.y - crosstrack_error
                crosstrack_error = myrobot.y
                int_crosstrack_error += crosstrack_error
                steer = - param1 * crosstrack_error \
                        - param2 * diff_crosstrack_error \
                        - param3 * int_crosstrack_error
                myrobot = myrobot.move(steer, speed)

                print myrobot, steer
                setx(myrobot.x) #turtle graphics
                sety(myrobot.y*100) #turtle graphics
                setheading(steer * 180.0/pi)

# Call your function with parameters of (0.2, 3.0, and 0.004)

#Please note that the 10 degrees misalignment of the steering
#wheels will result in a constant misorientation of steer
#for compensation purposes.

#turtle graphics: draw a coordinate system
#all y-positions will be scaled by 100
penup()
color('black')
setx(-10)
sety(0)
pendown()
setx(400)
penup()
setx(0)
sety(-1.1*100)
pendown()
sety(2.1*100)
for i in range(-1,3):
    penup()
    sety(i*100)
    setx(-10)
    pendown()
    setx(10)

#now for homework 5.1:

#assumption: no problem
color('blue')
run(0.2, 3.0, 0.004)

#assumption: tau1 missing (position)
color('red')
run(0.0, 3.0, 0.004)

#assumption: tau2 missing (differential)
color('orange')
run(0.2, 0.0, 0.004)

#assumption: tau3 missing (integral)
color('brown')
run(0.2, 3.0, 0.0)

# wait for keypress
raw_input('Press Enter...')