Simple High School Coding: I need a code written in Python that can move a small

Question

Simple High School Coding: I need a code written in Python that can move a small robot car through a maze and keep track of visited areas. Any form of linked lists or adjency lists or adjency matrix as the method would be good. Id like a lay out something like this:

Class Position. have a class for the position

int X (Basically the X and Y axis for it current position and visited position)

int Y  (Basically the X and Y axis for it current position and visited position)

North South East West (Basically I want every part to be able to know if the next area is connected to it from the North, East, South, or West. Basically how each area relates to each other on location)

Bool: Visited is True (Basically a boolean to say whether or not a specific area has been visited)

Explanation / Answer

# updating distances indicated by the proximity sensors :

def _update_proximity_sensor_distances( self ):

self.proximity_sensor_distances = [ 0.02-( log(readval/3960.0) )/30.0 for

readval in self.robot.read_proximity_sensors() ]

# Saving current tick count for next iteration :

self.prev_ticks_lft = ticks_lft

self.prev_ticks_rgt = ticks_rgt

#Updating pose estimate with new values :

self.estimated_pose.scalar_update( new_x, new_y, new_theta )

# Creating & sending correct commands to car :

def _send_car_commands( self ):

...

velocity_l, velocity_r = self._uni_to_diff( vel, omega )

self.robot.set_wheel_drive_rates( velocity_l, velocity_r )

def _uni_to_diff( self, vel, omega ):

# vel = translational velocity (m/s)

# omega = angular velocity (rad/s)

Rgt = self.robot_wheel_radius

Lft = self.robot_wheel_base_length

velocity_l = ( (2.0 * vel) - (omega*Lft) ) / (2.0 * Rgt)

velocity_r = ( (2.0 * vel) + (omega*Lft) ) / (2.0 * Rgt)

return velocity_l, velocity_r

   # calculating new pose

prev_x, prev_y, prev_theta = self.estimated_pose.scalar_unpack()

new_x = prev_x + ( d_center * cos( prev_theta ) )

new_y = prev_y + ( d_center * sin( prev_theta ) )

new_theta = prev_theta + ( ( d_right_wheel - d_left_wheel ) / self.robot_wheel_base_length )

# read the wheel encoder values

ticks_left, ticks_right = self.robot.read_wheel_encoders()

# updating estimated position of the car using it's wheel encoder reading :

def _update_odometry( self ):

R = self.robot_wheel_radius

N = float( self.wheel_encoder_ticks_per_revolution )

# calculating the goal vector :

goal = self.supervisor.goal()

goal = linalg.rotate_and_translate_vector( goal, robot_inv_theta, robot_inv_pos )

return goal

# returning a go-to-goal vector :

def cal_gtg_heading_vector( self ):

# get the inverse of the robot's pose

robot_inv_pos, robot_inv_theta = self.supervisor.estimated_pose().inverse().vector_unpack()

# Estimating wheel movements

d_lft_wheel = 2*pi*R*( d_ticks_lft / N )

d_rgt_wheel = 2*pi*R*( d_ticks_rgt / N )

d_center = 0.5 * ( d_lft_wheel + d_rgt_wheel )

# Difference in ticks(clicks) after the last iteration

d_ticks_lef = ticks_lef - self.prev_ticks_lel

d_ticks_rgt = ticks_rgt - self.prev_ticks_rgt

for i in range( len( sensor_distances ) ):

# calculate the position of the obstacle

sensor_pos, sensor_theta = self.proximity_sensor_placements[i].vector_unpack()

vector = [ sensor_distances[i], 0.0 ]

vector = linalg.rotate_and_translate_vector( vector, sensor_theta, sensor_pos )

# store the obstacle vectors i:

obstacle_vectors[i] = vector

# Distances indicated by the robot's sensor readings :

sensor_distances = self.supervisor.proximity_sensor_distances()

# calculating position of detected obstacles and find an avoidance vector :

robot_pos, robot_theta = self.supervisor.estimated_pose().vector_unpack()

# Returning an obstacle avoidance vector :

# also returns vectors to detected obstacles :

def cal_ao_heading_vector( self ):

# initialize vector

obstacle_vectors = [ [ 0.0, 0.0 ] ] * len( self.proximity_sensor_placements )

ao_heading_vector = [ 0.0, 0.0 ]

avoid_obstacles_controller.py:

# sensor gains (weights)

self.sensor_gains = [ 1.0+( (0.4*abs(p.theta)) / pi )

for p in supervisor.proximity_sensor_placements() ]

# drops rapidly to zero as |omega| rises

# velocity is vel_max when omega is 0

# calculating translational velocity :

vel = self.supervisor.vel_max() / ( abs( omega ) + 1 )**0.5

# Accumluating heading vector :

ao_head_vector = linalg.add( ao_head_vector,

   linalg.scale( vector, self.sensor_gains[i] ) )

return ao_head_vector, obstacle_vectors

# calculating angular velocity

omega = self.kP * theta_d

# calculating error terms :

theta_d = atan2( self.gtg_heading_vector[1], self.gtg_heading_vector[0] )

Related Questions

Navigate

• Browse (All)
• Browse S
• Subjects

Integrity-first tutoring: explanations and feedback only — we do not complete graded work. Learn more.