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