def setup(self): self.timer = Timer() self.utils = Utils(time.time(), 180) #start time, total game time self.sensors = Sensors(self.tamp) self.actuators = Actuators(self.tamp) self.motorController = MotorController(self.sensors, self.actuators) self.myState = startState(self.sensors, self.actuators, self.motorController, self.timer, self.utils)
def __init__(self): # set private attributs self.__world_model__ = WorldModel() self.__update_time__ = rospy.get_param('update_time', 0.9) self.__max_trys__ = rospy.get_param('max_trys', 10) self.__report__ = Report() self.__sensors__ = Sensors(self.__report__) self.__actuators__ = Actuators(self.__report__) # main agent attributs self.actions = Actions(self.__report__) # start thread for world model update thread.start_new_thread(self.__update_world_model__, ())
def __init__(self, J=np.diag([100, 100, 100]), controller=PDController(k_d=np.diag([.01, .01, .01]), k_p=np.diag([.1, .1, .1])), gyros=None, magnetometer=None, earth_horizon_sensor=None, actuators=Actuators(rxwl_mass=14, rxwl_radius=0.1845, rxwl_max_torque=0.68, noise_factor=0.01), dipole=np.array([0, 0, 0]), q=np.array([0, 0, 0, 1]), w=np.array([0, 0, 0]), r=np.array([0, 0, 0]), v=np.array([0, 0, 0])): """Constructs a Spacecraft object to store system objects, and state Args: J (numpy ndarray): the spacecraft's inertia tensor (3x3) (kg * m^2) controller (PDController): the controller that will compute control torques to meet desired pointing and angular velocity requirements gyros (Gyros): an object that models gyroscopes and simulates estimated angular velocity by introducing bias and noise to angular velocity measurements actuators (Actuators): an object that stores reaction wheel state and related methods; actually applies control torques generated by the controller object dipole (numpy ndarray): the spacecraft's residual magnetic dipole vector (A * m^2) q (numpy ndarray): the quaternion representing the attitude (from the inertial to body frame) of the spacecraft (at a given time) w (numpy ndarray): the angular velocity (rad/s) (3x1) in body coordinates of the spacecraft (at a given time) r (numpy ndarray): the inertial position of the spacecraft (m) v (numpy ndarray): the inertial velocity of the spacecraft (m/s) """ self.J = J self.controller = controller self.gyros = gyros self.magnetometer = magnetometer self.earth_horizon_sensor = earth_horizon_sensor self.actuators = actuators self.dipole = dipole self.q = q self.w = w self.r = r self.v = v
def main(): # Define 6U CubeSat mass, dimensions, drag coefficient sc_mass = 8 sc_dim = [226.3e-3, 100.0e-3, 366.0e-3] J = 1 / 12 * sc_mass * np.diag([ sc_dim[1]**2 + sc_dim[2]**2, sc_dim[0]**2 + sc_dim[2]**2, sc_dim[0]**2 + sc_dim[1]**2 ]) sc_dipole = np.array([0, 0.018, 0]) # Define two `PDController` objects—one to represent no control and one # to represent PD control with the specified gains no_controller = PDController(k_d=np.diag([0, 0, 0]), k_p=np.diag([0, 0, 0])) controller = PDController(k_d=np.diag([.01, .01, .01]), k_p=np.diag([.1, .1, .1])) # Northrop Grumman LN-200S Gyros gyros = Gyros(bias_stability=1, angular_random_walk=0.07) perfect_gyros = Gyros(bias_stability=0, angular_random_walk=0) # NewSpace Systems Magnetometer magnetometer = Magnetometer(resolution=10e-9) perfect_magnetometer = Magnetometer(resolution=0) # Adcole Maryland Aerospace MAI-SES Static Earth Sensor earth_horizon_sensor = EarthHorizonSensor(accuracy=0.25) perfect_earth_horizon_sensor = EarthHorizonSensor(accuracy=0) # Sinclair Interplanetary 60 mNm-sec RXWLs actuators = Actuators(rxwl_mass=226e-3, rxwl_radius=0.5 * 65e-3, rxwl_max_torque=20e-3, rxwl_max_momentum=0.18, noise_factor=0.03) perfect_actuators = Actuators(rxwl_mass=226e-3, rxwl_radius=0.5 * 65e-3, rxwl_max_torque=np.inf, rxwl_max_momentum=np.inf, noise_factor=0.0) # define some orbital parameters mu_earth = 3.986004418e14 R_e = 6.3781e6 orbit_radius = R_e + 400e3 orbit_w = np.sqrt(mu_earth / orbit_radius**3) period = 2 * np.pi / orbit_w # define a function that returns the inertial position and velocity of the # spacecraft (in m & m/s) at any given time def position_velocity_func(t): r = orbit_radius / np.sqrt(2) * np.array([ -np.cos(orbit_w * t), np.sqrt(2) * np.sin(orbit_w * t), np.cos(orbit_w * t), ]) v = orbit_w * orbit_radius / np.sqrt(2) * np.array([ np.sin(orbit_w * t), np.sqrt(2) * np.cos(orbit_w * t), -np.sin(orbit_w * t), ]) return r, v # compute the initial inertial position and velocity r_0, v_0 = position_velocity_func(0) # define the body axes in relation to where we want them to be: # x = Earth-pointing # y = pointing along the velocity vector # z = normal to the orbital plane b_x = -normalize(r_0) b_y = normalize(v_0) b_z = cross(b_x, b_y) # construct the nominal DCM from inertial to body (at time 0) from the body # axes and compute the equivalent quaternion dcm_0_nominal = np.stack([b_x, b_y, b_z]) q_0_nominal = dcm_to_quaternion(dcm_0_nominal) # compute the nominal angular velocity required to achieve the reference # attitude; first in inertial coordinates then body w_nominal_i = 2 * np.pi / period * normalize(cross(r_0, v_0)) w_nominal = np.matmul(dcm_0_nominal, w_nominal_i) # provide some initial offset in both the attitude and angular velocity q_0 = quaternion_multiply( np.array( [0, np.sin(2 * np.pi / 180 / 2), 0, np.cos(2 * np.pi / 180 / 2)]), q_0_nominal) w_0 = w_nominal + np.array([0.005, 0, 0]) # define a function that will model perturbations def perturb_func(satellite): return (satellite.approximate_gravity_gradient_torque() + satellite.approximate_magnetic_field_torque()) # define a function that returns the desired state at any given point in # time (the initial state and a subsequent rotation about the body x, y, or # z axis depending upon which nominal angular velocity term is nonzero) def desired_state_func(t): if w_nominal[0] != 0: dcm_nominal = np.matmul(t1_matrix(w_nominal[0] * t), dcm_0_nominal) elif w_nominal[1] != 0: dcm_nominal = np.matmul(t2_matrix(w_nominal[1] * t), dcm_0_nominal) elif w_nominal[2] != 0: dcm_nominal = np.matmul(t3_matrix(w_nominal[2] * t), dcm_0_nominal) return dcm_nominal, w_nominal # construct three `Spacecraft` objects composed of all relevant spacecraft # parameters and objects that resemble subsystems on-board # 1st Spacecraft: no controller # 2nd Spacecraft: PD controller with perfect sensors and actuators # 3rd Spacecraft: PD controller with imperfect sensors and actuators satellite_no_control = Spacecraft( J=J, controller=no_controller, gyros=perfect_gyros, magnetometer=perfect_magnetometer, earth_horizon_sensor=perfect_earth_horizon_sensor, actuators=perfect_actuators, q=q_0, w=w_0, r=r_0, v=v_0) satellite_perfect = Spacecraft( J=J, controller=controller, gyros=perfect_gyros, magnetometer=perfect_magnetometer, earth_horizon_sensor=perfect_earth_horizon_sensor, actuators=perfect_actuators, q=q_0, w=w_0, r=r_0, v=v_0) satellite_noise = Spacecraft(J=J, controller=controller, gyros=gyros, magnetometer=magnetometer, earth_horizon_sensor=earth_horizon_sensor, actuators=actuators, q=q_0, w=w_0, r=r_0, v=v_0) # Simulate the behavior of all three spacecraft over time simulate(satellite=satellite_no_control, nominal_state_func=desired_state_func, perturbations_func=perturb_func, position_velocity_func=position_velocity_func, stop_time=6000, tag=r"(No Control)") simulate(satellite=satellite_perfect, nominal_state_func=desired_state_func, perturbations_func=perturb_func, position_velocity_func=position_velocity_func, stop_time=6000, tag=r"(Perfect Estimation \& Control)") simulate(satellite=satellite_noise, nominal_state_func=desired_state_func, perturbations_func=perturb_func, position_velocity_func=position_velocity_func, stop_time=6000, tag=r"(Actual Estimation \& Control)")
def __init__(self): """ This method creates a tortoise. It initializes the sensors, the variables that control the random motion and creates a file with the PID of the process which has created the tortoise so that the watchdog (a background process) can stops the motors and LEDs in case the user process finishes because of an error. The tortoise is created uncalibrated. The reasons of termination of a user process could be because of normal termination or because of an error (exceptions, ctrl-c, ...). When an error happens, the motors and may be still on. In this case, the motors and LEDs should be turned off for the battery not to drain. The solution implemented is to have a background process (a watchdog) running continously. This process checks if the user process doesn't exist anymore (termination). If it doesn't, it stops the motors, switches off the LEDs and cleans up all the pins. In order to identy that the user script has finished, a file with the name [PID].pid is created in the folder ~/.tortoise_pids/, where [PID] is the PID of the user process. Regarding calibration, the purpose was to avoid calibration everytime the tortoise object is created. However, this hasn't been implemented yet. The idea could be to save the light values in a file and read that file when creating the tortoise object. Light conditions could have changed, so this should be done carefully. At the moment, the tortoise object is created without calibration. If the users want to use the light sensors, they need will need to execute the calibrateLight function before using those sensors. """ # Variables that control the calibration of the light sensors global isLightCalibrated global lowerBoundLight global upperBoundLight isLightCalibrated = False lowerBoundLight = 0 upperBoundLight = 0 # --- Variables that control the calibration of the light sensors --- # No warnings from the GPIO library GPIO.setwarnings(False) # Variables that control the random motion self.lastRandomCommand = None self.timesSameRandomCommandExecuted = 0 self.numberRepeatsRandomCommand = -1 self.lastRandomStepsWheelA = None self.lastRandomStepsWheelB = None self.lastRandomDirection = None # --- Variables that control the random motion --- # Setting the motors, sensors and actuators # Pin numbers of the motors motorPins = [13, 6, 5, 7, 20, 10, 9, 11] self.A = Motor(motorPins[0], motorPins[1], motorPins[2], motorPins[3]) self.B = Motor(motorPins[4], motorPins[5], motorPins[6], motorPins[7]) self.sensors = Sensors() self.actuators = Actuators() # Position 1 of the light sensors area in the PCB assigned to pin 17 self.sensors.setSensor(enums.SensorType.light, 1, 17) # Position 2 of the light sensors area in the PCB assigned to pin 4 self.sensors.setSensor(enums.SensorType.light, 2, 4) # Position 1 of the touch sensors area in the PCB assigned to pin 3 self.sensors.setSensor(enums.SensorType.emergencyStop, 1, 3) # Position 2 of the touch sensors area in the PCB assigned to pin 27 self.sensors.setSensor(enums.SensorType.touch, 2, 27) # Position 3 of the touch sensors area in the PCB assigned to pin 2 self.sensors.setSensor(enums.SensorType.touch, 3, 2) # Position 4 of the touch sensors area in the PCB assigned to pin 18 self.sensors.setSensor(enums.SensorType.touch, 4, 18) # Position 1 of the proximity sensors area in the PCB assigned to pin 19 self.sensors.setSensor(enums.SensorType.proximity, 1, 19) # Position 2 of the proximity sensors area in the PCB assigned to pin 21 self.sensors.setSensor(enums.SensorType.proximity, 2, 21) # Position 3 of the proximity sensors area in the PCB assigned to pin 22 self.sensors.setSensor(enums.SensorType.proximity, 3, 22) # Position 4 of the proximity sensors area in the PCB assigned to pin 26 self.sensors.setSensor(enums.SensorType.proximity, 4, 26) # Positions of the LEDs area in the PCB assigned to pins 8, 16, 25, 12 ledPins = [8, 16, 25, 12] self.actuators.initActuator(enums.ActuatorType.led, 1, ledPins[0]) self.actuators.initActuator(enums.ActuatorType.led, 2, ledPins[1]) self.actuators.initActuator(enums.ActuatorType.led, 3, ledPins[2]) self.actuators.initActuator(enums.ActuatorType.led, 4, ledPins[3]) # --- Setting the motors, sensors and actuators --- # Times pressed the touch sensor for the latching behavour self.lastTouch = [-1,-1,-1] # Minimum milliseconds to send to the motors as delay self.minDelayMotors = 2 # Creation of a file with the PID of the process # PID of process pid = os.getpid() # ~/.tortoise_pids/ directory = os.path.expanduser("~") + "/.tortoise_pids/" # Filename: [PID].pid f = open(directory + str(pid) + ".pid", "w") # First line: motor pins f.write(str(motorPins[0]) + " " + str(motorPins[1]) + " " + str(motorPins[2]) + " " + str(motorPins[3]) + " " + str(motorPins[4]) + " " + str(motorPins[5]) + " " + str(motorPins[6]) + " " + str(motorPins[7]) + "\n") # Second line: LED pins f.write(str(ledPins[0]) + " " + str(ledPins[1]) + " " + str(ledPins[2]) + " " + str(ledPins[3]) + "\n") f.close() # --- Creation of a file with the PID of the process --- # Waiting for the user to press the e-stop button self.state = enums.State.paused messages.printMessage('greetings') while self.getSensorData(enums.SensorType.emergencyStop, 4) == 0: time.sleep(0.1) messages.printMessage('running') self.state = enums.State.running
class Tortoise: """ This is the class which implements the high-level behaviour of the tortoises. In order to make a proper use of the tortoises, an instance of this class should be created. """ def __init__(self): """ This method creates a tortoise. It initializes the sensors, the variables that control the random motion and creates a file with the PID of the process which has created the tortoise so that the watchdog (a background process) can stops the motors and LEDs in case the user process finishes because of an error. The tortoise is created uncalibrated. The reasons of termination of a user process could be because of normal termination or because of an error (exceptions, ctrl-c, ...). When an error happens, the motors and may be still on. In this case, the motors and LEDs should be turned off for the battery not to drain. The solution implemented is to have a background process (a watchdog) running continously. This process checks if the user process doesn't exist anymore (termination). If it doesn't, it stops the motors, switches off the LEDs and cleans up all the pins. In order to identy that the user script has finished, a file with the name [PID].pid is created in the folder ~/.tortoise_pids/, where [PID] is the PID of the user process. Regarding calibration, the purpose was to avoid calibration everytime the tortoise object is created. However, this hasn't been implemented yet. The idea could be to save the light values in a file and read that file when creating the tortoise object. Light conditions could have changed, so this should be done carefully. At the moment, the tortoise object is created without calibration. If the users want to use the light sensors, they need will need to execute the calibrateLight function before using those sensors. """ # Variables that control the calibration of the light sensors global isLightCalibrated global lowerBoundLight global upperBoundLight isLightCalibrated = False lowerBoundLight = 0 upperBoundLight = 0 # --- Variables that control the calibration of the light sensors --- # No warnings from the GPIO library GPIO.setwarnings(False) # Variables that control the random motion self.lastRandomCommand = None self.timesSameRandomCommandExecuted = 0 self.numberRepeatsRandomCommand = -1 self.lastRandomStepsWheelA = None self.lastRandomStepsWheelB = None self.lastRandomDirection = None # --- Variables that control the random motion --- # Setting the motors, sensors and actuators # Pin numbers of the motors motorPins = [13, 6, 5, 7, 20, 10, 9, 11] self.A = Motor(motorPins[0], motorPins[1], motorPins[2], motorPins[3]) self.B = Motor(motorPins[4], motorPins[5], motorPins[6], motorPins[7]) self.sensors = Sensors() self.actuators = Actuators() # Position 1 of the light sensors area in the PCB assigned to pin 17 self.sensors.setSensor(enums.SensorType.light, 1, 17) # Position 2 of the light sensors area in the PCB assigned to pin 4 self.sensors.setSensor(enums.SensorType.light, 2, 4) # Position 1 of the touch sensors area in the PCB assigned to pin 3 self.sensors.setSensor(enums.SensorType.emergencyStop, 1, 3) # Position 2 of the touch sensors area in the PCB assigned to pin 27 self.sensors.setSensor(enums.SensorType.touch, 2, 27) # Position 3 of the touch sensors area in the PCB assigned to pin 2 self.sensors.setSensor(enums.SensorType.touch, 3, 2) # Position 4 of the touch sensors area in the PCB assigned to pin 18 self.sensors.setSensor(enums.SensorType.touch, 4, 18) # Position 1 of the proximity sensors area in the PCB assigned to pin 19 self.sensors.setSensor(enums.SensorType.proximity, 1, 19) # Position 2 of the proximity sensors area in the PCB assigned to pin 21 self.sensors.setSensor(enums.SensorType.proximity, 2, 21) # Position 3 of the proximity sensors area in the PCB assigned to pin 22 self.sensors.setSensor(enums.SensorType.proximity, 3, 22) # Position 4 of the proximity sensors area in the PCB assigned to pin 26 self.sensors.setSensor(enums.SensorType.proximity, 4, 26) # Positions of the LEDs area in the PCB assigned to pins 8, 16, 25, 12 ledPins = [8, 16, 25, 12] self.actuators.initActuator(enums.ActuatorType.led, 1, ledPins[0]) self.actuators.initActuator(enums.ActuatorType.led, 2, ledPins[1]) self.actuators.initActuator(enums.ActuatorType.led, 3, ledPins[2]) self.actuators.initActuator(enums.ActuatorType.led, 4, ledPins[3]) # --- Setting the motors, sensors and actuators --- # Times pressed the touch sensor for the latching behavour self.lastTouch = [-1,-1,-1] # Minimum milliseconds to send to the motors as delay self.minDelayMotors = 2 # Creation of a file with the PID of the process # PID of process pid = os.getpid() # ~/.tortoise_pids/ directory = os.path.expanduser("~") + "/.tortoise_pids/" # Filename: [PID].pid f = open(directory + str(pid) + ".pid", "w") # First line: motor pins f.write(str(motorPins[0]) + " " + str(motorPins[1]) + " " + str(motorPins[2]) + " " + str(motorPins[3]) + " " + str(motorPins[4]) + " " + str(motorPins[5]) + " " + str(motorPins[6]) + " " + str(motorPins[7]) + "\n") # Second line: LED pins f.write(str(ledPins[0]) + " " + str(ledPins[1]) + " " + str(ledPins[2]) + " " + str(ledPins[3]) + "\n") f.close() # --- Creation of a file with the PID of the process --- # Waiting for the user to press the e-stop button self.state = enums.State.paused messages.printMessage('greetings') while self.getSensorData(enums.SensorType.emergencyStop, 4) == 0: time.sleep(0.1) messages.printMessage('running') self.state = enums.State.running # --- Waiting for the user to press the e-stop button --- def getStateTortoise(self): """ Returns the state of the tortoise, either paused or running. :rtype: enums.State """ return self.state def setStateTortoise(self, toState): self.state = toState def calibrateLight(self): """ Calibrates the light sensor, defining the upper and lower bounds of the light, i.e. the values returned by the light sensor of the ambience and when a light source is placed in front of it. Currently, only the light sensor in the position 1 of the PCB is used for calibration. """ global lowerBoundLight, upperBoundLight, isLightCalibrated messages.printMessage('calibration_ambient') raw_input() lowerBoundLight = self.sensors.readSensor(enums.SensorType.light, 1) messages.printMessage('calibration_light_source') raw_input() upperBoundLight = self.sensors.readSensor(enums.SensorType.light, 1) isLightCalibrated = True messages.printMessage('calibration_complete') def getSensorData(self, sensor_type, position): """ Returns a different value depending on the type of sensor queried: - For the light sensor: an int in the range [0, 9] - For the touch sensor: 1 if the sensor has been pressed since the last time it was queried, 0 if not - For the e-stop: 1 if it's ON, 0 if it's OFF - For the proximity sensor: 1 if there's an obstacle (it's ON), 0 if not (it's OFF) :param sensor_type: type of the sensor queried :param position: position in the PCB of the sensor queried :type sensor_type: enums.SensorType :type position: int :rtype: int """ if (sensor_type == enums.SensorType.touch): if (position < 1 or position > 3): messages.printMessage('bad_touch_sensor') self.blinkLEDs_error() return -1 elif (sensor_type == enums.SensorType.emergencyStop): if (position != 4): messages.printMessage('bad_emergency_sensor') self.blinkLEDs_error() return -1 elif (sensor_type == enums.SensorType.light): if (position != 1 and position!=2): messages.printMessage('bad_light_sensor') self.blinkLEDs_error() return -1 elif (sensor_type == enums.SensorType.proximity): if (position < 1 or position > 4): messages.printMessage('bad_proximity_sensor') self.blinkLEDs_error() return -1 else: messages.printMessage('bad_sensor') self.blinkLEDs_error() return -1 # The value of the sensor is read at lowlevel value = self.sensors.readSensor(sensor_type, position) # For the light sensor, 'value' is the count of the light if sensor_type == enums.SensorType.light: return value if (upperBoundLight - lowerBoundLight) == 0: messages.printMessage('no_calibration') self.blinkLEDs_error() return -1 # A scale to the range [0, 9] is done lightVal = int(9 - round(abs(value-upperBoundLight)/(abs(upperBoundLight - lowerBoundLight)/9))) if lightVal < 0: messages.printMessage('no_calibration') self.blinkLEDs_error() return -1 return lightVal # For the touch sensor, 'value' is the number of times the sensor has been pressed elif sensor_type == enums.SensorType.touch: # Returns if the sensor has been pressed since the last time it was queried return self.getSwitchTriggered(position,value) # For the e-stop, 'value' is the number of times the sensor has been pressed elif sensor_type == enums.SensorType.emergencyStop: # Returns if it's either 1 (ON) or 0 (OFF) return value % 2 # For the proximity sensor, 'value' is either 1 (ON) or 0 (OFF) elif sensor_type == enums.SensorType.proximity: # Returns 1 (ON) or 0 (OFF) return value def getSwitchTriggered(self, position, value): if self.lastTouch[position-1] < 0: self.lastTouch[position-1] = value return 0 elif self.lastTouch[position-1] == value: return 0 else: self.lastTouch[position-1] = value return 1 def getLEDValue(self, position): """ Returns 1 if the LED is ON, 0 if it's OFF. :param position: position in the PCB of the sensor queried :type position: int :rtype: int """ if (position < 1 or position > 4): messages.printMessage('bad_LED') self.blinkLEDs_error() return -1 return self.actuators.getActuatorValue(enums.ActuatorType.led, position) def setLEDValue(self, position, value): """ Turns on/off an LED. :param position: position in the PCB of the sensor queried :param value: 1 (ON) or 0 (OFF) :type position: int :type value: int """ if(position < 1 or position > 4): messages.printMessage('bad_LED') self.blinkLEDs_error() return -1 if(value != 0 and value != 1): messages.printMessage('bad_LED_value') self.blinkLEDs_error() return -1 self.actuators.setActuatorValue(enums.ActuatorType.led, position, value) return 0 def blinkLEDs(self, positions, numberOfBlinks, delay, blocking = False): """ Blinks the LEDs wanted the number of times specified. :param positions: position or positions in the PCB of the LEDs wanted :param numberOfBlinks: number of blinks :param delay: milliseconds to wait between blinking :param blocking: whether the function should block forever or not. :type positions: int, [int] :type numberOfBlinks: int :type delay: int :type blocking: boolean .. warning:: If blocking == True, the thread will block forever because of infinite loop. It's only used when there are errors. """ if numberOfBlinks < 0: messages.printMessage('blinks_negative') self.blinkLEDs_error() return -1 if numberOfBlinks == 0: messages.printMessage('blinks_zero') self.blinkLEDs_error() return -1 if delay < 0: messages.printMessage('blinking_fast') self.blinkLEDs_error() return -1 # If no TypeError exception raised, 'positions' is an array try: # All positions are checked for y in range(0, len(positions)): if positions[y] < 0 or positions[y] > 4: messages.printMessage('bad_LED') self.blinkLEDs_error() return -1 except TypeError: # It's not an array but an integer if positions < 0 or positions > 4: messages.printMessage('bad_LED') self.blinkLEDs_error() return -1 # The current state of the LEDs is saved to restore it later previousStateLEDs = [ self.getLEDValue(x) for x in range(1, 5) ] cont = True # If blocking == True, it's an infinite loop to "stop" the execution of the program and keep blinkind the LEDs while cont: for x in range(0, numberOfBlinks): # If no TypeError exception raised, 'positions' is an array try: for y in range(0, len(positions)): self.actuators.setActuatorValue(enums.ActuatorType.led, positions[y], 1) time.sleep(delay) for y in range(0, len(positions)): self.actuators.setActuatorValue(enums.ActuatorType.led, positions[y], 0) time.sleep(delay) except TypeError: # It's not an array but an integer self.actuators.setActuatorValue(enums.ActuatorType.led, positions, 1) time.sleep(delay) self.actuators.setActuatorValue(enums.ActuatorType.led, positions, 0) time.sleep(delay) # Depending on the parameter, it blocks or not cont = blocking # If it doesn't block, the previous state of the LEDs is restored for x in range(1, 5): self.setLEDValue(x, previousStateLEDs[x - 1]) return 0 def blinkLEDs_error(self): self.blinkLEDs([1, 2, 3, 4], 3, 0.2, blocking = True) def moveMotors(self, stepsWheelA, stepsWheelB, delayWheelA, delayWheelB, direction): """ Move the motors of the wheels. Running the motors is done in different threads so that both wheels can move at the same time. The thread that executes this functions waits until the threads are finished, i.e. the motion is done. However, it doesn't block, but checks every half a second if the e-stop button has been pressed. If so, it stops the motors and exits. :param stepsWheelA: number of rotations of the motor attached to wheel A :param stepsWheelB: number of rotations of the motor attached to wheel B :param delayWheelA: controls the speed of rotation of the motor attached to wheel A (minimum is 2 milliseconds) :param delayWheelB: controls the speed of rotation of the motor attached to wheel B (minimum is 2 milliseconds) :param direction: the direction in which the tortoise should move :type stepsWheelA: int :type stepsWheelB: int :type delayWheelA: int :type delayWheelB: int :type direction: enums.Direction """ if( direction != enums.Direction.backwards_right and direction != enums.Direction.backwards_left and direction != enums.Direction.forwards_right and direction != enums.Direction.forwards_left and direction != enums.Direction.forwards and direction != enums.Direction.backwards and direction != enums.Direction.clockwise and direction != enums.Direction.counterClockwise ) : messages.printMessage('bad_direction') self.blinkLEDs_error() return -1 if(stepsWheelA < 0 or stepsWheelB < 0): messages.printMessage('bad_steps') self.blinkLEDs_error() return -1 if((stepsWheelA > 0 and delayWheelA < self.minDelayMotors) or (stepsWheelB > 0 and delayWheelB < self.minDelayMotors)): messages.printMessage('bad_delay') self.blinkLEDs_error() return -1 # If a stop command has been sent, the turtle will stop its movement if self.getSensorData(enums.SensorType.emergencyStop, 4) == 0: if self.getStateTortoise() == enums.State.running: self.setStateTortoise(enums.State.paused) messages.printMessage('paused') else: if self.getStateTortoise() == enums.State.paused: self.setStateTortoise(enums.State.running) messages.printMessage('resumed') # The threads are created. They aren't started yet though. motorAprocess_backwards = Process(target=self.A.backwards, args=(delayWheelA / 1000.00, stepsWheelA)) motorBprocess_backwards = Process(target=self.B.backwards, args=(delayWheelB / 1000.00, stepsWheelB)) motorAprocess_forwards = Process(target=self.A.forwards, args=(delayWheelA / 1000.00, stepsWheelA)) motorBprocess_forwards = Process(target=self.B.forwards, args=(delayWheelB / 1000.00, stepsWheelB)) # The specific wheels are started in order to accomplish the desired movement in the direction commanded if direction == enums.Direction.backwards_left or direction == enums.Direction.backwards or direction == enums.Direction.backwards_right: if stepsWheelA > 0: motorAprocess_backwards.start() if stepsWheelB > 0: motorBprocess_backwards.start() elif direction == enums.Direction.forwards_right or direction == enums.Direction.forwards or direction == enums.Direction.forwards_left: if stepsWheelA > 0: motorAprocess_forwards.start() if stepsWheelB > 0: motorBprocess_forwards.start() elif direction == enums.Direction.clockwise: if stepsWheelA > 0: motorAprocess_backwards.start() if stepsWheelB > 0: motorBprocess_forwards.start() elif direction == enums.Direction.counterClockwise: if stepsWheelA > 0: motorAprocess_forwards.start() if stepsWheelB > 0: motorBprocess_backwards.start() # The main loop pools the emergencyStop while the motors are running while motorAprocess_backwards.is_alive() or motorBprocess_backwards.is_alive() or motorAprocess_forwards.is_alive() or motorBprocess_forwards.is_alive(): # If a stop command has been sent, the turtle will stop its movement and exit this function if self.getSensorData(enums.SensorType.emergencyStop, 4) == 0: if self.getStateTortoise() == enums.State.running: self.setStateTortoise(enums.State.paused) messages.printMessage('paused') if motorAprocess_backwards.is_alive(): motorAprocess_backwards.terminate() motorAprocess_backwards.join() if motorBprocess_backwards.is_alive(): motorBprocess_backwards.terminate() motorBprocess_backwards.join() if motorAprocess_forwards.is_alive(): motorAprocess_forwards.terminate() motorAprocess_forwards.join() if motorBprocess_forwards.is_alive(): motorBprocess_forwards.terminate() motorBprocess_forwards.join() elif self.getStateTortoise() == enums.State.paused: self.setStateTortoise(enums.State.running) messages.printMessage('resumed') time.sleep(0.5) # When the movement finishes, the motors are turned off self.A.stopMotors() self.B.stopMotors() return 0 def moveForwards(self, steps): """ The tortoise moves forwards. :param steps: number of rotations of the motors :type steps: int """ return self.moveMotors(steps, steps, self.minDelayMotors, self.minDelayMotors, enums.Direction.forwards) def moveBackwards(self, steps): """ The tortoise moves backwards. :param steps: number of rotations of the motors :type steps: int """ return self.moveMotors(steps, steps, self.minDelayMotors, self.minDelayMotors, enums.Direction.backwards) def turnOnTheSpot(self, steps, direction): """ This function makes the tortoise turn with the centre of rotation in one of the wheels. :param steps: number of rotations of the motors :param direction: one of these four combinations: [forwards/backwards]_[left/right] :type steps: int :type direction: enums.Direction """ if(steps < 0): messages.printMessage('bad_steps') self.blinkLEDs_error() return -1 if( direction != enums.Direction.backwards_right and direction != enums.Direction.backwards_left and direction != enums.Direction.forwards_right and direction != enums.Direction.forwards_left ) : messages.printMessage('bad_direction_turn') self.blinkLEDs_error() return -1 # Only wheel A moves if direction == enums.Direction.backwards_right or direction == enums.Direction.forwards_right: return self.moveMotors(steps, 0, self.minDelayMotors, 0, direction) # Only wheel B moves elif direction == enums.Direction.backwards_left or direction == enums.Direction.forwards_left: return self.moveMotors(0, steps, 0, self.minDelayMotors, direction) def shuffleOnTheSpot(self, steps, direction): """ This function makes the tortoise turn with the centre of rotation between both wheels. :param steps: number of rotations of the motors :param direction: either clockwise or counter-clockwise :type steps: int :type direction: enums.Direction """ if(steps < 0): messages.printMessage('bad_steps') self.blinkLEDs_error() return -1 if( direction != enums.Direction.clockwise and direction != enums.Direction.counterClockwise ) : messages.printMessage('bad_shuffle') self.blinkLEDs_error() return -1 return self.moveMotors(steps, steps, self.minDelayMotors, self.minDelayMotors, direction) def shuffle45degrees(self, direction): """ This function tries to make the tortoise shuffle 45 degrees. :param direction: one of these four combinations: [forwards/backwards]_[left/right] :type direction: enums.Direction """ return self.shuffleOnTheSpot(180, direction) def turn(self, stepsWheelA, stepsWheelB, direction): """ This function makes the tortoise turn by specifying different steps for the wheels. The function computes the delay that the wheel with less rotations should have in order to finish at the same time than the other wheel. :param stepsWheelA: number of rotations of the motor attached to wheel A :param stepsWheelB: number of rotations of the motor attached to wheel B :param direction: one of these four combinations: [forwards/backwards]_[left/right] :type stepsWheelA: int :type stepsWheelB: int :type direction: enums.Direction """ if( direction != enums.Direction.backwards_right and direction != enums.Direction.backwards_left and direction != enums.Direction.forwards_right and direction != enums.Direction.forwards_left ) : messages.printMessage('bad_direction_turn') self.blinkLEDs_error() return -1 if (direction == enums.Direction.backwards_right or direction == enums.Direction.forwards_right) and (stepsWheelB >= stepsWheelA): messages.printMessage('bad_turn') self.blinkLEDs_error() return -1 if (direction == enums.Direction.backwards_left or direction == enums.Direction.forwards_left) and (stepsWheelA >= stepsWheelB): messages.printMessage('bad_turn') self.blinkLEDs_error() return -1 if(stepsWheelA < 0 or stepsWheelB < 0): messages.printMessage('bad_steps') self.blinkLEDs_error() return -1 if direction == enums.Direction.backwards_right or direction == enums.Direction.forwards_right: # The delay of the wheel with less movements is worked out so that both wheels finish more or less at the same time delay = (stepsWheelA * self.minDelayMotors) / stepsWheelB return self.moveMotors(stepsWheelA, stepsWheelB, self.minDelayMotors, delay, direction) elif direction == enums.Direction.backwards_left or direction == enums.Direction.forwards_left: # The delay of the wheel with less movements is worked out so that both wheels finish more or less at the same time delay = (stepsWheelB * self.minDelayMotors) / stepsWheelA return self.moveMotors(stepsWheelA, stepsWheelB, delay, self.minDelayMotors, direction) def turn45degrees_sharp(self, direction): """ This function tries to make the tortoise turn 45 degrees sharply. :param direction: one of these four combinations: [forwards/backwards]_[left/right] :type direction: enums.Direction """ if direction == enums.Direction.backwards_right or direction == enums.Direction.forwards_right: return self.turn(400, 75, direction) elif direction == enums.Direction.backwards_left or direction == enums.Direction.forwards_left: return self.turn(75, 400, direction) def turn30degrees_wide(self, direction): """ This function tries to make the tortoise turn 45 degrees wide. :param direction: one of these four combinations: [forwards/backwards]_[left/right] :type direction: enums.Direction """ if direction == enums.Direction.backwards_right or direction == enums.Direction.forwards_right: return self.turn(450, 250, direction) elif direction == enums.Direction.backwards_left or direction == enums.Direction.forwards_left: return self.turn(250, 450, direction) def doRandomMovement(self): """ Performs a natural random movement. The function chooses a movement, and it can be repeated up to three times. The probabilities of repeating the movement are as follows: - No repetition: 60% - Once: 25% - Twice: 10% - Three times: 5% The probabilities of choosing a random movement are as follows: - Move forwards: 30% - Move backwards: 10% - Shuffling: 10% - Turning 30 or 45 degrees: 10% - Turning with random steps: 40% The direction of movement for the turns and shuffles is chosen randomly. """ # New random command if self.numberRepeatsRandomCommand == -1 or self.timesSameRandomCommandExecuted == self.numberRepeatsRandomCommand: # The number of times the command is repeated is chosen randomly with decreasing probabilities up to 3 times. self.numberRepeatsRandomCommand = np.random.choice([0, 1, 2, 3], 1, p = [0.6, 0.25, 0.1, 0.05]) # As this is a new command, no repetitions done self.timesSameRandomCommandExecuted = 0 # Random steps for wheel A self.lastRandomStepsWheelA = np.random.randint(30, 300) # Random number between 0 and 1 for decision on the random movement randomNumber = np.random.random_sample() # 40% probability of moving forwards/backwards if(randomNumber < 0.4): # 75% of probability of moving forwards if(randomNumber < 0.30): self.moveForwards(self.lastRandomStepsWheelA) self.lastRandomCommand = self.moveForwards # 25% probability of moving backwards else: self.moveBackwards(self.lastRandomStepsWheelA) self.lastRandomCommand = self.moveBackwards # 10% probability of shuffling elif (randomNumber < 0.5): # Equal probability of going clockwise or counter clockwise self.lastRandomDirection = np.random.choice([enums.Direction.clockwise, enums.Direction.counterClockwise], 1) self.shuffle45degrees(self.lastRandomDirection) self.lastRandomCommand = self.shuffle45degrees # 10% probability of turning 30 or 45 degrees elif (randomNumber < 0.6): # Equal probability of moving forwards/backwards lef/right self.lastRandomDirection = np.random.choice([enums.Direction.forwards_right, enums.Direction.forwards_left, enums.Direction.backwards_right, enums.Direction.backwards_left], 1) # Equal probability of turning 30 degrees wide or 45 degrees sharp if(randomNumber < 0.55): self.turn45degrees_sharp(self.lastRandomDirection) self.lastRandomCommand = self.turn45degrees_sharp else: self.turn30degrees_wide(self.lastRandomDirection) self.lastRandomCommand = self.turn30degrees_wide # 40% of turning randomly else: # Random steps for wheel B self.lastRandomStepsWheelB = np.random.randint(30, 300) # Equal probability of moving forwards/backwards lef/right self.lastRandomDirection = np.random.choice([enums.Direction.forwards_right, enums.Direction.forwards_left, enums.Direction.backwards_right, enums.Direction.backwards_left], 1) if(self.lastRandomDirection == enums.Direction.forwards_left or self.lastRandomDirection == enums.Direction.backwards_left): if(self.lastRandomStepsWheelA >= self.lastRandomStepsWheelB): aux = self.lastRandomStepsWheelA self.lastRandomStepsWheelA = self.lastRandomStepsWheelB - 1 # To avoid the case of equals self.lastRandomStepsWheelB = aux else: if(self.lastRandomStepsWheelB >= self.lastRandomStepsWheelA): aux = self.lastRandomStepsWheelA self.lastRandomStepsWheelA = self.lastRandomStepsWheelB self.lastRandomStepsWheelB = aux - 1 # To avoid the case of equals self.turn(self.lastRandomStepsWheelA, self.lastRandomStepsWheelB, self.lastRandomDirection) self.lastRandomCommand = self.turn # Repeat last command else: self.timesSameRandomCommandExecuted = self.timesSameRandomCommandExecuted + 1 if self.lastRandomCommand == self.moveForwards: self.moveForwards(self.lastRandomStepsWheelA) elif self.lastRandomCommand == self.moveBackwards: self.moveBackwards(self.lastRandomStepsWheelA) elif self.lastRandomCommand == self.shuffle45degrees: self.shuffle45degrees(self.lastRandomDirection) elif self.lastRandomCommand == self.turn45degrees_sharp: self.turn45degrees_sharp(self.lastRandomDirection) elif self.lastRandomCommand == self.turn30degrees_wide: self.turn30degrees_wide(self.lastRandomDirection) elif self.lastRandomCommand == self.turn: self.turn(self.lastRandomStepsWheelA, self.lastRandomStepsWheelB, self.lastRandomDirection)
def __init__(self): global isLightCalibrated global lowerBoundLight global upperBoundLight GPIO.setwarnings(False) # self.lock = threading.RLock() self.lastRandomCommand = None self.timesSameRandomCommandExecuted = 0 self.numberRepeatsRandomCommand = -1 self.lastRandomStepsWheelA = None self.lastRandomStepsWheelB = None self.lastRandomDirection = None isLightCalibrated = False lowerBoundLight = 0 upperBoundLight = 0 # Previous: [4, 17, 23, 24, 27, 22, 18, 5] motorPins = [13, 6, 5, 7, 20, 10, 9, 11] ledPins = [8, 16, 25, 12] # CREATING FILE WITH PID # PID of process pid = os.getpid() # ~/.tortoise_pids/ directory = os.path.expanduser("~") + "/.tortoise_pids/" # Filename: [PID].pid f = open(directory + str(pid) + ".pid", "w") # First line: motor pins f.write(str(motorPins[0]) + " " + str(motorPins[1]) + " " + str(motorPins[2]) + " " + str(motorPins[3]) + " " + str(motorPins[4]) + " " + str(motorPins[5]) + " " + str(motorPins[6]) + " " + str(motorPins[7]) + "\n") # Second line: LED pins f.write(str(ledPins[0]) + " " + str(ledPins[1]) + " " + str(ledPins[2]) + " " + str(ledPins[3]) + "\n") f.close() # ---------------------- # TODO: change to self.Motor.Left self.A = Motor(motorPins[0], motorPins[1], motorPins[2], motorPins[3]) self.B = Motor(motorPins[4], motorPins[5], motorPins[6], motorPins[7]) self.sensors = Sensors() self.actuators = Actuators() self.minDelayMotors = 2 self.state = enums.State.paused self.sensors.setSensor(enums.SensorType.light, 1, 17) # Previous: 16 self.sensors.setSensor(enums.SensorType.light, 2, 4) # Previous: 2 self.sensors.setSensor(enums.SensorType.emergencyStop, 1, 3) # Previous: 6 self.sensors.setSensor(enums.SensorType.touch, 1, 27) # Previous: 8 self.sensors.setSensor(enums.SensorType.touch, 2, 2) # Previous: 13 self.sensors.setSensor(enums.SensorType.touch, 3, 18) # Previous: 7 self.sensors.setSensor(enums.SensorType.proximity, 1, 19) # Previous: 10 self.sensors.setSensor(enums.SensorType.proximity, 2, 21) # Previous: 11 self.sensors.setSensor(enums.SensorType.proximity, 3, 22) # Previous: x self.sensors.setSensor(enums.SensorType.proximity, 4, 26) # Previous: x self.actuators.initActuator(enums.ActuatorType.led, 1, ledPins[0]) # Previous: 19 self.actuators.initActuator(enums.ActuatorType.led, 2, ledPins[1]) # Previous: 26 self.actuators.initActuator(enums.ActuatorType.led, 3, ledPins[2]) # Previous: x self.actuators.initActuator(enums.ActuatorType.led, 4, ledPins[3]) # Previous: x self.lastTouch = [-1,-1,-1] #print "light sensor value:" #print self.sensors.readSensor(enums.SensorType.light, 1) #if not isLightCalibrated: #self.calibrateLight() # try: # thread.start_new_thread(self.pauseAndResume, ()) # except: # print "Error: unable to start thread" messages.printMessage('greetings') while self.getSensorData(enums.SensorType.emergencyStop, 1) == 0: time.sleep(0.1) messages.printMessage('running') self.state = enums.State.running
class Tortoise: def __init__(self): global isLightCalibrated global lowerBoundLight global upperBoundLight GPIO.setwarnings(False) # self.lock = threading.RLock() self.lastRandomCommand = None self.timesSameRandomCommandExecuted = 0 self.numberRepeatsRandomCommand = -1 self.lastRandomStepsWheelA = None self.lastRandomStepsWheelB = None self.lastRandomDirection = None isLightCalibrated = False lowerBoundLight = 0 upperBoundLight = 0 # Previous: [4, 17, 23, 24, 27, 22, 18, 5] motorPins = [13, 6, 5, 7, 20, 10, 9, 11] ledPins = [8, 16, 25, 12] # CREATING FILE WITH PID # PID of process pid = os.getpid() # ~/.tortoise_pids/ directory = os.path.expanduser("~") + "/.tortoise_pids/" # Filename: [PID].pid f = open(directory + str(pid) + ".pid", "w") # First line: motor pins f.write(str(motorPins[0]) + " " + str(motorPins[1]) + " " + str(motorPins[2]) + " " + str(motorPins[3]) + " " + str(motorPins[4]) + " " + str(motorPins[5]) + " " + str(motorPins[6]) + " " + str(motorPins[7]) + "\n") # Second line: LED pins f.write(str(ledPins[0]) + " " + str(ledPins[1]) + " " + str(ledPins[2]) + " " + str(ledPins[3]) + "\n") f.close() # ---------------------- # TODO: change to self.Motor.Left self.A = Motor(motorPins[0], motorPins[1], motorPins[2], motorPins[3]) self.B = Motor(motorPins[4], motorPins[5], motorPins[6], motorPins[7]) self.sensors = Sensors() self.actuators = Actuators() self.minDelayMotors = 2 self.state = enums.State.paused self.sensors.setSensor(enums.SensorType.light, 1, 17) # Previous: 16 self.sensors.setSensor(enums.SensorType.light, 2, 4) # Previous: 2 self.sensors.setSensor(enums.SensorType.emergencyStop, 1, 3) # Previous: 6 self.sensors.setSensor(enums.SensorType.touch, 1, 27) # Previous: 8 self.sensors.setSensor(enums.SensorType.touch, 2, 2) # Previous: 13 self.sensors.setSensor(enums.SensorType.touch, 3, 18) # Previous: 7 self.sensors.setSensor(enums.SensorType.proximity, 1, 19) # Previous: 10 self.sensors.setSensor(enums.SensorType.proximity, 2, 21) # Previous: 11 self.sensors.setSensor(enums.SensorType.proximity, 3, 22) # Previous: x self.sensors.setSensor(enums.SensorType.proximity, 4, 26) # Previous: x self.actuators.initActuator(enums.ActuatorType.led, 1, ledPins[0]) # Previous: 19 self.actuators.initActuator(enums.ActuatorType.led, 2, ledPins[1]) # Previous: 26 self.actuators.initActuator(enums.ActuatorType.led, 3, ledPins[2]) # Previous: x self.actuators.initActuator(enums.ActuatorType.led, 4, ledPins[3]) # Previous: x self.lastTouch = [-1,-1,-1] #print "light sensor value:" #print self.sensors.readSensor(enums.SensorType.light, 1) #if not isLightCalibrated: #self.calibrateLight() # try: # thread.start_new_thread(self.pauseAndResume, ()) # except: # print "Error: unable to start thread" messages.printMessage('greetings') while self.getSensorData(enums.SensorType.emergencyStop, 1) == 0: time.sleep(0.1) messages.printMessage('running') self.state = enums.State.running def getStateTortoise(self): return self.state def setStateTortoise(self, toState): self.state = toState def calibrateLight(self): global lowerBoundLight, upperBoundLight, isLightCalibrated messages.printMessage('calibration_ambient') raw_input() #lowerBoundLight = max(self.sensors.readSensor(enums.SensorType.light, 1), self.sensors.readSensor(enums.SensorType.light, 2)) lowerBoundLight = self.sensors.readSensor(enums.SensorType.light, 1) #print "Light in normal conditions is: ", lowerBoundLight messages.printMessage('calibration_light_source') raw_input() #upperBoundLight = min((self.sensors.readSensor(enums.SensorType.light, 1), self.sensors.readSensor(enums.SensorType.light, 2))) upperBoundLight = self.sensors.readSensor(enums.SensorType.light, 1) # print "Light when there is a light source is:", upperBoundLight isLightCalibrated = True messages.printMessage('calibration_complete') def getSensorData(self, sensor_type, position): if (sensor_type == enums.SensorType.touch): if (position < 1 or position > 3): messages.printMessage('bad_touch_sensor') self.blinkLEDs_error() return -1 elif (sensor_type == enums.SensorType.light): if (position != 1 and position!=2): messages.printMessage('bad_light_sensor') self.blinkLEDs_error() return -1 elif (sensor_type == enums.SensorType.proximity): if (position < 1 or position > 4): messages.printMessage('bad_proximity_sensor') self.blinkLEDs_error() return -1 elif (sensor_type == enums.SensorType.emergencyStop): if (position != 1): messages.printMessage('bad_emergency_sensor') self.blinkLEDs_error() return -1 else: messages.printMessage('bad_sensor') self.blinkLEDs_error() return -1 value = self.sensors.readSensor(sensor_type, position) if sensor_type == enums.SensorType.light: return value if (upperBoundLight - lowerBoundLight) == 0: messages.printMessage('no_calibration') self.blinkLEDs_error() return -1 # Scale lightVal = int(9 - round(abs(value-upperBoundLight)/(abs(upperBoundLight - lowerBoundLight)/9))) if lightVal < 0: messages.printMessage('no_calibration') self.blinkLEDs_error() return -1 return lightVal elif sensor_type == enums.SensorType.touch: return self.getSwitchTriggered(position,value) elif sensor_type == enums.SensorType.emergencyStop: return value % 2 else: return value def getSwitchTriggered(self, position, value): if self.lastTouch[position-1]<0: self.lastTouch[position-1]=value return 0 elif self.lastTouch[position-1]==value: return 0 else: self.lastTouch[position-1]=value return 1 def getLEDValue(self, position): if (position < 1 or position > 4): messages.printMessage('bad_LED') self.blinkLEDs_error() return -1 return self.actuators.getActuatorValue(enums.ActuatorType.led, position) def setLEDValue(self, position, value): if(position < 1 or position > 4): messages.printMessage('bad_LED') self.blinkLEDs_error() return -1 if(value != 0 and value != 1): messages.printMessage('bad_LED_value') self.blinkLEDs_error() return -1 self.actuators.setActuatorValue(enums.ActuatorType.led, position, value) return 0 def blinkLEDs(self, positions, numberOfBlinks, delay, blocking = False): if numberOfBlinks < 0: messages.printMessage('blinks_negative') self.blinkLEDs_error() return -1 if numberOfBlinks == 0: messages.printMessage('blinks_zero') self.blinkLEDs_error() return -1 if delay < 0: messages.printMessage('blinking_fast') self.blinkLEDs_error() return -1 try: for y in range(0, len(positions)): if positions[y] < 0 or positions[y] > 4: messages.printMessage('bad_LED') self.blinkLEDs_error() return -1 except TypeError: # It's not an array but an integer if positions < 0 or positions > 4: messages.printMessage('bad_LED') self.blinkLEDs_error() return -1 previousStateLEDs = [ self.getLEDValue(x) for x in range(1, 5) ] cont = True # Infinite loop to "stop" the execution of the program and keep blinkind the LEDs while cont: for x in range(0, numberOfBlinks): try: for y in range(0, len(positions)): self.actuators.setActuatorValue(enums.ActuatorType.led, positions[y], 1) time.sleep(delay) for y in range(0, len(positions)): self.actuators.setActuatorValue(enums.ActuatorType.led, positions[y], 0) time.sleep(delay) except TypeError: # It's not an array but an integer self.actuators.setActuatorValue(enums.ActuatorType.led, positions, 1) time.sleep(delay) self.actuators.setActuatorValue(enums.ActuatorType.led, positions, 0) time.sleep(delay) cont = blocking # If it doesn't block, the previous state of the LEDs is restored for x in range(1, 5): self.setLEDValue(x, previousStateLEDs[x - 1]) return 0 def blinkLEDs_error(self): self.blinkLEDs([1, 2, 3, 4], 3, 0.2, blocking = True) def moveMotors(self, stepsWheelA, stepsWheelB, delayWheelA, delayWheelB, direction): if( direction != enums.Direction.backwards_right and direction != enums.Direction.backwards_left and direction != enums.Direction.forwards_right and direction != enums.Direction.forwards_left and direction != enums.Direction.forwards and direction != enums.Direction.backwards and direction != enums.Direction.clockwise and direction != enums.Direction.counterClockwise ) : messages.printMessage('bad_direction') self.blinkLEDs_error() return -1 if(stepsWheelA < 0 or stepsWheelB < 0): messages.printMessage('bad_steps') self.blinkLEDs_error() return -1 if((stepsWheelA > 0 and delayWheelA < self.minDelayMotors) or (stepsWheelB > 0 and delayWheelB < self.minDelayMotors)): messages.printMessage('bad_delay') self.blinkLEDs_error() return -1 # If a stop command has been sent, the turtle will stop its movement if self.getSensorData(enums.SensorType.emergencyStop, 1) == 0: if self.getStateTortoise() == enums.State.running: self.setStateTortoise(enums.State.paused) messages.printMessage('paused') else: if self.getStateTortoise() == enums.State.paused: self.setStateTortoise(enums.State.running) messages.printMessage('resumed') motorAprocess_backwards = Process(target=self.A.backwards, args=(delayWheelA / 1000.00, stepsWheelA)) motorBprocess_backwards = Process(target=self.B.backwards, args=(delayWheelB / 1000.00, stepsWheelB)) motorAprocess_forwards = Process(target=self.A.forwards, args=(delayWheelA / 1000.00, stepsWheelA)) motorBprocess_forwards = Process(target=self.B.forwards, args=(delayWheelB / 1000.00, stepsWheelB)) if direction == enums.Direction.backwards_left or direction == enums.Direction.backwards or direction == enums.Direction.backwards_right: if stepsWheelA > 0: motorAprocess_backwards.start() if stepsWheelB > 0: motorBprocess_backwards.start() elif direction == enums.Direction.forwards_right or direction == enums.Direction.forwards or direction == enums.Direction.forwards_left: if stepsWheelA > 0: motorAprocess_forwards.start() if stepsWheelB > 0: motorBprocess_forwards.start() elif direction == enums.Direction.clockwise: if stepsWheelA > 0: motorAprocess_backwards.start() if stepsWheelB > 0: motorBprocess_forwards.start() elif direction == enums.Direction.counterClockwise: if stepsWheelA > 0: motorAprocess_forwards.start() if stepsWheelB > 0: motorBprocess_backwards.start() # The main loop pools the emergencyStop while motorAprocess_backwards.is_alive() or motorBprocess_backwards.is_alive() or motorAprocess_forwards.is_alive() or motorBprocess_forwards.is_alive(): # If a stop command has been sent, the turtle will stop its movement if self.getSensorData(enums.SensorType.emergencyStop, 1) == 0: if self.getStateTortoise() == enums.State.running: self.setStateTortoise(enums.State.paused) messages.printMessage('paused') if motorAprocess_backwards.is_alive(): motorAprocess_backwards.terminate() motorAprocess_backwards.join() if motorBprocess_backwards.is_alive(): motorBprocess_backwards.terminate() motorBprocess_backwards.join() if motorAprocess_forwards.is_alive(): motorAprocess_forwards.terminate() motorAprocess_forwards.join() if motorBprocess_forwards.is_alive(): motorBprocess_forwards.terminate() motorBprocess_forwards.join() elif self.getStateTortoise() == enums.State.paused: self.setStateTortoise(enums.State.running) messages.printMessage('resumed') time.sleep(0.5) self.A.stopMotors() self.B.stopMotors() return 0 def moveForwards(self, steps): return self.moveMotors(steps, steps, self.minDelayMotors, self.minDelayMotors, enums.Direction.forwards) def moveBackwards(self, steps): return self.moveMotors(steps, steps, self.minDelayMotors, self.minDelayMotors, enums.Direction.backwards) def turnOnTheSpot(self, steps, direction): if(steps < 0): messages.printMessage('bad_steps') self.blinkLEDs_error() return -1 if( direction != enums.Direction.backwards_right and direction != enums.Direction.backwards_left and direction != enums.Direction.forwards_right and direction != enums.Direction.forwards_left ) : messages.printMessage('bad_direction_turn') self.blinkLEDs_error() return -1 if direction == enums.Direction.backwards_right or direction == enums.Direction.forwards_right: return self.moveMotors(steps, 0, self.minDelayMotors, 0, direction) elif direction == enums.Direction.backwards_left or direction == enums.Direction.forwards_left: return self.moveMotors(0, steps, 0, self.minDelayMotors, direction) def shuffleOnTheSpot(self, steps, direction): if(steps < 0): messages.printMessage('bad_steps') self.blinkLEDs_error() return -1 if( direction != enums.Direction.clockwise and direction != enums.Direction.counterClockwise ) : messages.printMessage('bad_shuffle') self.blinkLEDs_error() return -1 return self.moveMotors(steps, steps, self.minDelayMotors, self.minDelayMotors, direction) def turn(self, stepsWheelA, stepsWheelB, direction): if( direction != enums.Direction.backwards_right and direction != enums.Direction.backwards_left and direction != enums.Direction.forwards_right and direction != enums.Direction.forwards_left ) : messages.printMessage('bad_direction_turn') self.blinkLEDs_error() return -1 if (direction == enums.Direction.backwards_right or direction == enums.Direction.forwards_right) and (stepsWheelB >= stepsWheelA): messages.printMessage('bad_turn') self.blinkLEDs_error() return -1 if (direction == enums.Direction.backwards_left or direction == enums.Direction.forwards_left) and (stepsWheelA >= stepsWheelB): messages.printMessage('bad_turn') self.blinkLEDs_error() return -1 if(stepsWheelA < 0 or stepsWheelB < 0): messages.printMessage('bad_steps') self.blinkLEDs_error() return -1 if direction == enums.Direction.backwards_right or direction == enums.Direction.forwards_right: delay = (stepsWheelA * self.minDelayMotors) / stepsWheelB return self.moveMotors(stepsWheelA, stepsWheelB, self.minDelayMotors, delay, direction) elif direction == enums.Direction.backwards_left or direction == enums.Direction.forwards_left: delay = (stepsWheelB * self.minDelayMotors) / stepsWheelA return self.moveMotors(stepsWheelA, stepsWheelB, delay, self.minDelayMotors, direction) def doRandomMovement2(self): maxTimesCommandRepeated = 3 # New random command if self.numberRepeatsRandomCommand == -1 or self.timesSameRandomCommandExecuted == self.numberRepeatsRandomCommand: self.numberRepeatsRandomCommand = np.random.randint(maxTimesCommandRepeated + 1) self.timesSameRandomCommandExecuted = 0 # Random number between 30 and 180 numberOfSteps = np.random.randint(30, 180) self.lastRandomStepsWheelA = numberOfSteps self.lastRandomStepsWheelB = numberOfSteps # Random number between 0 and 1 randomNumber = np.random.random_sample() if(randomNumber < 0.4): if(randomNumber < 0.2): self.moveForwards(numberOfSteps) self.lastRandomCommand = self.moveForwards else: self.moveBackwards(numberOfSteps) self.lastRandomCommand = self.moveBackwards else: # Random enums.Direction: left of right if(np.random.random_sample() < 0.5): direction = enums.Direction.forwards_left else: direction = enums.Direction.forwards_right self.lastRandomDirection = direction if(randomNumber < 0.7): self.turnOnTheSpot(numberOfSteps, direction) else: self.turnOnTheSpot(numberOfSteps, direction) self.lastRandomCommand = self.turnOnTheSpot # Repeat last command else: self.timesSameRandomCommandExecuted = self.timesSameRandomCommandExecuted + 1 # TODO: change wheel A/B! if self.lastRandomCommand == self.moveForwards: self.moveForwards(self.lastRandomStepsWheelA) elif self.lastRandomCommand == self.moveBackwards: self.moveBackwards(self.lastRandomStepsWheelA) elif self.lastRandomCommand == self.turnOnTheSpot: self.turnOnTheSpot(self.lastRandomStepsWheelA, self.lastRandomDirection) def doRandomMovement(self): # Random number between 30 and (509/4 + 30) numberOfSteps = int(509/4*np.random.random_sample() + 30) # Random number between 0 and 1 randomNumber = np.random.random_sample() if(randomNumber < 0.4): if(randomNumber < 0.2): self.moveForwards(numberOfSteps) else: self.moveBackwards(numberOfSteps) else: # Random enums.Direction: left of right if(np.random.random_sample() < 0.5): direction = enums.Direction.forwards_left else: direction = enums.Direction.forwards_right if(randomNumber < 0.7): self.turnOnTheSpot(numberOfSteps, direction) else: self.turnOnTheSpot(numberOfSteps, direction)
if __name__ == '__main__': try: pygame.mixer.init() if str(sys.argv)[0] == "init": state = 0 tail_alt = True tail_angle = 0 else: f = open("state.txt", "r") contents = f.read() state = contents.split("\n")[0] tail_alt = contents.split("\n")[1] tail_angle = contents.split("\n")[2] robot = Robot(int(state), int(tail_angle.split(":")[1]), bool( tail_alt.split(":")[1])) actuators = Actuators(vibration_motor_pin, servo_motor, robot) sensors = Sensors(left_capacitive_touch_sensor_pin, right_capacitive_touch_sensor_pin, robot, actuators) audio = Audio(robot) robot.start() read_touch_sensors_thread = threading.Thread( target=sensors.read_back_touch_sensors()) thread_state["touch_sensor_thread"] = read_touch_sensors_thread read_touch_sensors_thread.start() except KeyboardInterrupt: f = open("state.txt", "w") f.write("state: "+str(robot.get_state())) f.write("\ntail_alternate: "+str(robot.get_tail_alternates())) f.write("\ntail_angle: "+str(robot.get_tail_angle())) f.close()
from actuators import Actuators import time if __name__ == '__main__': actuators = Actuators() time.sleep(10) # wait 10 seconds for everything else to start up while True: actuators.run() time.sleep(1)