def __init__(self, name, init_xy, init_dir, init_battery, \
noise_vel, noise_turn, noise_radio, noise_mag, fail_prob, controller, case=None):
'''
Params: length in no. of cells
breadth in no. of cells
'''
self.name = name
self.xy = [float(i) for i in init_xy] # In meters
self.dir = float(init_dir)
self.mag_dir = float(init_dir)
self.is_alive = True
self.is_moving = False
self.is_backing_off = False
self.has_turned = False
self.command = Command(0,0,0)
self.last_command = Command(0,0,0)
self.__real_command = Command(0,0,0)
self.command_time_cnt = 0
self.noise_velocity = float(noise_vel)
self.noise_turn = float(noise_turn)
self.noise_radio = float(noise_radio)
self.noise_mag = float(noise_mag)
self.noise_fingerprint = [[self.noise_radio/2,0],[0,self.noise_radio/2]]
self.odometer = Odometer(self.noise_velocity, self.noise_turn)
self.radio = Radio(self)
self.mag = Magnetometer(self)
self.has_collided = False
self.is_goal_reached = False
self.fail_prob = fail_prob
self.collision_count = 0
# Records estimated location
self.pf_estimated_xy = np.zeros([1, 2], dtype=np.float32)
self.dr_estimated_xy = np.zeros([1, 2], dtype=np.float32)
self.pf_particles_xy = np.ones([100, 2], dtype=float) * self.xy
self.sig_match_cnt = 0
# Add references to the central controller
self.controller = controller
self.last_goal_dir = np.zeros([1, 2], dtype=np.float32)
self.backoff_time_cnt = 0
if case is not None:
self.sig_db = case.rf_signature_db
class SensorFly(object):
'''
SensorFly node
'''
def __init__(self, name, init_xy, init_dir, init_battery, \
noise_vel, noise_turn, noise_radio, noise_mag, fail_prob, controller, case=None):
'''
Params: length in no. of cells
breadth in no. of cells
'''
self.name = name
self.xy = [float(i) for i in init_xy] # In meters
self.dir = float(init_dir)
self.mag_dir = float(init_dir)
self.is_alive = True
self.is_moving = False
self.is_backing_off = False
self.has_turned = False
self.command = Command(0,0,0)
self.last_command = Command(0,0,0)
self.__real_command = Command(0,0,0)
self.command_time_cnt = 0
self.noise_velocity = float(noise_vel)
self.noise_turn = float(noise_turn)
self.noise_radio = float(noise_radio)
self.noise_mag = float(noise_mag)
self.noise_fingerprint = [[self.noise_radio/2,0],[0,self.noise_radio/2]]
self.odometer = Odometer(self.noise_velocity, self.noise_turn)
self.radio = Radio(self)
self.mag = Magnetometer(self)
self.has_collided = False
self.is_goal_reached = False
self.fail_prob = fail_prob
self.collision_count = 0
# Records estimated location
self.pf_estimated_xy = np.zeros([1, 2], dtype=np.float32)
self.dr_estimated_xy = np.zeros([1, 2], dtype=np.float32)
self.pf_particles_xy = np.ones([100, 2], dtype=float) * self.xy
self.sig_match_cnt = 0
# Add references to the central controller
self.controller = controller
self.last_goal_dir = np.zeros([1, 2], dtype=np.float32)
self.backoff_time_cnt = 0
if case is not None:
self.sig_db = case.rf_signature_db
def setMoveCommand(self, command_params):
'''
Params: turn - the angle to turn in degrees
time - the time to setMoveCommand in seconds
'''
self.is_moving = True
[turn, time, velocity] = command_params
# Set the command_params to be executed now
self.command.time = float(time)
self.command.turn = float(turn)
self.command.velocity = float(velocity)
self.__real_command = copy.deepcopy(self.command)
self.__real_command.velocity = self.odometer.velocity(velocity)
self.__real_command.turn = self.odometer.turn(turn)
self.command_time_cnt = self.__real_command.time
# Store the last executed command_params
self.last_command = copy.deepcopy(self.__real_command)
def update(self, deltick, arena):
'''
Params: deltick - the time tick in seconds
'''
# Move
if self.is_moving:
# Turn instantly in first tick
if (self.command_time_cnt == self.__real_command.time):
self.dir = (self.dir + self.__real_command.turn) % 360
self.has_turned = True
else:
self.has_turned = False
# Get the magnetometer reading for direction
self.mag_dir = self.mag.getDirection()
# Set the time taking into account the tick size
if (self.command_time_cnt - deltick <= 0):
deltime = self.command_time_cnt
self.command_time_cnt = 0
else:
self.command_time_cnt = self.command_time_cnt - deltick
deltime = deltick
oldpos = self.xy
newx = self.xy[0] + ((self.__real_command.velocity * deltime) * \
math.cos(math.radians(self.dir)))
newy = self.xy[1] + ((self.__real_command.velocity * deltime) * \
math.sin(math.radians(self.dir)))
newpos = [newx, newy]
is_collision = self.isCollision(oldpos,newpos,arena)
# If no collision setMoveCommand to new point
if is_collision == False:
self.xy = [newx, newy]
else: # Else stop at object boundary
self.is_moving = False
self.command_time_cnt = 0
self.is_backing_off = False
# If the movement is complete
if (self.command_time_cnt == 0):
self.is_moving = False
self.is_backing_off = False
else:
deltime = 0
def isCollision(self, oldpos, newpos, arena):
'''
Compute if collision
'''
[oldX, oldY] = arena.gridmap.xytocell(oldpos)
[newX, newY] = arena.gridmap.xytocell(newpos)
covCells = self.bresenhamLine(oldX, oldY, newX, newY)
for c in covCells:
if arena.gridmap.map[c[0]][c[1]] == arena.gridmap.v_obstacle: # hit obstacle
self.has_collided = True
self.collision_count += 1
p = random.random()
if (p < self.fail_prob):
self.is_alive = False
return True
self.has_collided = False
return False
def bresenhamLine(self,x,y,x2,y2):
# Brensenham line algorithm
steep = 0
coords = []
dx = abs(x2 - x)
if (x2 - x) > 0: sx = 1
else: sx = -1
dy = abs(y2 - y)
if (y2 - y) > 0: sy = 1
else: sy = -1
if dy > dx:
steep = 1
x,y = y,x
dx,dy = dy,dx
sx,sy = sy,sx
d = (2 * dy) - dx
for _ in range(0,dx):
if steep: coords.append((y,x))
else: coords.append((x,y))
while d >= 0:
y = y + sy
d = d - (2 * dx)
x = x + sx
d = d + (2 * dy)
coords.append((x2,y2))
return coords
def getRadioSignature(self):
# anchors = [0,1,2,3,5,6]
# signature = np.zeros(6,np.float32)
# for i,a in enumerate(anchors):
# [x,y] = self.xytocell(self.xy)
# signature[i] = np.random.choice(self.sig_db[(y,x,a)])
signature = np.random.multivariate_normal(self.xy, self.noise_fingerprint)
# self.radio.rss(self.controller.anchors)
return signature
def xytocell(self, xy):
X = int(round(xy[0]))
Y = int(round(xy[1]))
if X < 0:
X = 0
elif X >= 10:
X = 10-1
if Y < 0:
Y=0
elif Y >= 7:
Y=7-1
return [X,Y]
def __init__(self, magnetometer=None):
if magnetometer == None:
self.magnetometer = Magnetometer()
else:
self.magnetometer = magnetometer
self.x = []
self.y = []
self.z = []
def __init__(self, magnetometer=None):
"""
Creates a DataLogger instance, optionally uses a Magnetometer instance if given, otherwises creates a new
instance
:param magnetometer: the optional magnetometer instance it will use to record data
"""
if magnetometer == None:
self.magnetometer = Magnetometer()
else:
self.magnetometer = magnetometer
self.data = []
class DataLogger:
"""Logs data from the magenetomer for implementing the calibration levels"""
def __init__(self, magnetometer=None):
if magnetometer == None:
self.magnetometer = Magnetometer()
else:
self.magnetometer = magnetometer
self.x = []
self.y = []
self.z = []
def measure(self, num_readings, time_ms_per_reading=0):
"""time per reading must be more than 7 ms"""
if (time_ms_per_reading > 7):
time_sleep = time_ms_per_reading - 7
for i in range(num_readings + 2):
self.magnetometer.take_measurement()
if i > 1:
self.x.append(self.magnetometer.x)
self.y.append(self.magnetometer.y)
self.z.append(self.magnetometer.z)
sleep_ms(time_sleep)
else:
for i in range(num_readings):
self.magnetometer.take_measurement()
if i > 1:
self.x.append(self.magnetometer.x)
self.y.append(self.magnetometer.y)
self.z.append(self.magnetometer.z)
def get_json(self):
data["x"] = self.x
data["y"] = self.y
data["z"] = self.z
return ujson.dumps(data)
def std_dev(self):
mean = sum(self.z) / len(self.z)
z = math.sqrt(
sum(map(lambda x: (x - mean) * (x - mean), self.z)) / len(self.z))
mean = sum(self.y) / len(self.y)
y = math.sqrt(
sum(map(lambda x: (x - mean) * (x - mean), self.y)) / len(self.y))
mean = sum(self.x) / len(self.x)
x = math.sqrt(
sum(map(lambda x: (x - mean) * (x - mean), self.x)) / len(self.x))
return (x, y, z)
def main():
dt = 0.01
LEFT_MOTOR_INPUT = (17, 27)
RIGHT_MOTOR_INPUT = (5, 6)
LEFT_ENCODER_INPUT = {'hall_sensor_A': 23, 'hall_sensor_B': 24, 'ticks_per_revolution': 2400}
RIGHT_ENCODER_INPUT = {'hall_sensor_A': 13, 'hall_sensor_B': 19, 'ticks_per_revolution': 2350}
left_motor = Motor(LEFT_MOTOR_INPUT[0], LEFT_MOTOR_INPUT[1])
right_motor = Motor(RIGHT_MOTOR_INPUT[0], RIGHT_MOTOR_INPUT[1])
left_encoder = QuadratureEncoder(LEFT_ENCODER_INPUT['ticks_per_revolution'], LEFT_ENCODER_INPUT['hall_sensor_A'], LEFT_ENCODER_INPUT['hall_sensor_B'])
right_encoder = QuadratureEncoder(RIGHT_ENCODER_INPUT['ticks_per_revolution'], RIGHT_ENCODER_INPUT['hall_sensor_A'], RIGHT_ENCODER_INPUT['hall_sensor_B'])
gps = GPS()
magnetometer = Magnetometer()
robot = Robot(left_motor, right_motor, left_encoder, right_encoder, gps, magnetometer)
controller = PIDController(robot)
target = SphericalPoint(-12.016592, -77.049883 )
# target = SphericalPoint(-12.016679333333334,-77.05032666666666)
# target = SphericalPoint(-12.016824833333333,-77.04993633333333)
# target = SphericalPoint(-12.016822250210852, -77.04970762346649
rover_manager = RoverManager(robot, controller, target)
print(target.toENU(robot.reference))
# rover_image_manager = ImageProcessing(robot)
# rover_image_manager.execute()
epoch = 0
while(True):
print("epoch: %d" %epoch)
gps_enabled = (epoch > 0 and (epoch % 100 == 0))
rover_manager.execute_with_filter(gps_enabled = gps_enabled)
epoch += 1
for i in range (0,Nf):
voltage[i,:] = PWM(0, tf, dt, V, freq[i], duty).signal #[i,:], filas
v_rms[i] = np.sqrt(np.mean(voltage[i,:]**2)) #rms root mean sqr value
v_med[i] = np.mean(voltage[i,:])
# Sensors
# Electric current
i_std = 22e-5 #[A]
i_bias = 22e-6 #[A]
current_sensor = CurrentSensor(i_bias, i_std)
# Magnetometer
B_std = 1 #[uT]
B_bias = 1e-1 #[uT]
magnetometer = Magnetometer(B_bias, B_std)
z = 16e-3 #[m]
# MTQ Data Storage #define variables para guardar datos dsp, se inician todos los datos en cero
i_data = np.zeros((Nf, N))
m_data = np.zeros((Nf, N))
B_data = np.zeros((Nf, N))
m_calc = np.zeros((Nf, N))
i_rms = np.zeros(Nf)
m_rms = np.zeros(Nf)
B_rms = np.zeros(Nf)
m_calc_rms = np.zeros(Nf)
i_med = np.zeros(Nf)
class DataLogger:
"""
Logs data from the magenetometer
"""
def __init__(self, magnetometer=None):
"""
Creates a DataLogger instance, optionally uses a Magnetometer instance if given, otherwises creates a new
instance
:param magnetometer: the optional magnetometer instance it will use to record data
"""
if magnetometer == None:
self.magnetometer = Magnetometer()
else:
self.magnetometer = magnetometer
self.data = []
def measure(self, num_readings, time_ms_per_reading=0):
"""
Takes reading from the magnetometer and scales to the correct units
:param num_readings: number of readings to take
:param time_ms_per_reading: time to spend taking each reading
:return: no return value
"""
self.data = []
"""time per reading must be more than 7 ms"""
if (time_ms_per_reading > 7):
time_sleep = time_ms_per_reading - 7
for i in range(num_readings + 2):
self.magnetometer.take_measurement()
if i > 1:
vector = Vector3D()
vector.x = self.magnetometer.x
vector.y = self.magnetometer.y
vector.z = self.magnetometer.z
self.data.append(vector)
sleep_ms(time_sleep)
else:
for i in range(num_readings):
self.magnetometer.take_measurement()
if i > 1:
vector = Vector3D()
vector.x = self.magnetometer.x
vector.y = self.magnetometer.y
vector.z = self.magnetometer.z
self.data.append(vector)
if num_readings >= 10:
self.data = self.data[5:]
def get_json(self):
"""
Converts recorded magnetometer data to JSON
:return: JSON object containing magnetometer readings
"""
return ujson.dumps(self.data)
def mean(self):
"""
Calculates the mean of the recorded data
:return: Vector3D instance of the mean value recorded in each axis
"""
if self.data == []:
print("Take measurement first using measure()")
return 0
self.data_mean = Vector3D()
for i in self.data:
self.data_mean = self.data_mean + i
self.data_mean = self.data_mean.mul(1 / len(self.data))
return self.data_mean
def std_dev(self):
"""
Calculate the standard deviation of the recorded data
:return: Vector3D instance of the std deviation value recorded in each axis
"""
self.mean()
res = Vector3D()
for i in range(len(self.data)):
res = res + (self.data[i] - self.data_mean) * (self.data[i] -
self.data_mean)
res = res.mul(1 / len(self.data))
res = res.sqrt()
self.data_std_dev = res
return res
from magnetometer import Magnetometer
from networking import WiFi
from networking import MQTTManager
import time
from calibration import DataLogger
wifi_conn = WiFi() #establishes a wifi connection
magnetometer = Magnetometer()
mqttmanager = MQTTManager()
logger = DataLogger(magnetometer)
while (True):
logger.measure()
mqttmanager.publish("SensorData", logger.get_json())
time.sleep_ms(5000)
