def __init__(self,
boat,
destination,
positionThreshold=1.0,
controller_name="PointAndShoot"):
super(LineFollower, self).__init__(boat)
self._x0 = boat.state[0]
self._x1 = destination[0]
self._y0 = boat.state[1]
self._y1 = destination[1]
self._dx = self._x1 - self._x0
self._dy = self._y1 - self._y0
self._th = np.arctan2(self._dy, self._dx)
self._L = np.sqrt(np.power(self._dx, 2.) + np.power(self._dy, 2.))
self._destination = destination
self._lookAhead = 5.0
self._positionThreshold = positionThreshold
if controller_name == "PointAndShoot":
THRUST_PID = [0.15, 0, 0] #[0.5, 0.01, 10.00] # P, I, D
HEADING_PID = [0.1, 0, 0] #[1.0, 0.0, 1.0] # P, I, D
HEADING_ERROR_SURGE_CUTOFF_ANGLE = 180.0 # [degrees of heading error at which thrust is forced to be zero, follows a half-cosine shape]
self.controller = Controllers.PointAndShootPID(
boat, THRUST_PID, HEADING_PID,
HEADING_ERROR_SURGE_CUTOFF_ANGLE, positionThreshold)
elif controller_name == "QLearnPointAndShoot":
self.controller = Controllers.QLearnPointAndShoot(boat)
def main():
evManager = EventManager.EventManager()
# Initialize important controllers/listeners
keybd = Controllers.KeyboardController(evManager)
spinner = Controllers.CPUSpinnerController(evManager)
pygameView = Views.PygameView(evManager)
# --------------------------
game = Game(evManager)
# Temp Test Code
#pygameView.SetTrack(game.charactors[0])
# -------------
spinner.Run()
def __init__(self,
boat,
destination,
positionThreshold=1.0,
controller_name="PointAndShoot"):
super(DestinationOnly, self).__init__(boat)
self._destinationState = destination
if controller_name == "PointAndShoot":
THRUST_PID = [0.5, 0, 0] #[0.5, 0.01, 10.00] # P, I, D
HEADING_PID = [0.7, 0, 0] #[1.0, 0.0, 1.0] # P, I, D
HEADING_ERROR_SURGE_CUTOFF_ANGLE = 180.0 # [degrees of heading error at which thrust is forced to be zero, follows a half-cosine shape]
self.controller = Controllers.PointAndShootPID(
boat, THRUST_PID, HEADING_PID,
HEADING_ERROR_SURGE_CUTOFF_ANGLE, positionThreshold)
elif controller_name == "QLearnPointAndShoot":
self.controller = Controllers.QLearnPointAndShoot(boat)
def doStep():
state.step += 1
if setting.verbose:
print("step", state.step)
traci.simulationStep()
# adding new vehicles, if any
departed = traci.simulation.getSubscriptionResults()[
tc.VAR_DEPARTED_VEHICLES_IDS]
for v in departed:
traci.vehicle.subscribe(v, dataFromTraciState)
vehicleSetParams[v] = getSetParams(v)
if v == 'ego':
thisv = egov
else:
thisv = alterv
controllers[v] = Controllers.Inter1Constant(v, thisv)
#getController(v, vehicleSetParams[v])
if controllers[v] is not None:
if setting.verbose:
print("allowing forward and lane crashes for", v)
traci._sendIntCmd(tc.CMD_SET_VEHICLE_VARIABLE, tc.VAR_SPEEDSETMODE,
v, controllers[v].speedMode)
traci.vehicle.setLaneChangeMode(v, controllers[v].laneChangeMode)
# gather vehicle states for this step
vStates = {}
for vehID, subs in traci.vehicle.getSubscriptionResults().iteritems():
tempParams = getFromSubscription(vehID, subs, dataFromTraciState)
vStates[vehID] = VState(tempParams + vehicleSetParams[vehID])
#
for vehID, vState in vStates.iteritems():
sensor = sensorToUse(vState, realign=True)
#
for otherID, otherState in vStates.iteritems():
if vehID != otherID:
#
# check for collisions
if collisionCheck.check(vState, otherState):
#print "collision! pos",vState.x,vState.y,"step",state.step
if state.collisionOccurred <= 0:
state.collisionOccurred = state.step * .1
break
#
# update sensor
sensor.addObstacle(otherState)
#
if setting.verbose:
for vstat in sensor.getObstacles():
print(vehID, "detects", vstat.x, vstat.y, "speed", vstat.speed)
# store this vehicle movement
output.add([[
state.step * .1, vehID, vState.x, vState.y, vState.angle,
vState.speed
]])
# use data to decide speed
if controllers[vehID] is not None:
controllers[vehID].updateSpeed(vState.speed)
commandedSpeed = controllers[vehID].nextStep(sensor.getObstacles())
traci.vehicle.setSpeed(vehID, commandedSpeed)
if setting.verbose:
print("setting speed", commandedSpeed)
def sendRequest(self, request: Request):
routeBuilder = self.routes[request.json['form_redirect']]
for i in routeBuilder.middleware:
result = Middlewares.callMiddleware(i, 'handle', request)
if type(
result
) == Request: # If middleware would return success, it will go to next middleware
request = result
continue
else: # It will return the response to viewHandler
return result
className, methodName = routeBuilder.controller.split('.')
return Controllers.callController(className, methodName, request)
def __init__(self, boat, fixed_thrust=0.2, angle_divisions=8):
super(PseudoRandomBalancedHeading, self).__init__(boat)
self.boat = boat
self.th = 0
self.angle_bins = np.linspace(-np.pi,
np.pi,
angle_divisions,
endpoint=False)
self.angle_bin_counts = np.zeros(self.angle_bins.shape)
self.angle_bin_selection_counts = np.zeros(self.angle_bins.shape)
self.angle_start_time = 0
self.angle_duration = 0
self.elapsed_time = 0
self.controller = Controllers.MaintainHeading(boat, [0.5, 0, 0.5],
fixed_thrust)
def assureResourceLoaded(self,
importName,
resources,
searchPath=None,
specialAttrs=None,
report=False):
if searchPath is None:
searchPath = self.buildResourceSearchList()
try:
f, fn, desc = Utils.find_dotted_module(importName, searchPath)
except ImportError:
if report:
self.editor.setStatus(
_('Could not find %s') % importName, 'Error')
return False
if f is None:
return False
f.close()
import Controllers
Model, main = Controllers.identifyFile(fn)
for ResourceClass in Controllers.resourceClasses:
if issubclass(Model, ResourceClass):
try:
imageMod, rootName, rootMod = self.loadResource(
importName, searchPath)
resources[importName] = imageMod
specialAttrs[rootName] = rootMod
if report:
self.editor.setStatus(
_('Loaded resource: %s') % importName)
except ImportError:
self.editor.setStatus(
_('Could not load %s') % importName, 'Error')
return False
return True
if report:
self.editor.setStatus(
_('%s is not a valid Resource Module') % importName, 'Error')
return False
def idModel(self, name, src=None):
# XXX This should be cached until rename or delete
absPath = self.normaliseModuleRelativeToApp(self.modules[name][2])
import Controllers
from Explorers.Explorer import splitURI
prot, cat, res, fn = splitURI(absPath)
if src is None:
if self.editor.modules.has_key(name):
self.moduleModels[name], main = identifySource(
self.editor.modules[name].model.getDataAsLines())
if self.editor.modules.has_key(absPath):
self.moduleModels[name], main = identifySource(
self.editor.modules[absPath].model.getDataAsLines())
else:
try: self.moduleModels[name], main = \
Controllers.identifyFile(res, localfs=prot=='file')
except: pass
else:
self.moduleModels[name], main = identifySource(src)
def idModel(self, name, src=None):
# XXX This should be cached until rename or delete
absPath = self.normaliseModuleRelativeToApp(self.modules[name][2])
import Controllers
from Explorers.Explorer import splitURI
prot, cat, res, fn = splitURI(absPath)
if src is None:
if self.editor.modules.has_key(name):
self.moduleModels[name], main = identifySource(
self.editor.modules[name].model.getDataAsLines())
if self.editor.modules.has_key(absPath):
self.moduleModels[name], main = identifySource(
self.editor.modules[absPath].model.getDataAsLines())
else:
try: self.moduleModels[name], main = \
Controllers.identifyFile(res, localfs=prot=='file')
except: pass
else:
self.moduleModels[name], main = identifySource(src)
def assureResourceLoaded(self, importName, resources, searchPath=None,
specialAttrs=None, report=False):
if searchPath is None:
searchPath = self.buildResourceSearchList()
try:
f, fn, desc = Utils.find_dotted_module(importName, searchPath)
except ImportError:
if report:
self.editor.setStatus(_('Could not find %s')%importName, 'Error')
return False
if f is None:
return False
f.close()
import Controllers
Model, main = Controllers.identifyFile(fn)
for ResourceClass in Controllers.resourceClasses:
if issubclass(Model, ResourceClass):
try:
imageMod, rootName, rootMod = self.loadResource(importName,
searchPath)
resources[importName] = imageMod
specialAttrs[rootName] = rootMod
if report:
self.editor.setStatus(_('Loaded resource: %s')%importName)
except ImportError:
self.editor.setStatus(_('Could not load %s')%importName, 'Error')
return False
return True
if report:
self.editor.setStatus(_('%s is not a valid Resource Module')%importName, 'Error')
return False
'''
Created on 2009-11-26
@author: beaudoin
'''
import PyUtils
import Core, wx
import Controllers
PyUtils.load("RigidBodies.FiniteFlatGround")
PyUtils.loadMany("RigidBodies.DodgeBall", 5)
character = PyUtils.load("Characters.BipV3")
character.computeMass()
controller = Controllers.DanceController(character)
controller.loadFromFile("../data/controllers/bipV3/fWalk_4.sbc")
#controller.loadFromFile("../data/controllers/bipV3/fWalk_3.sbc")
Core.cvar.SimGlobals_stanceKnee = 0.00
Core.cvar.SimGlobals_rootSagittal = 0.3
Core.cvar.SimGlobals_stepHeight = 0.25
Core.cvar.SimGlobals_duckWalk = 0.2
Core.cvar.SimGlobals_VDelSagittal = 2.0
Core.cvar.SimGlobals_stepTime = 0.2
#Core.cvar.SimGlobals_upperBodyTwist = 0.2
controller.initialSetup()
params = pd.read_csv(paramFile,header=0)
else:
iteration = 1
params = None
call(['mkdir','-p',outputFolder+'/'+options.outputName]) # add folder
## run iteration
while iteration <= options.numiter:
err = 0
starttime = time.time()
Sim = Sumo(configuration, options.gui)
## now set up parameters and vehicles
err += Sim.createVehicle('ego','main_0',VEHsize[0])
#egoControl = Controllers.RearEndEgo(vehicleSpeed(), vehicleAccel())
egoControl = Controllers.RearEndRandom(vehicleSpeed(),maxdecel=.5)
err += Sim.createVehicle('lead','main_0',50+VEHsize[0])
#leadControl = Controllers.RearEndBrake(vehicleSpeed(), vehicleAccel())
leadControl = Controllers.RearEndRandom(vehicleSpeed(),maxdecel=5.)
# params_iter = pd.DataFrame([[egoControl.speed, leadControl.speed,
# egoControl.accel, leadControl.accel,
# leadControl.brakeMagnitude, leadControl.brakeTime,
# -1.]], columns=ParameterColumns)
params_iter = pd.DataFrame([[egoControl.speed, leadControl.speed, -1.]],
columns=ParameterColumns)
output = None
ttime = 0
maxTime = 100 # seconds
lanelength = Sim.getLaneInfo('main_0')[0]
def __init__(self, boat):
super(DoNothing, self).__init__(boat)
self.controller = Controllers.DoNothing()
] #+[Img.imgstrip("men/CMan")[0]]
breaking = False
Img.musplay("ChOrDs.ogg")
while not breaking:
for event in pygame.event.get():
if event.type == pygame.QUIT:
sys.exit()
elif event.type == pygame.MOUSEBUTTONDOWN:
breaking = True
screen.fill((255, 0, 0))
Img.bcentre(tfont, "BOMB BATTLES", screen)
Img.bcentre(sfont, "Click to start", screen, 50)
pygame.display.flip()
clock.tick(60)
breaking = False
controllers = [Controllers.Keyboard1(),
Controllers.Keyboard2()] + [
Controllers.UniJoyController(n)
for n in range(pygame.joystick.get_count())
]
activecons = []
acps = []
rsps = []
rsc = []
while not breaking:
gevents = pygame.event.get()
for event in gevents:
if event.type == pygame.QUIT:
sys.exit()
elif event.type == pygame.MOUSEBUTTONDOWN and len(rsps) > 1:
breaking = True
print('\n1 - WebPage')
print('2 - MidiDevice')
userChoice = input('\nYour choice is: ')
print('\nChoose the type of control of the ProBots motors:')
print('\n1 - Sabertooth 2x25')
print('2 - PWM Controller OSMC3-2')
userChoice1 = input('\nYour choice is: ')
userChoice = str(userChoice)
userChoice1 = str(userChoice1)
# Initialization of classes from local files
PC = Sabertooth.PacketizedCommunication()
Battery = BatteryMonitor.BatteryVoltage()
PID = Controllers.PIDControllers()
Enc = Encoders.EncodersReadings()
Pconst = ProBotConstants.Constants()
Restartconst = RestartProgram.Restart()
UserChoice = Pub_Sub.userChoice(userChoice)
#PWM.start(channel, duty, freq, polarity)
PWM.start(Pconst.PWM_RF, 0, Pconst.PWM_Freq, 0)
PWM.start(Pconst.PWM_RR, 0, Pconst.PWM_Freq, 0)
PWM.start(Pconst.PWM_LF, 0, Pconst.PWM_Freq, 0)
PWM.start(Pconst.PWM_LR, 0, Pconst.PWM_Freq, 0)
# Configuration the type of GPIO's
GPIO.setup(Pconst.RedLED, GPIO.OUT)
GPIO.setup(Pconst.GreenLED, GPIO.OUT)
GPIO.setup(Pconst.BlueLED, GPIO.OUT)
def HandleProcessor():
controller = Controllers.RaspberryPiController()
def rotateEE():
# Creating a ROS node to publish PWM signals to an Arduino subscriber
rospy.init_node('pwmNode', anonymous=True)
pwmPublisher3 = rospy.Publisher('pwm3', Float32, queue_size=1)
rospy.Subscriber("joint3_theta", Float32, joint3Callback)
secs = 3
print("Waiting", secs, "seconds...")
time.sleep(secs)
print('Control EE to open handle')
tX = 0 #Degrees
tF = 130 #Degrees
P = 50
I = 0.1
D = 15
eTot = 0
eOld = abs(tF - tX)
e3 = abs(tF - tX)
tX_new = 0
while (e3 >= 3):
#tX = measured angle
tX_new = joint3_queue.pop()
if (abs(tF - tX_new) <= 3):
print('Abort')
break
if (tF < tX_new):
dir = 1
else:
dir = 0
Calc_PID = C.PID_EE(P, I, D, tF, tX_new, eTot, eOld, dir)
#Returns eTot, eNew, signal, and tX
eOld = Calc_PID[1]
eTot = Calc_PID[0]
signal_j3 = Calc_PID[2]
#send this to PWM converted representation via Arduino
pwmPublisher3.publish(signal_j3)
time.sleep(0.1) # TODO need to change time delay
print("Joint 3: " + str(signal_j3) + "angle: " + str(tX_new))
#print("Error: " + str(e3) + " Signal: " + str(signal_j3))
signal_j3 = 0
pwmPublisher3.publish(signal_j3)
nudge = False
while (nudge == False):
rospy.init_node('pwmNode', anonymous=True)
pwmPublisher1 = rospy.Publisher('pwm1', Float32, queue_size=1)
pwmPublisher2 = rospy.Publisher('pwm2', Float32, queue_size=1)
rospy.Subscriber("joint1_theta", Float32, joint1Callback)
rospy.Subscriber("joint2_theta", Float32, joint2Callback)
signal_j1 = -85
signal_j2 = 50
pwmPublisher1.publish(signal_j1)
pwmPublisher2.publish(signal_j2)
time.sleep(0.25)
signal_j1 = 0
signal_j2 = 0
pwmPublisher1.publish(signal_j1)
pwmPublisher2.publish(signal_j2)
#Perform forward kinematics to 'nudge'door and prevent it relocking
nudge = True
tF = 0
tX = tX_new
e4 = abs(tF - tX)
print(e4)
eOld = e4
eTot = 0
P = 50
I = 0.5
D = 10
while (e4 >= 3):
#tX = measured angle
tX_new = joint3_queue.pop()
if (abs(tF - tX_new) <= 3):
break
if (tF < tX_new):
dir = 1
else:
dir = 0
Calc_PID2 = C.PID_EE(P, I, D, tF, tX_new, eTot, eOld, dir)
#Returns eTot, eNew, signal, and tX
eOld = Calc_PID2[1]
eTot = Calc_PID2[0]
signal_j3 = Calc_PID2[2]
pwmPublisher3.publish(signal_j3)
time.sleep(0.1) # TODO need to change time delay
#tX_new = joint3_queue.pop() #- signal_j3/1000 #TEMPORARY FOR TESTING
#e4 = abs(tF-tX_new)
#print("Error: " + str(e4) + " Signal: " + str(signal_j3))
#send this to PWM converted representation via Arduino
signal_j3 = 0
pwmPublisher3.publish(signal_j3)
while True:
continue
def home2Handle_Control(t1_f, t2_f):
print('Drive Arm to needed position')
rospy.init_node('pwmNode', anonymous=True)
pwmPublisher1 = rospy.Publisher('pwm1', Float32, queue_size=1)
pwmPublisher2 = rospy.Publisher('pwm2', Float32, queue_size=1)
rospy.Subscriber("joint1_theta", Float32, joint1Callback)
rospy.Subscriber("joint2_theta", Float32, joint2Callback)
secs = 3
print("Waiting", secs, "seconds...")
time.sleep(secs)
t1_f = n.interp(112, [35, 275], [-120, 120])
t2_f = n.interp(140, [60, 300], [120, -120])
t1_i = -90 #deg
t2_i = 90 #deg
e1 = abs(t1_f - t1_i)
e2 = abs(t2_f - t2_i)
#error = n.transpose([e1,e2])
#Joint Control independently for now:
#Tune for each joint
P = 12
I = 0.1
D = 10
eTot = 0
eOld = e1
dir = 0
tX_new = 0
while (e1 >= 1):
#tX = measured angle
tX_new = n.interp(joint1_queue.pop(), [35, 275], [-120, 120])
print("Popped from t1: " + str(tX_new))
if (abs(t1_f - tX_new) <= 1):
break
if (t1_f < tX_new):
dir = 0
else:
dir = 1
Calc_PID = C.PID_EE(P, I, D, t1_f, tX_new, eTot, eOld, dir)
#Returns eTot, eNew, signal, and tX
eOld = Calc_PID[1]
eTot = Calc_PID[0]
signal_j1 = Calc_PID[2]
#send this to PWM converted representation via Arduino
pwmPublisher1.publish(signal_j1)
time.sleep(0.1)
#tX_new = n.interp(joint1_queue.pop(),[35,275],[-120,120]) # + signal_j1/1000 #TEMPORARY FOR TESTING
#e1 = abs(t1_f-tX_new)
#print("Error: " + str(e1) + " Signal: " + str(signal_j1))
#Kill Signal
signal_j1 = 0
pwmPublisher1.publish(signal_j1)
P = 9
#I = 0.01
I = 0
D = 5
eTot = 0
eOld = e2
tX_new = 0
while (e2 >= 1):
#tX = measured angle
tX_new = n.interp(joint2_queue.pop(), [60, 300], [120, -120])
print("Joint 2: " + str(tX_new))
if (abs(t2_f - tX_new) <= 1):
break
if (t2_f < tX_new):
dir = 0
else:
dir = 1
Calc_PID = C.PID_EE2(P, I, D, t2_f, tX_new, eTot, eOld, dir)
#Returns eTot, eNew, signal, and tX
eOld = Calc_PID[1]
eTot = Calc_PID[0]
signal_j2 = Calc_PID[2]
#send this to PWM converted representation via Arduino
pwmPublisher2.publish(signal_j2)
time.sleep(0.01)
#Kill Signal 2
signal_j2 = 0
pwmPublisher2.publish(signal_j2)
from flask import Flask
from waitress import serve
from flask_cors import CORS
from flask_gzip import Gzip
import Config
import Models
import Controllers
import Authentication
import Workers
app = Flask(__name__, static_url_path='', static_folder='./Ui')
CORS(app, resources={r'/*': {'origins': '*'}})
Gzip(app)
Config.initialize(app)
Controllers.initialize(app)
Models.initialize(app)
Authentication.initialize(app)
Workers.initialize(app)
# print(app.url_map)
if __name__ == '__main__':
print(f'Listening on all interfaces on port 8888')
serve(app, host='0.0.0.0', port=8888)
import os, json
import Dashboard, Controllers #mysql
from ControllerLibrary import LoadHardwareConfiguration
from DatabaseTools import DatabaseInterface
from multiprocessing import Process, Manager
if __name__ == '__main__':
#ControllerFrontend = FrontEnd.initialize_frontend()
#ControllerFrontend.run_server(debug=True)
#LoadHardwareConfiguration()
hardware_configuration = LoadHardwareConfiguration()
Controllers.registerProxy('DBConnection', DatabaseInterface,
Controllers.DatabaseInterfaceProxy,
Controllers.TankControllerManager)
# Controllers.registerProxy('Synchronizer', Controllers.Synchronizer, Controllers.SynchronizerProxy,
# Controllers.TankControllerManager)
SystemManager = Controllers.TankControllerManager()
SystemManager.start()
#DatabaseConnector = SystemManager.DBConnection()
ProcessSynchronizer = Controllers.Synchronizer(SystemManager)
ProcessSynchronizer.database_connector = SystemManager.DBConnection()
SystemController = Controllers.TankController(ProcessSynchronizer,
hardware_configuration)
#SystemController.start()
TankControllerFrontEnd, TankControllerSharedMemory = Dashboard.InitializeFrontend(
hardware_configuration=hardware_configuration,