class control_velocity(object): def __init__(self): self.linear_pid = PID(95, 85, 1, 62.915) self.angular_pid = PID(6, 4, 0.1, 3.125) self.linear_pid.setpoint = 0 self.angular_pid.setpoint = 0 self.left_pub = rospy.Publisher('/wheel_left/duty', Int16, queue_size=1) self.right_pub = rospy.Publisher('/wheel_right/duty', Int16, queue_size=1) rospy.Subscriber("/cmd_vel", Twist, self.update_setpoints) rospy.Subscriber("/wheel_odom", Odometry, self.update_duty) def update_setpoints(self, cmd_vel): self.linear_pid.setpoint = cmd_vel.linear.x self.angular_pid.setpoint = cmd_vel.angular.z def update_duty(self, odom): twist = odom.twist.twist if self.linear_pid.setpoint == 0 and self.angular_pid.setpoint == 0: self.linear_pid.clear_integrator() self.angular_pid.clear_integrator() linear = self.linear_pid.calc(twist.linear.x) angular = self.angular_pid.calc(twist.angular.z) self.left_pub.publish(Int16(-linear + angular)) self.right_pub.publish(Int16(-linear - angular))
class MotorController: def __init__(self, mcp_channel, pins, pid_constants, forward): self.current_sensor = CurrentSensor(mcp_channel) self._motor = Motor(pins['pwm'], pins['dir'], forward) self._encoder = RotaryEncoder(pins['a'], pins['b']) self._pid = PID(pid_constants) self._vel_filter = IrregularDoubleExponentialFilter( *motor_vel_smoothing_factors) self._vel = 0 def adjust_motor_speed(self, target_vel, time_elapsed): # use a pi controller to control the speed and limit the output to # [-1, 1] (-1 and 1 correspond to 100% pwm output) error = (target_vel - self._vel) / max_wheel_vel speed = min(max(self._pid.calc(error, time_elapsed), -1), 1) self._motor.set_speed(speed) def read_encoder(self, time_elapsed): # gets the amount the wheel has turned in the last interval and # calculates the wheel's speed with a filter added in dist_traveled = self._encoder.get_pos_dif() * distance_ratio self._vel = self._vel_filter.filter(dist_traveled / time_elapsed, time_elapsed) return dist_traveled def reset(self): self._motor.set_speed(0) self._pid.reset() self._encoder.reset() def stop(self): self._motor.stop() self._encoder.stop()
class ServoController: """Handles the high-level tasks of controlling a servo. It keeps track of the servo's angle and allows for relative movements.""" def __init__(self, pin, pid_constants, left_limit=-pi * 2 / 3, right_limit=pi / 2): self._servo = Servo(pin) self._pid = PID(pid_constants) if left_limit >= right_limit: raise Exception('left_limit >= right_limit') self._left_limit = left_limit self._right_limit = right_limit self.angle = 0.0 # servo cannot be set faster than 50Hz, so 20ms lower_limit self._timer = Timer(0.02) def move_by(self, offset): time_elapsed = self._timer.elapsed() if time_elapsed is None: return offset = self._pid.calc(offset, time_elapsed) # ensure that servo moves at most by max_move offset = symmetric_limit(offset, max_servo_move) self._move_to(self.angle + offset) def _move_to(self, angle): # ensure servo doesn't move beyond limits angle = limit(angle, self._left_limit, self._right_limit) self.angle = angle self._servo.move_to(self._angle_ratio(angle)) def _angle_ratio(self, angle): return (angle - self._left_limit) / (self._right_limit - self._left_limit) def start(self): self._timer.start() def stop(self): self._move_to(0) self._pid.reset() self._timer.reset()
class Controller(object): def __init__(self): self.left_wall_pid = PID(14, 2, 0.5) self.right_wall_pid = PID(14, 2, 0.5) self.left_wall_pid.setpoint = 0.055 self.right_wall_pid.setpoint = 0.055 self.goal_angle = None # distance to goal from start position self.goal_distance = None # distance to goal from current position self.dist_to_goal = 0 self.start_position = None self.position = None self.start_angle = None self.angle = None self.left_dist = 0 self.right_dist = 0 self.front_dist = 0 self.commands = [] self.busy = False self.callback = None self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1) rospy.Subscriber("/wheel_odom", Odometry, self.update) rospy.Subscriber("/sensors/ir_left", Float32, self.ir_left) rospy.Subscriber("/sensors/ir_right", Float32, self.ir_right) rospy.Subscriber("/sensors/ir_front", Float32, self.ir_front) def ir_left(self, data): self.left_dist = data.data def ir_right(self, data): self.right_dist = data.data def ir_front(self, data): self.front_dist = data.data def update(self, odom): # populate current state variables pose = odom.pose.pose twist = odom.twist.twist q = pose.orientation self.angle = transformations.euler_from_quaternion( [q.x, q.y, q.z, q.w])[2] self.position = pose.position if not self.busy and self.commands: data = self.commands.pop(0) data[0]() self.callback = data[1] print("Starting action: " + data[2]) self.busy = True # traverse if needed cmd_vel = Twist() linear_vel = 0 angular_vel = 0 if self.goal_distance != None: # heading correction if self.left_dist > 0.03 and self.left_dist < 0.08: angular_vel -= self.left_wall_pid.calc(self.left_dist) if self.right_dist > 0.03 and self.right_dist < 0.08: angular_vel += self.right_wall_pid.calc(self.right_dist) # vroom offset = abs(self.goal_distance) - dist( self.position.x, self.position.y, self.start_position.x, self.start_position.y) self.dist_to_goal = offset if abs(offset) < 0.01 and abs(twist.linear.x) < 0.01: self.goal_distance = None elif abs(offset) < 0.05: linear_vel = signum(offset) * max(0, 0.15 - abs(angular_vel) / 10) else: linear_vel = signum(offset) * max(0, 0.2 - abs(angular_vel) / 10) # absolute position correction via front IR if offset > 0.08 and offset < 0.18: error = self.front_dist - offset - 0.015 if abs(error) < 0.06: self.goal_distance += error if self.goal_angle != None: offset = wrap_angle(self.goal_angle - self.angle) # print(offset) if abs(offset) < 0.06 and abs(twist.angular.z) < 0.2 and signum( offset) == -signum(twist.angular.z): self.goal_angle = None elif abs(offset) < 0.05: angular_vel = signum(offset) * 1.5 elif abs(offset) < 0.2: angular_vel = signum(offset) * 2.5 else: angular_vel = signum(offset) * 5 if self.busy and self.goal_angle == None and self.goal_distance == None: self.busy = False print('DONE') linear_vel = 0 angular_vel = 0 self.callback and self.callback() cmd_vel.linear.x = linear_vel cmd_vel.angular.z = angular_vel self.cmd_vel_pub.publish(cmd_vel) def turn(self, angle, cb=None): def f(): self.goal_angle = wrap_angle(self.angle + angle) self.commands.append((f, cb, 'turn by ' + str(angle))) def turnTo(self, angle, cb=None): def f(): self.goal_angle = wrap_angle(angle) self.commands.append((f, cb, 'turn to ' + str(angle))) def straight(self, dist, cb=None): def f(): self.start_position = self.position self.start_angle = self.angle self.goal_distance = dist def c(): theta = atan2(self.position.y - self.start_position.y, self.position.x - self.start_position.x) self.goal_angle = theta self.busy = True self.callback = cb self.commands.append((f, c, 'move ' + str(dist * 100) + 'cm'))
class Run(): RADIUS = 40 # MIRSのタイヤ間の距離[cm] USS_RADIUS = 19 USS_DIST = 30 # MIRSの中心から超音波センサまでの距離[cm] USS_DIFF = 6 Kp = 50 Ki = 1 Kd = 1 TARGET_DIST = 0.4 # 目標となる壁との距離[m] INTERVAL = 0.01 # 制御周期[s] USS_DICT_LIST = ["f", "sf", "s", "sb", "b"] def __init__(self): self.isLeft = True self.vel_left_sum = 0 self.vel_right_sum = 0 self.uss = {} self.speed = 15 self.cmd_prev = [] self.pid = PID((Run.Kp, Run.Ki, Run.Kd), 0.1, Run.TARGET_DIST) def set_val(self, is_left, uss): self.isLeft = is_left self.uss["f"] = uss[0] self.uss["b"] = uss[4] if self.isLeft: self.uss["sf"] = uss[7] self.uss["s"] = uss[6] self.uss["sb"] = uss[5] else: self.uss["sf"] = uss[1] self.uss["s"] = uss[2] self.uss["sb"] = uss[3] def straight(self): speed_mod = self.pid.calc(self.calc_dist()) # 近いと正、遠いと負 speed_l, speed_r = self.speed + speed_mod, self.speed - speed_mod return self.is_duplicate_cmd([["velocity", speed_l, speed_r]]) def is_duplicate_cmd(self, cmd): if self.cmd_prev == cmd: return cmd else: return [] def calc_ratio(self): front = self.uss["sf"] back = self.uss["sb"] tmp = ((back - front) / Run.USS_DIST)**2.0 return (tmp + 1)**-0.5 def calc_angle(self): front = self.uss["sf"] back = self.uss["sb"] tmp = math.acos(self.calc_ratio()) if (self.isLeft and front > back) or (not self.isLeft and back > front): tmp *= -1 return math.degrees(tmp) def calc_dist(self): tmp = self.calc_ratio() return tmp * (self.uss["s"] + Run.USS_RADIUS)
def control_process(self, *args): '''This function will called from joystick_async as subprocess''' control_optflow_pipe_read, control_cv_pipe_read, control_tof_pipe_read, control_imu_pipe_read, ext_control_pipe_write, ext_control_pipe_read, nice_level = args os.nice(nice_level) # Init the ToF kalman filter tof_filter = self.tof_filter_init() KFXY, KFXY_z, KFXY_u = self.xy_of_filter_init() ''' PID Init ''' throttle_pd = PID(self.PZ_GAIN, self.IZ_GAIN, self.DZ_GAIN) #throttle PID roll_pd = PID(self.PY_GAIN, self.IY_GAIN, self.DY_GAIN) #roll PID pitch_pd = PID(self.PX_GAIN, self.IX_GAIN, self.DX_GAIN) #pitch PID prev_velocity_pitch = 0 prev_velocity_roll = 0 prev_error_roll = 0 prev_error_pitch = 0 '''Z-axis init''' prev_altitude_sensor = None altitude_sensor = None altitude = None altitude_corrected = None value_available = False postition_hold = False init_altitude = 0 '''CMDS init''' CMDS = {'throttle': 0, 'roll': 0, 'pitch': 0} prev_time = time.time() OF_DIS = of_dis = 0 # For init error_altitude = error_roll = error_pitch = velocity_pitch = velocity_roll = 0 '''Angular Speed''' pre_roll = 0 pre_pitch = 0 while True: CMDS['throttle'] = 0 CMDS['roll'] = 0 # The betaflight config trim the pitch -10 for unbalance of the drone CMDS['pitch'] = 0 # Let the OF Pipe run control_tof_pipe_read.send('a') '''Read the joystick_node trigger the auto mode or not''' if ext_control_pipe_write.poll( ): # joystick loop tells when to save the current values postition_hold = ext_control_pipe_write.recv() if not postition_hold: self.LANDING = True self.DATA = True init_altitude = None '''Update the IMU value''' if control_imu_pipe_read.poll(): self.imu, battery_voltage, self.IMU_TIME = control_imu_pipe_read.recv( ) # [[accX,accY,accZ], [gyroX,gyroY,gyroZ], [roll,pitch,yaw]] #DEBUG USE imut = time.time() angu_time = prev_time - self.IMU_TIME if angu_time > 0.001: angu_roll = (self.imu[2][0] - pre_roll) / angu_time angu_pitch = (self.imu[2][1] - pre_pitch) / angu_time pre_roll = self.imu[2][0] pre_pitch = self.imu[2][1] else: angu_roll = angu_pitch = 0 if self.TAKEOFF: # Tested voltage throttle relationship TAKEOFF_THRUST = int(1015 - 60 * (battery_voltage)) '''Update the ToF Kalman Filter with the ground value''' if postition_hold and altitude_sensor: # Remember to reset integrator here too! prev_altitude_sensor = init_altitude = altitude_sensor postition_hold = False # initial value from the sensor # the cognifly have the initial heigth of 0.11m tof_filter.x = np.array([[init_altitude], [0]]) continue '''Vertical Movement Control''' if init_altitude: # Update the ToF Filter dt = prev_time - self.TOF_Time # For init reading will very large, but normal case would not larger than 1s tof_filter.F[0, 1] = dt if abs(dt < 3) else 0 tof_filter.B[0] = 0.5 * (dt**2) if abs(dt < 3) else 0 tof_filter.B[1] = dt tof_filter.predict( u=0) #Just test for non -9.81*(0.99-imu[0][2]) # Capture the Predicted value altitude = tof_filter.x[0, 0] velocity = tof_filter.x[1, 0] # '''Takeoff Setting''' if self.TAKEOFF: if len(self.TAKEOFF_LIST): CMDS[ 'throttle'] = self.TAKEOFF_LIST[0] * TAKEOFF_THRUST value_available = True self.TAKEOFF_LIST.pop(0) cancel_gravity_value = CMDS['throttle'] else: # control_optflow_pipe_read.send('a') init_altitude = self.TAKEOFF_ALTITUDE velocity = 0 self.TAKEOFF = False # '''PID at Throttle''' if (not self.TAKEOFF): error_altitude = init_altitude - altitude # altitude next_throttle = throttle_pd.calc(error_altitude, time=dt, velocity=-velocity) # Set throttle by PID control CMDS['throttle'] = self.value_limit( next_throttle, self.ABS_MAX_VALUE_THROTTLE) # Add the cancel gravity set point with the angle compensate CMDS['throttle'] += cancel_gravity_value / ( (np.cos(self.imu[2][0] * np.pi * 1 / 180)) * (np.cos(self.imu[2][1] * np.pi * 1 / 180))) value_available = True prev_altitude_sensor = altitude_corrected # LANDING if not self.TAKEOFF and self.LANDING: CMDS['throttle'] = cancel_gravity_value + (15 / (altitude + 0.5)) '''Update the ToF value''' if control_tof_pipe_read.poll(): if not init_altitude: altitude_sensor, self.TOF_Time = control_tof_pipe_read.recv( ) # Flushing the old value # altitude_sensor = control_tof_pipe_read.recv() # Flushing the old value else: # turning the altitdue back to ground, reference as global coordinate altitude_sensor, self.TOF_Time = control_tof_pipe_read.recv( ) #DEBUG USE toft = time.time() # The sensor is not at the center axis of rotation # The following parts are going to turn the offset bact the origin # ROLL: (Measure * cos(roll_angle)) - (sensor_offset * sin(sensor_angle - roll_angle) # PITCH: (Measure - offset) * cos(pitch_angle) # combine: (Measure * cos(roll) * cos(pitch)) - (offset * sin(sensor-roll) * cos(pitch)) # CogniFly offset -> z: -40mm, y: +38mm altitude_corrected = altitude_sensor * (np.cos( self.imu[2][0] * np.pi * 1 / 180)) * (np.cos( self.imu[2][1] * np.pi * 1 / 180)) offset = self.TOFOFFSET_R * np.sin(self.TOFOFFSET_ANGLE - ( self.imu[2][0] * np.pi * 1 / 180)) * (np.cos( self.imu[2][1] * np.pi * 1 / 180)) tof_filter.update([ self.truncate(altitude_corrected - offset), self.truncate(((altitude_corrected - offset) - prev_altitude_sensor) / dt) ]) '''Update the XY Filter''' # if ((not TAKEOFF) and (abs(error_altitude) < 0.2)): if (not self.TAKEOFF): # For init reading will very large, but normal case would not larger than 1s dt_OF = prev_time - self.OF_TIME dt_IMU = prev_time - self.IMU_TIME # KFXY.R[0,0] = (0.005+(velocity/90)) # Increase the noise for the filter when up and down # KFXY.R[1,1] = (0.005+(velocity/90)) # # For init reading will very large, but normal case would not larger than 1s # KFXY.F[0,2] = dt_OF if abs(dt_OF<3) else 0 # KFXY.F[1,3] = dt_OF if abs(dt_OF<3) else 0 # KFXY.B[2,2] = dt_IMU if abs(dt_IMU<3) else 0 # KFXY.B[3,3] = dt_IMU if abs(dt_IMU<3) else 0 # # Another angular speed can be optained by (atitude/dt) # # linear speed can be optained by angluar_speed*height # KFXY_u[2,0] = self.truncate(9.81*(self.imu[0][0])*np.cos(self.imu[2][1])) #imu[0][0]->ax Pitch acc #imu[2][1]->Pitch angle # KFXY_u[3,0] = self.truncate(9.81*(self.imu[0][1])*np.cos(self.imu[2][0])) #imu[0][1]->ay Roll acc #imu[2][0]->Roll angle if control_optflow_pipe_read.poll(): KFXY_z[0, 0], KFXY_z[ 1, 0], of_dis, self.OF_TIME = control_optflow_pipe_read.recv( ) # it will block until a brand new value comes. #DEBUG USE oft = time.time() # KFXY.update(KFXY_z*(-altitude))# To real scale # X-Y reversed # KFXY.predict(u=0)#KFXY_u) # [dx, dy, vx, vy] OF_DIS += of_dis * altitude '''X-Y control''' factor = 1.6 #Seem the data is scaled up 1.6 times error_roll = self.truncate((OF_DIS[1]) / factor) error_pitch = self.truncate((OF_DIS[0]) / factor) velocity_roll_tmp = self.truncate(KFXY_z[1, 0] * (-altitude)) velocity_pitch_tmp = self.truncate(KFXY_z[0, 0] * (-altitude)) self.a = np.exp(-abs(velocity_pitch_tmp - velocity_pitch)) velocity_pitch += self.a * (velocity_pitch_tmp - velocity_pitch) self.a = np.exp(-abs(velocity_roll_tmp - velocity_roll)) velocity_roll += self.a * (velocity_roll_tmp - velocity_roll) # prev_error_roll = error_roll # prev_error_pitch = error_pitch # velocity_roll_tmp = -self.truncate((error_roll-prev_error_roll)/dt_OF) # velocity_pitch_tmp = -self.truncate((error_pitch-prev_error_pitch)/dt_OF) # velocity_roll += self.a*(velocity_roll_tmp - prev_velocity_roll) # velocity_pitch += self.a*(velocity_pitch_tmp - prev_velocity_pitch) # prev_velocity_roll = velocity_roll # prev_velocity_pitch = velocity_pitch # velocity_roll = self.truncate(KFXY.x[3,0]) # velocity_pitch = self.truncate(KFXY.x[2,0]) # The new cognifly is reversed the pi orientation next_roll = roll_pd.calc(-error_roll, velocity=velocity_roll) # Y next_pitch = pitch_pd.calc(-error_pitch, velocity=velocity_pitch) # X CMDS['roll'] = next_roll if abs( next_roll) <= self.ABS_MAX_VALUE_ROLL else ( -1 if next_roll < 0 else 1) * self.ABS_MAX_VALUE_ROLL CMDS['pitch'] = next_pitch if abs( next_pitch) <= self.ABS_MAX_VALUE_PITCH else ( -1 if next_pitch < 0 else 1) * self.ABS_MAX_VALUE_PITCH value_available = True print( ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n" ) print("OF Distance: ", OF_DIS) print( "THROTTLE :{2:.2f} | ALT:{1:.2f} | ERR:{0:.2f} | Vec:{3:.2f}" .format(error_altitude, altitude, next_throttle, velocity)) print("dt_OF:{:.2f} | dt_IMU:{:.2f}".format(dt_OF, dt_IMU)) print("Angular Roll: {:.2f} | Pitch: {:.2f}".format( angu_roll, angu_pitch)) print("ERROR ROLL : %2.2f error| %2.2f roll| %2.2f of" % (error_roll, next_roll, 0)) print("ERROR PITCH: %2.2f error| %2.2f pitch| %2.2f of" % (error_pitch, next_pitch, 0)) print( "ROLL velocity: ", -KFXY.x[3, 0], KFXY_z[1, 0] * (-altitude), self.truncate( (self.imu[2][0] * np.pi * 1 / 180 * altitude / dt))) print( "PITCH velocity", -KFXY.x[2, 0], KFXY_z[0, 0] * (-altitude), self.truncate( (self.imu[2][1] * np.pi * 1 / 180 * altitude / dt))) print( "TIME:{0:1.2f} | OF:{1:.2f} | IMU:{2:.2f} | TOF:{3:.2f}" .format(time.time(), (self.OF_TIME - oft), (self.IMU_TIME - imut), (self.TOF_Time - toft))) self.data.append( (CMDS['throttle'], CMDS['roll'], CMDS['pitch'], altitude, error_altitude, velocity, error_roll, velocity_roll, angu_roll, error_pitch, velocity_pitch, angu_pitch)) if self.DATA: np.save("control_t_auto", self.data) print(">>>>>>>>>>>>>>>>>>SAVED!") self.DATA = False '''Send out the CMDS values back to the joystick loop''' if value_available and (not ext_control_pipe_read.poll()): ext_control_pipe_write.send(CMDS) value_available = False time.sleep(self.PERIOD) prev_time = time.time()
class Controller(object): def __init__(self): self.left_wall_pid = PID(14, 2, 0.5) self.right_wall_pid = PID(14, 2, 0.5) self.left_wall_pid.setpoint = 0.055 self.right_wall_pid.setpoint = 0.055 self.goal_angle = None self.goal_distance = None self.start_position = None self.position = None self.start_angle = None self.angle = None self.left_dist = 0 self.right_dist = 0 self.front_dist = 0 self.commands = [] self.busy = False self.callback = None self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1) rospy.Subscriber("/wheel_odom", Odometry, self.update) rospy.Subscriber("/sensors/ir_left", Float32, self.ir_left) rospy.Subscriber("/sensors/ir_right", Float32, self.ir_right) rospy.Subscriber("/sensors/ir_front", Float32, self.ir_front) def ir_left(self, data): print("ASJKFLAJKFSLJAKSHELLOOOOOOOOOOOOOOOOOO") self.left_dist = data.data def ir_right(self, data): self.right_dist = data.data def ir_front(self, data): self.front_dist = data.data def update(self, odom): # populate current state variables pose = odom.pose.pose twist = odom.twist.twist q = pose.orientation self.angle = transformations.euler_from_quaternion([q.x, q.y, q.z, q.w])[2] self.position = pose.position if not self.busy and self.commands: data = self.commands.pop(0) data[0]() self.callback = data[1] print("Starting action: " + data[2]) self.busy = True # traverse if needed cmd_vel = Twist() linear_vel = 0 angular_vel = 0 if self.goal_distance != None: # heading correction if self.left_dist > 0.03 and self.left_dist < 0.08: angular_vel -= self.left_wall_pid.calc(self.left_dist) if self.right_dist > 0.03 and self.right_dist < 0.08: angular_vel += self.right_wall_pid.calc(self.right_dist) # vroom offset = abs(self.goal_distance) - dist(self.position.x, self.position.y, self.start_position.x, self.start_position.y) if abs(offset) < 0.01 and abs(twist.linear.x) < 0.01: self.goal_distance = None elif abs(offset) < 0.05: linear_vel = signum(offset) * 0.12 else: linear_vel = signum(offset) * 0.18 # absolute position correction via front IR if offset > 0.08 and offset < 0.18: error = self.front_dist - offset - 0.015 if abs(error) < 0.06: self.goal_distance += error if self.goal_angle != None: offset = wrap_angle(self.goal_angle - self.angle) # print(offset) if abs(offset) < 0.06 and abs(twist.angular.z) < 0.2 and signum(offset) == -signum(twist.angular.z): self.goal_angle = None elif abs(offset) < 0.05: angular_vel = signum(offset) * 1.5 elif abs(offset) < 0.2: angular_vel = signum(offset) * 2.5 else: angular_vel = signum(offset) * 5 if self.busy and self.goal_angle == None and self.goal_distance == None: self.busy = False print('DONE') linear_vel = 0 angular_vel = 0 self.callback and self.callback() cmd_vel.linear.x = linear_vel cmd_vel.angular.z = angular_vel self.cmd_vel_pub.publish(cmd_vel) def turn(self, angle, cb=None): def f(): self.goal_angle = wrap_angle(self.angle + angle) self.commands.append((f, cb, 'turn by ' + str(angle))) def turnTo(self, angle, cb=None): def f(): self.goal_angle = wrap_angle(angle) self.commands.append((f, cb, 'turn to ' + str(angle))) def straight(self, dist, cb=None): def f(): self.start_position = self.position self.start_angle = self.angle self.goal_distance = dist def c(): theta = atan2(self.position.y - self.start_position.y, self.position.x - self.start_position.x) self.goal_angle = theta self.busy = True self.callback = cb self.commands.append((f, c, 'move ' + str(dist * 100) + 'cm'))