def __init__(self, display, size, scale, time):
BaseObj.__init__(self, BaseObj.ROBOT)
self.rand = Random()
self.tick = 0
self.ir_angle = [0,0] #angle is in radians
self.target_ir_angle = [0,0] #angle is in radians
self.target_speed = [0,0]
self.wheel_speed = [0,0]
self.wheel_accel = [2,2]
self.wheel_drag = [0,0]
self.asr = True #automatic speed regulation
self.wheel_pos = [0,0]
self.encoder = [0.0,0.0]
self.prev_encoder = [0.0,0.0]
self.real_time = 0
self.prev_time = 0
self.last_speed_change = 0
self.x = 0
self.y = 0
self.size = size
#dist between wheel centres is 23.5cm, robot is 26cm wide
self.wheelbase = 22.5*self.size/26.0
self.scale = scale
self.noise_model = NO_NOISE
self.noise_value = 0
self.rand_noise = False
self.max_ir_range = [0 , 0]
self.max_ir_range[FRONT] = 100/26 * size
self.max_ir_range[SIDE] = 40/26 * size
self.max_us_range = 3 * size
self.ir_dist = [self.max_ir_range[FRONT], self.max_ir_range[FRONT],
self.max_ir_range[SIDE], self.max_ir_range[SIDE]]
self.ir_sample_time = time
self.ir_scan_speed = 20 * 3.14/180 #20 degrees per update
self.us_dist = self.max_us_range
self.last_ir_front_reading = 0
self.last_ir_side_reading = 0
self.last_us_reading = 0
self.always_show_front_ir = False
self.always_show_side_ir = False
self.always_show_us = False
self.async_enabled = False
self.async_update_interval = 100
self.async_last_update = 0
self.async_us = False
self.async_iflr = False
self.async_islr = False
self.async_melr = False
self.angle = 0 #angle is in radians
self.bump = [False,False]
self.bump_got = [False,False]
self.latch_bump = False
self.line_y = -0.4 * size
self.line_spacing = 0.05 * size
self.line_readings = [127,127,127]
self.last_line_reading = 0
self.vis = RobotVis(display, 100, 100, 30, self.size,
self.max_ir_range[FRONT], self.max_ir_range[SIDE])
self.reset_origin()
self.front_noise_model = FrontNoiseModel();
self.side_noise_model = SideNoiseModel();
class RobotModel(BaseObj):
def __init__(self, display, size, scale, time):
BaseObj.__init__(self, BaseObj.ROBOT)
self.rand = Random()
self.tick = 0
self.ir_angle = [0,0] #angle is in radians
self.target_ir_angle = [0,0] #angle is in radians
self.target_speed = [0,0]
self.wheel_speed = [0,0]
self.wheel_accel = [2,2]
self.wheel_drag = [0,0]
self.asr = True #automatic speed regulation
self.wheel_pos = [0,0]
self.encoder = [0.0,0.0]
self.prev_encoder = [0.0,0.0]
self.real_time = 0
self.prev_time = 0
self.last_speed_change = 0
self.x = 0
self.y = 0
self.size = size
#dist between wheel centres is 23.5cm, robot is 26cm wide
self.wheelbase = 22.5*self.size/26.0
self.scale = scale
self.noise_model = NO_NOISE
self.noise_value = 0
self.rand_noise = False
self.max_ir_range = [0 , 0]
self.max_ir_range[FRONT] = 100/26 * size
self.max_ir_range[SIDE] = 40/26 * size
self.max_us_range = 3 * size
self.ir_dist = [self.max_ir_range[FRONT], self.max_ir_range[FRONT],
self.max_ir_range[SIDE], self.max_ir_range[SIDE]]
self.ir_sample_time = time
self.ir_scan_speed = 20 * 3.14/180 #20 degrees per update
self.us_dist = self.max_us_range
self.last_ir_front_reading = 0
self.last_ir_side_reading = 0
self.last_us_reading = 0
self.always_show_front_ir = False
self.always_show_side_ir = False
self.always_show_us = False
self.async_enabled = False
self.async_update_interval = 100
self.async_last_update = 0
self.async_us = False
self.async_iflr = False
self.async_islr = False
self.async_melr = False
self.angle = 0 #angle is in radians
self.bump = [False,False]
self.bump_got = [False,False]
self.latch_bump = False
self.line_y = -0.4 * size
self.line_spacing = 0.05 * size
self.line_readings = [127,127,127]
self.last_line_reading = 0
self.vis = RobotVis(display, 100, 100, 30, self.size,
self.max_ir_range[FRONT], self.max_ir_range[SIDE])
self.reset_origin()
self.front_noise_model = FrontNoiseModel();
self.side_noise_model = SideNoiseModel();
def reset(self):
self.reset_motor_encoders()
self.clear_points()
self.clear_trail()
self.disable_async()
def set_controller(self, controller):
self.controller = controller
def set_noise_model(self, noise_model, noise_value):
self.noise_model = noise_model
self.noise_value = noise_value
def set_rand_noise(self, rand_noise):
self.rand_noise = rand_noise
def set_asr(self, val):
if val == 1:
self.asr = True
else:
self.asr = False
def reset_origin(self):
self.origin_x = self.x
self.origin_y = self.y
self.origin_angle = self.angle
def clear_points(self):
self.vis.clear_points()
def clear_trail(self):
self.vis.clear_trail()
def set_point(self, x, y):
x = x * 100.0/26.0
y = y * -100.0/26.0
d = sqrt(x*x + y*y)
a2 = atan2(y, x)
nx = self.origin_x + d * cos (self.origin_angle + a2)
ny = self.origin_y + d * sin (self.origin_angle + a2)
self.vis.set_point(nx, ny)
def set_trail(self):
self.vis.set_trail(self.x, self.y)
def set_display_text(self, line, text):
self.vis.set_display_text(line, text)
def redraw(self):
self.vis.draw()
return
def show_front_ir(self, value):
self.always_show_front_ir = value
def show_side_ir(self, value):
self.always_show_side_ir = value
def show_us(self, value):
self.always_show_us = value
def set_target_ir_angle(self, side, angle):
self.target_ir_angle[side] = angle * (pi/180)
def set_ir_angle(self, side, angle):
self.ir_angle[side] = angle * (pi/180)
self.target_ir_angle[side] = self.ir_angle[side] #manual control overrides target
self.vis.set_ir_angle(self.ir_angle[L], self.ir_angle[R])
def set_ir_angles(self, ir_angle_left, ir_angle_right):
self.set_ir_angle(L, ir_angle_left)
self.set_ir_angle(R, ir_angle_left)
def set_ir_dist(self, sensor, side, dist, time):
self.ir_sample_time = time
if self.noise_model == SAMPLED_NOISE:
if sensor == FRONT:
#print "scale: ", self.scale, " dist: ", dist
randdist = self.front_noise_model.add_noise(dist * self.scale) / self.scale
if sensor == SIDE:
randdist = self.side_noise_model.add_noise(dist * self.scale) / self.scale
elif self.noise_model == GAUSSIAN_NOISE:
randdist = self.rand.gauss(dist, self.noise_value*sqrt(dist)/10)
elif self.noise_model == ANGULAR_NOISE:
if sensor == FRONT:
sense_value = self.dist_to_gp2d12(dist)
sq = self.noise_value*sqrt(sense_value)/10
randvalue = self.rand.gauss(sense_value + sq, sq)
randdist = self.gp2d12_to_dist(randvalue)
elif sensor == SIDE:
sense_value = self.dist_to_gp2d120(dist)
sq = sqrt(sense_value)/2
randvalue = self.rand.gauss(sense_value + sq, sq)
randdist = self.gp2d120_to_dist(randvalue)
else:
randdist = dist
sensornum = self.sensor_num(sensor,side)
if self.rand_noise:
#randomly report an under-read every 10 seconds on average
r = self.rand.randint(0,300)
if r == 1:
randdist /= 2
self.ir_dist[sensornum] = randdist
self.vis.set_sensor_range(sensornum, int(0.26*randdist))
return randdist
def get_ir_front_dist(self, side):
return self.dist_to_gp2d12(self.ir_dist[self.sensor_num(FRONT,side)] * self.scale)
# call read_ir_front_dist when the user actively reads, otherwise
# call get_ir_front_dist to obtain the value
def read_ir_front_dist(self, side):
self.last_ir_front_reading = self.tick
return self.dist_to_gp2d12(self.ir_dist[self.sensor_num(FRONT,side)] * self.scale)
def get_ir_sample_time(self):
return self.ir_sample_time
def dist_to_gp2d12(self, dist):
#dist is in cm
if dist < 10:
value = 678 * dist/10.0
else:
value = 6787.0/(dist+4) + 3
return value
def gp2d12_to_dist(self, ir):
dist = (6787 / (ir - 3)) - 4
return dist
def dist_to_gp2d120(self, dist):
#dist is in cm
if dist < 3:
volts = 3 * dist/3
else:
volts = 13 / (dist + 0.42)
value = 1024 * volts / 5.0
return value
def gp2d120_to_dist(self, ir):
dist = (2914 / (ir + 5)) - 1;
return dist;
def get_ir_side_dist(self, side):
return self.dist_to_gp2d120(self.ir_dist[self.sensor_num(SIDE,side)] * self.scale)
# call read_ir_side_dist when the user actively reads, otherwise
# call get_ir_side_dist to obtain the value
def read_ir_side_dist(self, side):
self.last_ir_side_reading = self.tick
return self.dist_to_gp2d120(self.ir_dist[self.sensor_num(SIDE,side)] * self.scale)
#for calculating positions of sensor readings, we need to know
#where the sensor beam actually is.
def get_ir_position(self, sensor, side):
if sensor == FRONT:
#first assume the robot and sensor are both pointing straight up
if side == L:
x1 = self.x + (12.5*self.size)/40
cx = self.x + (12.5*self.size)/40
ir_angle = (pi/4) - self.ir_angle[side]
elif side == R:
x1 = self.x - (12.5*self.size)/40
cx = self.x - (12.5*self.size)/40
ir_angle = (-pi/4) - self.ir_angle[side]
y1 = self.y - (16*self.size)/40
x2 = x1
y2 = y1 - self.max_ir_range[FRONT]
cy = self.y - (15*self.size)/40
#now rotate the beam around the sensor to account for
#sensor angle, then rotate the beam around the robot
#center to account for robot angle.
x1,y1 = self.rotate(x1,y1,cx, cy, ir_angle)
x1,y1 = self.rotate(x1,y1,self.x, self.y, self.angle)
x2,y2 = self.rotate(x2,y2,cx, cy, ir_angle)
x2,y2 = self.rotate(x2,y2,self.x, self.y, self.angle)
return x1,y1,x2,y2,self.max_ir_range[FRONT]
elif sensor == SIDE:
if side == L:
x1 = self.x + (16*self.size)/40
x2 = x1 + self.max_ir_range[SIDE]
elif side == R:
x1 = self.x - (16*self.size)/40
x2 = x1 - self.max_ir_range[SIDE]
y1 = self.y + (13*self.size)/40
y2 = y1
x1,y1 = self.rotate(x1,y1,self.x, self.y, self.angle)
x2,y2 = self.rotate(x2,y2,self.x, self.y, self.angle)
#x1,y1 are the robot end of the sensor beam
#x2,y2 are the far end of the sensor beam at maximum range
return x1,y1,x2,y2,self.max_ir_range[SIDE]
def get_us_triangle(self):
x1 = self.x
y1 = self.y - (12*self.size)/40
x2 = self.x + self.size * 1.45
y2 = self.y - self.size * 4
x3 = self.x - self.size * 1.45
y3 = self.y - self.size * 4
x1,y1 = self.rotate(x1,y1,self.x, self.y, self.angle)
x2,y2 = self.rotate(x2,y2,self.x, self.y, self.angle)
x3,y3 = self.rotate(x3,y3,self.x, self.y, self.angle)
return x1,y1,x2,y2,x3,y3,2*self.max_ir_range[FRONT]
def get_angle_from_us(self, x, y):
#returns the angle of a point from the position of the US
#sensor relative to the direction of the robot
#calculate the current location of the US sensor
us_x = self.x
us_y = self.y - (12*self.size)/40
us_x,us_y = self.rotate(us_x,us_y,self.x, self.y, self.angle)
#calculate location of point relative to US sensor
his_x = x - us_x
his_y = y - us_y
#rotate the world so robot faces up
his_x,his_y = self.rotate_about_origin(his_x, his_y, -self.angle)
angle = atan2(his_x,-his_y)
return angle
def set_us_dist(self, dist):
if dist > 1000:
print(dist)
self.us_dist = dist
def get_us_dist(self):
self.last_us_reading = self.tick
return self.us_dist * self.scale
# set_bump_latch enables latching bump readings. Sometimes a
# bounce might cause a reading of a bump switch to be missed.
# Setting latch means a bump reading will be maintained until the
# next reading of that sensor.
def set_bump_latch(self, mode):
self.latch_bump = mode
def set_bump(self, left_bumped, right_bumped):
if left_bumped:
self.bump[L] = True
self.bump_got[L] = False
else:
self.bump[L] = False
if right_bumped:
self.bump[R] = True
self.bump_got[R] = False
else:
self.bump[R] = False
def get_bump(self, side):
self.bump_got[side] = True
if self.bump[side]:
return "1"
else:
return "0"
def get_perimeter(self):
#df is half the width of the front straight side
df = 0.3*self.size
#dr is half the width of the robot
dr = 0.5*self.size
x1,y1 = self.rotate(self.x + dr, self.y - df,
self.x, self.y, self.angle)
x2,y2 = self.rotate(self.x + df, self.y - dr,
self.x, self.y, self.angle)
x3,y3 = self.rotate(self.x - df, self.y - dr,
self.x, self.y, self.angle)
x4,y4 = self.rotate(self.x - dr, self.y - df,
self.x, self.y, self.angle)
x5,y5 = self.rotate(self.x - dr, self.y + df,
self.x, self.y, self.angle)
x6,y6 = self.rotate(self.x - df, self.y + dr,
self.x, self.y, self.angle)
x7,y7 = self.rotate(self.x + df, self.y + dr,
self.x, self.y, self.angle)
x8,y8 = self.rotate(self.x + dr, self.y + df,
self.x, self.y, self.angle)
return x1,y1,x2,y2,x3,y3,x4,y4,x5,y5,x6,y6,x7,y7,x8,y8
def get_linesensor_positions(self):
xm = self.x
ym = self.y + self.line_y
xl = xm + self.line_spacing
yl = ym
xr = xm - self.line_spacing
yr = ym
xl,yl = self.rotate(xl, yl, self.x, self.y, self.angle)
xr,yr = self.rotate(xr, yr, self.x, self.y, self.angle)
xm,ym = self.rotate(xm, ym, self.x, self.y, self.angle)
return [xl,xr,xm],[yl,yr,ym]
def set_linesensor_readings(self, values):
self.line_readings = values
def get_linesensor_readings(self):
self.last_line_reading = self.tick
return self.line_readings
def init_robot_posn(self, x, y, angle):
self.angle = angle * (pi / 180)
self.start_angle = self.angle
self.x = x
self.start_x = x
self.y = y
self.start_y = y
self.vis.set_posn(x, y, angle);
def reset_posn(self):
self.angle = self.start_angle
self.x = self.start_x
self.y = self.start_y
self.target_speed = [0,0]
self.wheel_speed = [0,0]
self.vis.set_posn(self.x, self.y, self.angle);
self.clear_trail()
self.clear_points()
def set_speed(self, side, target_speed):
self.target_speed[side] = target_speed
self.last_speed_change = self.real_time
def fwd(self, target_speed):
self.set_speed(L, target_speed)
self.set_speed(R, target_speed)
def stop(self):
self.fwd(0)
def crash(self, corner):
if corner == -1:
self.set_bump(False, False)
return
self.un_update_position()
#should handle collision better than this!
if corner == L:
#front left corner
self.set_bump(True, False)
if self.target_speed[L] > 0 and self.wheel_speed[L] > 0:
#bounce
self.wheel_speed[L] = -1 * self.wheel_accel[L]
else:
self.wheel_speed[L] = 0
elif corner == R:
#front right corner
self.set_bump(False, True)
if self.target_speed[R] > 0 and self.wheel_speed[R] > 0:
self.wheel_speed[R] = -0.5 * (self.wheel_speed[R] + self.wheel_accel[R])
else:
self.wheel_speed[R] = 0
else:
#collision is at rear
for side in L,R:
self.wheel_speed[side] = -self.wheel_speed[side]
self.target_speed[side] = 0
def turn(self, target_speed):
self.set_speed(L, target_speed)
self.set_speed(R, 0-target_speed)
def update_wheelspeed(self):
#really simple model for now
for side in L,R:
effective_target_speed = self.target_speed[side]
if self.asr == False:
if effective_target_speed> 0:
effective_target_speed -= self.wheel_drag[side]
if effective_target_speed < 0:
effective_target_speed = 0
else:
effective_target_speed += self.wheel_drag[side]
if effective_target_speed > 0:
effective_target_speed = 0
if self.wheel_speed[side] < effective_target_speed:
self.wheel_speed[side] += self.wheel_accel[side]
#did we overshoot?
if self.wheel_speed[side] > effective_target_speed:
self.wheel_speed[side] = effective_target_speed
elif (self.wheel_speed[side] > effective_target_speed):
self.wheel_speed[side] -= self.wheel_accel[side]
#did we overshoot?
if self.wheel_speed[side] < effective_target_speed:
self.wheel_speed[side] = effective_target_speed
def update_servos(self):
#really simple model for now
changed = False
for side in L,R:
if (self.target_ir_angle[side] < self.ir_angle[side]):
changed = True
self.ir_angle[side] -= self.ir_scan_speed
if self.target_ir_angle[side] > self.ir_angle[side]:
#don't overshoot
self.ir_angle[side] = self.target_ir_angle[side]
elif self.target_ir_angle[side] > self.ir_angle[side]:
changed = True
self.ir_angle[side] += self.ir_scan_speed
if self.target_ir_angle[side] < self.ir_angle[side]:
#don't overshoot
self.ir_angle[side] = self.target_ir_angle[side]
if changed:
self.vis.set_ir_angle(self.ir_angle[L], self.ir_angle[R])
def un_update_position(self):
self.x = self.prev_x
self.y = self.prev_y
self.angle = self.prev_angle
def update_position(self):
self.update_wheelspeed()
self.prev_x = self.x
self.prev_y = self.y
self.prev_angle = self.angle
speed_gain = 55
#dist moved by left wheel
dl = self.wheel_speed[L] / speed_gain
self.encoder[L] += dl
#dist moved by right wheel
dr = self.wheel_speed[R] / speed_gain
self.encoder[R] += dr
angle_change = (dl - dr) / self.wheelbase
if (dl == dr):
#special case to avoid divide by zero in general case
#dx = 0
#dy = dl
dh = dl
else:
r = dl/angle_change
dh = (2*r - self.wheelbase) * sin(angle_change/2)
#dx = dh * sin(angle_change)
#dy = dh * cos(angle_change)
deltax = dh * sin(self.angle + angle_change/2)
deltay = 0 - dh * cos(self.angle + angle_change/2)
self.angle = self.angle + angle_change
self.x = self.x + deltax
self.y = self.y + deltay
self.vis.set_posn(self.x, self.y, self.angle);
#update wheel positions (used for drag calculations)
radius = 47 #because robot is 100 units wide
w_x = radius * cos(self.angle)
w_y = radius * sin(self.angle)
self.wheel_pos[L] = (self.x - w_x, self.y - w_y)
self.wheel_pos[R] = (self.x + w_x, self.y + w_y)
return self.x,self.y
def get_wheel_pos(self):
return self.wheel_pos[L], self.wheel_pos[R]
def set_drag(self, left_drag, right_drag):
self.wheel_drag[L] = left_drag
self.wheel_drag[R] = right_drag
def get_drag(self):
return self.wheel_drag[L], self.wheel_drag[R]
def get_encoder(self, side):
v = self.encoder[side]*220.0*1.14/80.0
self.prev_encoder[side] = v
return self.encoder[side]*220.0*1.14/80.0
def reset_motor_encoders(self):
self.encoder[L] = 0.0
self.encoder[R] = 0.0
def sensor_num(self, sensor, side):
return sensor*2 + side
def set_blob(self,num,x,y):
self.vis.set_blob(num,x,y)
def enable_async(self, sensor):
self.async_enabled = True
if sensor == "US":
self.async_us = True
elif sensor == "IFLR":
self.async_iflr = True
elif sensor == "ISLR":
self.async_islr = True
elif sensor == "MELR":
self.async_melr = True
else:
self.async_enabled = False
return False
return True
def disable_async(self):
self.async_enabled = False
def set_async_update_interval(self, update_interval):
self.async_update_interval = update_interval/1000.0
def self_update(self, new_time, real_time):
self.real_time = real_time
self.tick += 1
#if it's two seconds since the last motor command, shut down
if self.real_time - self.last_speed_change > 2:
self.target_speed = [0,0]
self.last_speed_change = self.real_time
self.update_servos()
x_offset, y_offset = self.update_position()
self.prev_time = new_time
ir_front_visible = True
if (self.tick - self.last_ir_front_reading) > 10 and self.always_show_front_ir == False:
ir_front_visible = False
ir_side_visible = True
if (self.tick - self.last_ir_side_reading) > 10 and self.always_show_side_ir == False:
ir_side_visible = False
us_visible = True
if (self.tick - self.last_us_reading) > 10 and self.always_show_us == False:
us_visible = False
line_visible = True
if (self.tick - self.last_line_reading) > 10:
line_visible = False
self.vis.set_sensor_visibility(ir_front_visible, ir_side_visible,
us_visible, line_visible)
#self.vis.set_ir_dists(self.ir_dist)
#self.vis.set_us_dist(self.us_dist*self.scale)
if self.async_enabled and \
(real_time - self.async_last_update) \
>= self.async_update_interval:
self.async_last_update = real_time
s = "S " + str(self.get_bump(L)) + " " + str(self.get_bump(R))
if self.async_us:
s = s + " " + str(int(self.get_us_dist()))
if self.async_iflr:
s = s + " " + str(int(self.read_ir_front_dist(L))) + " " + \
str(int(self.read_ir_front_dist(R)))
if self.async_islr:
s = s + " " + str(int(self.read_ir_side_dist(L))) + " " + \
str(int(self.read_ir_side_dist(R)))
if self.async_melr:
s = s + " " + str(int(self.get_encoder(L))) + " " + \
str(int(self.get_encoder(R)))
s = s + "\n"
self.controller.async_report(s)
return x_offset, y_offset
