def update(self):
clock = Clock()
while True:
t, dt = clock.getTandDT()
self.theta, self.omega = [-x for x in self.__gyro.rate_and_angle]
self.x, self.y, self.dx, self.dy = self.__localization.getData(
radians(self.theta), radians(self.__motor_rear.speed), dt)
def velocity_move(self, vel_linear, vel_angular, time):
# initialization for current mode
self.__velocity_controller.setTargetVelocities(vel_linear, vel_angular)
clock = Clock()
fh = open("vel.txt", "w")
while clock.getCurrentTime() <= time:
try:
t, dt = clock.getTandDT()
theta, omega = [-x for x in self.__sensor_gyro.rate_and_angle
] # returns ANGLE AND RATE
x, y, dx, dy, v_r = self.__localization.getData(
radians(theta), radians(self.__motor_rear.speed), dt)
self.__robot_state = [x, y, dx, dy, theta,
omega] # update state
u_v, u_phi = self.__velocity_controller.getControls(
radians(self.__motor_rear.speed),
radians(self.__motor_steer.position), radians(omega), dt)
fh.write("%f %f %f \n" % (t, v_r, radians(omega)))
self.__motor_rear.run_direct(duty_cycle_sp=u_v)
self.__motor_steer.run_direct(duty_cycle_sp=u_phi)
if self.__button_halt.enter:
break
except KeyboardInterrupt:
break
# off motors
fh.close()
self.__motor_rear.duty_cycle_sp = 0
self.__motor_steer.duty_cycle_sp = 0
def move(self,
vel_linear=None,
vel_angular=None,
trajectory=None,
mapping=None):
"""
Examples:
mode_1. move(trajectory=given_trajectory) --- following given trajectory;
mode_2. move(vel_linear=0.3, vel_angular=0.3) --- moving with given velocities;
mode_3. move(`mode_1 or mode_2`, mapping=mapping) --- moving and mapping.
"""
# initialization for current mode
if vel_linear and vel_angular not in [None]:
self.__velocity_controller.setTargetVelocities(
vel_linear, vel_angular)
clock = Clock()
while True:
try:
t, dt = clock.getTandDT()
theta, omega = [-x for x in self.__sensor_gyro.rate_and_angle
] # !!! returns ANGLE AND RATE :)
x, y, dx, dy = self.__localization.getData(
radians(theta), radians(self.__motor_rear.speed), dt)
self.__robot_state = [x, y, dx, dy, theta,
omega] # update state
if trajectory is not None:
point = trajectory.getCoordinates(t)
v_des, omega_des = self.__trajectory_controller.getControls(
point, x, y, dx, dy, radians(theta), dt)
self.__velocity_controller.setTargetVelocities(
v_des, omega_des)
u_v, u_phi = self.__velocity_controller.getControls(
radians(self.__motor_rear.speed),
radians(self.__motor_steer.position), radians(omega), dt)
if mapping is not None:
mapping.updateMap(x, y, radians(theta))
self.__motor_rear.run_direct(duty_cycle_sp=u_v)
self.__motor_steer.run_direct(duty_cycle_sp=u_phi)
if self.__button_halt.enter:
break
except KeyboardInterrupt:
break
# off motors
self.__motor_rear.duty_cycle_sp = 0
self.__motor_steer.duty_cycle_sp = 0
raise SystemExit
def turnWheel(self, phi_desired):
""" Turns front wheels on the desired angle 'phi_desired' """
pid_phi = PID(100.0, 500.0, 5.0, 100, -100)
t = 0
clock = Clock()
while t < 1: # FIXME: seems that need steady state condition
t, dt = clock.getTandDT()
phi_current = self.__motor_steer.position
error_phi = radians(phi_desired - phi_current)
u_phi = pid_phi.getControl(error_phi, dt)
self.__motor_steer.run_direct(duty_cycle_sp=u_phi)
Sound.speak('Wheels were turned!')
def trajectory_move(self, trajectory):
clock = Clock()
fh = open("test.txt", "w")
while not trajectory.is_end:
try:
t, dt = clock.getTandDT()
theta, omega = [-x for x in self.__sensor_gyro.rate_and_angle
] # !!! returns ANGLE AND RATE :)
x, y, dx, dy, v_r, beta = self.__localization.getData(
radians(theta), radians(self.__motor_rear.speed), dt)
self.__robot_state = [x, y, dx, dy, theta,
omega] # update state
point = trajectory.getCoordinatesTime(t)
v_des, omega_des = self.__trajectory_controller.getControls(
point, x, y, dx, dy, beta, dt)
self.__velocity_controller.setTargetVelocities(
v_des, omega_des)
u_v, u_phi = self.__velocity_controller.getControls(
radians(self.__motor_rear.speed),
radians(self.__motor_steer.position), radians(omega), dt)
fh.write(
"%f %f %f %f %f %f %f %f %f %f %f\n" %
(t, x, y, point.x, point.y, v_r, v_des, theta,
radians(omega), omega_des, self.__motor_steer.position))
self.__motor_rear.run_direct(duty_cycle_sp=u_v)
self.__motor_steer.run_direct(duty_cycle_sp=u_phi)
#print(trajectory.is_end)
if self.__button_halt.enter:
break
except KeyboardInterrupt:
break
# off motors
fh.close()
self.__motor_rear.duty_cycle_sp = 0
self.__motor_steer.duty_cycle_sp = 0
def path_move(self, trajectory, v_des=0.2):
fh = open("test.txt", "w")
clock = Clock()
while not trajectory.is_end:
try:
t, dt = clock.getTandDT()
theta, omega = [-x for x in self.__sensor_gyro.rate_and_angle
] # !!! returns ANGLE AND RATE :)
x, y, dx, dy, v_r = self.__localization.getData(
radians(theta), radians(self.__motor_rear.speed), dt)
self.__robot_state = [x, y, dx, dy, theta,
omega] # update state
x_ref, y_ref, kappa, t_hat = trajectory.getCoordinatesDistance(
x, y)
omega_des = self.__path_controller.getControls(
v_r, x, y, x_ref, y_ref, radians(theta), kappa, t_hat)
self.__velocity_controller.setTargetVelocities(
v_des, omega_des)
u_v, u_phi = self.__velocity_controller.getControls(
radians(self.__motor_rear.speed),
radians(self.__motor_steer.position), radians(omega), dt)
fh.write("%f %f %f %f %f\n" % (t, x, y, x_ref, y_ref))
self.__motor_rear.run_direct(duty_cycle_sp=u_v)
self.__motor_steer.run_direct(duty_cycle_sp=u_phi)
if self.__button_halt.enter:
break
except KeyboardInterrupt:
break
# off motors
self.__motor_rear.duty_cycle_sp = 0
self.__motor_steer.duty_cycle_sp = 0