from Sensor import Sensor
from PinMaster import PinMaster
from Parameters import Parameters
# ----------------------------------------------------
# INITIALIZE GLOBAL VARIABLES
# ----------------------------------------------------
# Serial communication with Arduino
ser = serial.Serial('/dev/ttyACM0', 9600)
# Create Parameters object
params = Parameters()
# Add Navigation Parameters (name, value, description)
params.addParam('DATA_SAMPLE_SIZE', 3, 'Sample size for data (increase to stabilize at cost of speed)')
params.addParam('MAX_ITER', 10, 'Maximum number of Sense-Plan-Act Cycles')
params.addParam('MOVEMENT_WEIGHT', 5.0, 'Weight factor for movement vector vs move units to be traveled')
params.addParam('TIMEOUT', 0.1, 'How many seconds until sensor loop times out and returns a bunch of zeros')
params.addParam('FORWARD_VECTOR', [0, 1, 0], 'Describes the forward (Theta = 0) direction in the robot frame')
params.addParam('BACKWARD_VECTOR', [0, -1, 0], 'Describes the backwards (Theta = 0) direction in the robot frame')
params.addParam('WAIT_TIME', 2, 'How many seconds in between exploration steps')
# Add Tuning Parameters
params.addParam('DISTANCE_WEIGHT', [0.1, 0.1, 0.2],
'Weighting of [X, Y, Theta] each when determining distance metric')
# Add Motor Parameters
params.addParam('SEC_PER_TURN', 2.8, 'Seconds required to turn 1 unit')
params.addParam('SEC_PER_MOVE', 1.0, 'Seconds required to move 1 unit')
params.addParam('DIST_PER_MOVE', 10.0, 'Centimeters in 1 move unit')
import serial
import numpy as np
from Sensor import Sensor
from PinMaster import PinMaster
from Parameters import Parameters
import math
import sys
# Serial communication with Arduino
ser = serial.Serial('/dev/ttyACM0', 9600)
# Create Parameters object
params = Parameters()
# Add Navigation Parameters (name, value, description)
params.addParam('DATA_SAMPLE_SIZE', 3, 'Sample size for data (increase to stabilize at cost of speed)')
params.addParam('MAX_ITER', 2, 'Maximum number of Sense-Plan-Act Cycles')
params.addParam('MOVE_PER_TURN', 1, 'How far you want the robot to move each step (increments of 10cm)')
params.addParam('TIMEOUT', 0.1, 'How many seconds until sensor loop times out and returns a bunch of zeros')
params.addParam('FORWARD_VECTOR', [0, 1, 0], 'Describes the forward (Theta = 0) direction in the robot frame')
# Add SLAM Parameters (name, value, description)
params.addParam('RAND_DIST_MU', 0, 'Center of distribution (cm)')
params.addParam('RAND_DIST_SIGMA', 1, 'Standard deviation (cm)')
params.addParam('RAND_ANG_MU', 0, 'Degrees')
params.addParam('RAND_ANG_SIGMA', 10, 'Degrees')
params.addParam('RAND_NUM', 10, 'Number of random samples')
# Add Kalman Filter Parameters
params.addParam('OBSERVATION_NOISE', 0.1, 'Kalman Filter observation noise')
