def __init__(self):
self.motor_last_change = 0
if os.access("/var/www/daarrt.conf", os.F_OK) :
print "Real DAARRT creation"
# Import modules
from drivers.trex import TrexIO
from drivers.razor import RazorIO
from drivers.hcsr04 import SonarIO, PIN_MAP
self.trex = TrexIO(0x07)
self.razor = RazorIO()
self.sonar = [SonarIO(i[0], i[1]) for i in PIN_MAP] # [Arriere, Droite, Avant, Gauche]
# self.sonar = [SonarIO(2, 3), SonarIO(4, 5), SonarIO(6, 7), SonarIO(8, 9)] # [Arriere, Droite, Avant, Gauche]
else :
print "Create virtual DAARRT"
from multiprocessing import Process,Manager
import vDaarrt.simulation as simulation
import vDaarrt.modules.vTrex as vTrex
import vDaarrt.modules.vSonar as vSonar
import vDaarrt.modules.vRazor as vRazor
global simuProc
manager = Manager()
self.ns = manager.Namespace()
self.trex = vTrex.vTrex()
self.razor = vRazor.vRazorIO()
self.sonarLeft = vSonar.vSonar(1,0,0)
self.sonarRight = vSonar.vSonar(2,0,0)
self.sonarFront = vSonar.vSonar (3,0,0)
self.sonarBack = vSonar.vSonar(4,0,0)
self.sonar = [self.sonarBack,self.sonarRight,self.sonarFront,self.sonarLeft]
self.ns.isAlive = True
simuProc = Process(target = simulation.simulate,args = (self.ns,self.trex.package,self.trex.changeData,self.trex.changeDataEnco,self.sonarLeft.changeSonarLeft,self.sonarRight.changeSonarRight,self.sonarFront.changeSonarFront,self.sonarBack.changeSonarBack,self.razor.changeCap))
simuProc.start()
print "Running Simulation"
def __init__(self):
# test if on real robot, if yes load the drivers
# if os.access("/var/www/daarrt.conf", os.F_OK) :
if os.access("/sys/class/gpio/gpio266", os.F_OK):
print("Real DART is being created")
# Import modules
from drivers.trex import TrexIO
from drivers.sonars import SonarsIO
from drivers.razor import RazorIO
from drivers.rear_odo import RearOdos
self.__trex = TrexIO()
self.__sonars = SonarsIO()
self.__razor = RazorIO()
self.__rear_odos = RearOdos()
self.__trex.command["use_pid"] = 0 # 1
self.tolerance = 5
self.minSpeed = 50
# if not create virtual drivers for the robot running in V-REP
else:
print("Virtual DART is being created")
import threading
import socket
import sys
import struct
import time
# socket connection to V-REP
self.__HOST = 'localhost' # IP of the sockect
self.__PORT = 30100 # port (set similarly in v-rep)
self.minSpeed = 50
self.__s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
print('Socket created')
# bind socket to local host and port
try:
# prevent to wait for timeout for reusing the socket after crash
# from : http://stackoverflow.com/questions/29217502/socket-error-address-already-in-use
self.__s.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
self.__s.bind((self.__HOST, self.__PORT))
except socket.error as msg:
print(msg)
sys.exit()
print('Socket bind completed')
# start listening on socket
self.__s.listen(10)
print('Socket now listening')
# reset variables
self.__vMotorLeft = 0.0
self.__vMotorRight = 0.0
self.__vEncoderLeft = 0
self.__vEncoderRight = 0
self.__vSonarLeft = 0.0
self.__vSonarRight = 0.0
self.__vSonarFront = 0.0
self.__vSonarRear = 0.0
self.__vHeading = 0.0
# initiate communication thread with V-Rep
self.__simulation_alive = True
a = self.__s
vrep = threading.Thread(target=self.vrep_com_socket, args=(a, ))
vrep.start()
# start virtual drivers
import vDaarrt.modules.vTrex as vTrex
import vDaarrt.modules.vSonar as vSonar
import vDaarrt.modules.vRazor as vRazor
self.__trex = vTrex.vTrex()
self.__razor = vRazor.vRazorIO()
self.__sonars = vSonar.vSonar(self)
class Daarrt():
def __init__(self):
self.motor_last_change = 0
if os.access("/var/www/daarrt.conf", os.F_OK) :
print "Real DAARRT creation"
# Import modules
from drivers.trex import TrexIO
from drivers.razor import RazorIO
from drivers.hcsr04 import SonarIO
self.trex = TrexIO(0x07)
self.razor = RazorIO()
self.sonar = [SonarIO(2, 3), SonarIO(4, 5), SonarIO(6, 7), SonarIO(8, 9)] # [Arriere, Droite, Avant, Gauche]
else :
print "Create virtual DAARRT"
from multiprocessing import Process,Manager
import vDaarrt.simulation as simulation
import vDaarrt.modules.vTrex as vTrex
import vDaarrt.modules.vSonar as vSonar
import vDaarrt.modules.vRazor as vRazor
global simuProc
manager = Manager()
self.ns = manager.Namespace()
self.trex = vTrex.vTrex()
self.razor = vRazor.vRazorIO()
self.sonarLeft = vSonar.vSonar(1,0,0)
self.sonarRight = vSonar.vSonar(2,0,0)
self.sonarFront = vSonar.vSonar (3,0,0)
self.sonarBack = vSonar.vSonar(4,0,0)
self.sonar = [self.sonarBack,self.sonarRight,self.sonarFront,self.sonarLeft]
self.ns.isAlive = True
simuProc = Process(target = simulation.simulate,args = (self.ns,self.trex.package,self.trex.changeData,self.trex.changeDataEnco,self.sonarLeft.changeSonarLeft,self.sonarRight.changeSonarRight,self.sonarFront.changeSonarFront,self.sonarBack.changeSonarBack,self.razor.changeCap))
simuProc.start()
print "Running Simulation"
###########################
## T-REX ##
###########################
def status(self):
'''
Read status from trex
Return as a byte array
'''
raw_status = self.trex.i2cRead()
return struct.unpack(">cchhHhHhhhhhh", raw_status)[2:]
def reset(self):
'''
Reset the trex controller to default
Stop dc motors...
'''
self.trex.reset()
def motor(self, left, right):
'''
Set speed of the dc motors
left and right can have the folowing values: -255 to 255
-255 = Full speed astern
0 = stop
255 = Full speed ahead
'''
self.motor_last_change = time.time()*1000
try : lsign = left / abs(left)
except ZeroDivisionError, e : lsign = 1
try : rsign = right / abs(right)
except ZeroDivisionError, e : rsign = 1
left, right = lsign * min(abs(left), 255), rsign * min(abs(right), 255)
left = int(left)
right = int(right)
self.trex.package['lm_speed_high_byte'] = high_byte(left)
self.trex.package['lm_speed_low_byte'] = low_byte(left)
self.trex.package['rm_speed_high_byte'] = high_byte(right)
self.trex.package['rm_speed_low_byte'] = low_byte(right)
self.trex.i2cWrite()
def servo(self, servo, position):
'''
Set servo position
Servo = 1 to 6
Position = Typically the servo position should be a value between 1000 and 2000 although it will vary depending on the servos used
'''
servo = str(servo)
position = int(position)
self.trex.package['servo_' + servo + '_high_byte'] = high_byte(position)
self.trex.package['servo_' + servo + '_low_byte'] = low_byte(position)
self.trex.i2cWrite()
###########################
## Razor 9-DOF IMU ##
###########################
def getAngles(self):
'''
Return angles measured by the Razor (yaw/pitch/roll calculated automatically from the 9-axis data).
'''
return struct.unpack('fff', self.razor.getAngles())
def getSensorData(self):
"""
Output SENSOR data of all 9 axes in text format.
One frame consist of three 3x3 float values = 36 bytes. Order is: acc x/y/z, mag x/y/z, gyr x/y/z.
"""
return struct.unpack('fffffffff', self.razor.getRawSensorData())
def getCalibratedSensorData(self):
"""
Output CALIBRATED SENSOR data of all 9 axes in text format.
One frame consist of three 3x3 float values = 36 bytes. Order is: acc x/y/z, mag x/y/z, gyr x/y/z.
"""
return struct.unpack('fffffffff', self.razor.getCalibratedSensorData())
###########################
## Sonar HC-SR04 ##
###########################
def getSonars(self):
'''
Return angles measured by the Razor (calculated automatically from the 9-axis data).
'''
dist = [s.getValue() for s in self.sonar]
for name, val in zip(["Sonar " + i + " : " for i in ["arriere", "droite", "avant", "gauche"]], dist) :
if val == -1 : print name + "range <= 5 cm"
else : print name + "%.1f" % val
return dist