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
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)
