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)
def __init__(self):
self.__dartSim = False
# trap hit of ctrl-x to stop robot and exit (emergency stop!)
signal.signal(signal.SIGINT, self.interrupt_handler)
# test if on real robot, if yes load the drivers
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.encoders import EncodersIO
from drivers.imu9 import Imu9IO
self.__trex = TrexIO()
self.__sonars = SonarsIO()
self.__encoders = EncodersIO()
self.__trex.command["use_pid"] = 0 # 1
self.__imu = Imu9IO()
self.__init_mission_time = True
self.__start_mission_time = 0.0
self.__mission_time = self.realMissionTime
# if not create virtual drivers for the robot running in V-REP
else:
self.__dartSim = True
print("Virtual DART is being created")
import threading
import socket
import sys
import struct
import time
self.vdart_ready = threading.Event()
self.vdart_ready.clear()
# socket connection to V-REP
self.__HOST = 'localhost' # IP of the sockect
self.__PORT = 30100 # port (set similarly in v-rep)
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')
self.vSimTime = 0.0
self.__vLocation = [0., 0., 0.]
self.__vAccel = [0., 0., 0.]
self.__vGyro = [0., 0., 0.]
# reset variables
#self.__vMotorLeft_Mem = 0
#self.__vMotorRight_Mem = 0
#self.__vMotorLeft = 0
#self.__vMotorRight = 0
self.__vEncoderFrontLeft = 0
self.__vEncoderFrontRight = 0
self.__vEncoderRearLeft = 0
self.__vEncoderRearRight = 0
self.__vSonarFrontLeft = 0.0
self.__vSonarFrontRight = 0.0
self.__vSonarLeft = 0.0
self.__vSonarRight = 0.0
self.__vSonarFront = 0.0
self.__vSonarRear = 0.0
self.__vHeading = 0.0
self.__vVoltage = 0.0
# left wheels motion control
self.__vMotorDirectionLeft = 1
self.__vMotorCmdLeft = 0
#self.__vMotorCmdLastLeft = 0
#self.__vMotorAngSpeedLeft = 0.0
self.__vMotorCmdWithInertiaLeft = 0.0
self.__vMotorAngSpeedWithInertiaLeft = 0.0
self.__vMotorAngSpeedVrepLeft = 0.0
self.__vMotorAngSpeedDriftVrepLeft = 0.0
self.__vMotorCmdActualLastLeft = 0
self.__vMotorInertiaLeft = 0.0
# right wheels motion control
self.__vMotorDirectionRight = 1
self.__vMotorCmdRight = 0
#self.__vMotorCmdLastRight = 0
#self.__vMotorAngSpeedRight = 0.0
self.__vMotorCmdWithInertiaRight = 0.0
self.__vMotorAngSpeedWithInertiaRight = 0.0
self.__vMotorAngSpeedVrepRight = 0.0
self.__vMotorAngSpeedDriftVrepRight = 0.0
self.__vMotorCmdActualLastRight = 0
self.__vMotorInertiaRight = 0.0
# control and simulation parameters
#self.__vInertiaLeft = "none"
#self.__vInertiaRight = "none"
self.__vInertiaCoefUp = 0.05 # 0.1 1st order LP filter (speed up)
self.__vInertiaCoefDown = 0.1 # 0.1 1st order LP filter (speed down)
self.__vDeadZoneUp = 70.0 # dead zone when starting
self.__vDeadZoneDown = 30.0 # dead zone when stopping
self.__vDeadZoneLeft = self.__vDeadZoneUp
self.__vDeadZoneRight = self.__vDeadZoneUp
self.__vMotorLeftRightDiff = 1.01 # +/- % assymetric motor speed
self.__vCmdToAngSpeed = 1. / 7.8 # speed cmd to actual angular speed
# load and start virtual drivers
#vTrex = imp.load_source('vTrex', '../vDartV2/vTrex.py')
#vSonars = imp.load_source('vSonars', '../vDartV2/vSonars.py')
#vImu = imp.load_source('vImu', '../vDartV2/vImu.py')
#vI2cHead = imp.load_source('vI2c', '../vDartV2/vI2cHead.py')
sys.path.append("../vDartV2")
import vSonars as vSonars
import vTrex as vTrex
import vImu9 as vImu9
import vEncoders as vEncoders
import vSimVar as vSimVar
#import vI2C as vI2C
self.vSimVar = vSimVar
self.__trex = vTrex.TrexIO()
self.__imu = vImu9.Imu9IO()
self.__sonars = vSonars.SonarsIO()
self.__encoders = vEncoders.EncodersIO()
self.__init_mission_time = True
self.__start_mission_time = 0.0
self.__mission_time = self.virtualMissionTime
# initiate communication thread with V-Rep
self.__simulation_alive = True
sk = self.__s
ev = self.vdart_ready
vrep = threading.Thread(target=self.vrep_com_socket,
args=(
sk,
ev,
))
vrep.start()
# debug log
self.tlog = []
# wait for vdart to be ready
self.vdart_ready.wait()
print("Dart ready ...")
class Dart():
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)
@property
def s(self):
return self.__s
@property
def vMotorLeft(self):
return self.__vMotorLeft
@property
def vMotorRight(self):
return self.__vMotorRight
@property
def vSonarFront(self):
return self.__vSonarFront
@property
def vSonarRear(self):
return self.__vSonarRear
@property
def vSonarLeft(self):
return self.__vSonarLeft
@property
def vSonarRight(self):
return self.__vSonarRight
@property
def vEncoderLeft(self):
return self.__vEncoderLeft
@property
def vEncoderRight(self):
return self.__vEncoderRight
@property
def simulation_alive(self):
return self.__simulation_alive
@property
def vHeading(self):
return self.__vHeading
@property
def angles(self):
return self.__razor.angles
def vrep_com_socket(vdart, s):
print(s)
RECV_BUFFER = 4096 # buffer size
while True:
#wait to accept a connection - blocking call
conn, addr = s.accept()
print('Connected with ' + addr[0] + ':' + str(addr[1]))
#print (conn)
while True:
#print ("socket read",conn)
data = conn.recv(RECV_BUFFER)
#print (len(data))
hd0, hd1, sz, lft, vSonarFront, vSonarRear, vSonarLeft, vSonarRight, vEncoderLeft, vEncoderRight, vHeading, simulationTime = struct.unpack(
'<ccHHffffffff', data)
#print (hd0,hd1,sz,lft,vSonarFront, vSonarRear,vSonarLeft, vSonarRight, vEncoderLeft, vEncoderRight, vHeading, simulationTime)
#print (vEncoderLeft, vEncoderRight, vHeading)
vdart.vrep_update_sim_param(vEncoderLeft, vEncoderRight,
vSonarLeft, vSonarRight,
vSonarFront, vSonarRear, vHeading)
vMotorLeft = vdart.vMotorLeft
vMotorRight = vdart.vMotorRight
strSend = struct.pack('<BBHHff', data[0], data[1], 8, 0,
vMotorLeft, vMotorRight)
conn.send(strSend)
if not vdart.simulation_alive:
break
time.sleep(0.010)
if not vdart.simulation_alive:
break
def vrep_update_sim_param(self, vEncoderLeft, vEncoderRight, vSonarLeft,
vSonarRight, vSonarFront, vSonarRear, vHeading):
self.__vEncoderLeft = vEncoderLeft
self.__vEncoderRight = vEncoderRight
self.__vSonarLeft = vSonarLeft
self.__vSonarRight = vSonarRight
self.__vSonarFront = vSonarFront
self.__vSonarRear = vSonarRear
self.__trex.status["left_encoder"] = vEncoderLeft
self.__trex.status["right_encoder"] = vEncoderRight
self.__vMotorLeft = self.__trex.command["left_motor_speed"]
self.__vMotorRight = self.__trex.command["right_motor_speed"]
self.__vHeading = vHeading
# insert here your functions to control the robot using motors, sonars
# encoders and heading
# ...
def rotation(self, speed=50, sens=1, duration=1):
"""
sens = 1 -> sens horaire
sens = -1 -> sens anti-horaire
"""
t1 = time.time()
t0 = time.time()
while t1 - t0 <= duration:
speed = speed % 250
self.__trex.command['left_motor_speed'] = speed * sens
self.__trex.command['right_motor_speed'] = -speed * sens
self.__trex.i2c_write()
t1 = time.time()
self.motor(0, 'left')
self.motor(0, 'right')
def ligne_droite(self, speed=50, sens=1):
speed = speed % 250
self.__trex.command['left_motor_speed'] = speed * sens
self.__trex.command['right_motor_speed'] = speed * sens
self.__trex.i2c_write()
def get_odometers(self):
"""
renvoie la valeur de gauche puis de droite
"""
left = self.__trex.status['left_encoder']
right = self.__trex.status['right_encoder']
self.__trex.i2c_read()
return left, right
def get_sonar(self, sonar_name):
return self.__sonars.get_distance(sonar_name)
def motor(self, speed, side, sens=1):
if speed < self.minSpeed and speed != 0: #définition d'une vitesse minimale
speed = self.minSpeed
speed = int(speed)
if side == 'left':
self.__trex.command['left_motor_speed'] = speed * sens
if side == 'right':
self.__trex.command['right_motor_speed'] = speed * sens
self.__trex.i2c_write()
def get_angles(self):
return -self.__razor.angles[0]
# @property
# def get_angles(self):
# return - self.__razor.angles[0]
def stop(self):
print("Game Over")
# stop the connection to the simulator
self.__simulation_alive = False
# add a command to stop the motors
# ...
self.motor(0, 'left')
self.motor(0, 'right')
def setHeading(self, head):
# print('setheading')
headOk = False
if head > 180:
head = 180
if head < -180:
head = -180
speed = 50
while not headOk:
headMes = self.get_angles()
# print(headMes)
# headMes = self.get_angles() + 2 * coef * random.random() - coef
# print(headMes)
headErr = head - headMes
if abs(headErr) < self.tolerance:
headOk = True
self.motor(0, 'left')
self.motor(0, 'right')
else:
if (headErr > 0
and headErr < 180) or headErr < -180: #Turn left
self.motor(speed, 'left', -1)
self.motor(speed, 'right', 1)
else: #Turn right
self.motor(speed, 'left', 1)
self.motor(speed, 'right', -1)
time.sleep(0.1)
def setHeadingProp(self, head, alpha=1.2):
# print('setHeadingProp')
headOk = False
if head > 180:
head = 180
if head < -180:
head = -180
maxSpeed = 130
while not headOk:
headMes = self.get_angles()
# headMes = self.get_angles() + 2 * coef * random.random() - coef
# print(headMes)
headErr = head - headMes
# print('head',head)
# print('headMes',headMes)
# print('ERREUR',headErr)
if (-180 < headErr < 180):
delta = abs(headErr)
else:
delta = abs(headErr) - 180
speed = delta * alpha
if speed > maxSpeed:
speed = maxSpeed
if abs(headErr) < self.tolerance:
# print('lmqksdfjqlmsdkfjqsdlmkfjqslmdkfjqmsldkfjqslmdf')
headOk = True
self.motor(0, 'left')
self.motor(0, 'right')
else:
if (headErr > 0
and headErr < 180) or headErr < -180: #Turn left
self.motor(speed, 'left', -1)
self.motor(speed, 'right', 1)
else: #Turn right
self.motor(speed, 'left', 1)
self.motor(speed, 'right', -1)
time.sleep(0.1)
# print('fin set heading')
def giveHeadingProp(self, head, alpha=1.95):
# print('giveHeadingProp')
if head > 180:
head = 180
if head < -180:
head = -180
maxSpeed = 130
headMes = self.get_angles()
# print(headMes)
headErr = head - headMes
if (-180 < headErr < 180):
delta = abs(headErr)
else:
delta = abs(headErr) - 180
speed = delta * alpha
if speed > maxSpeed:
speed = maxSpeed
if abs(headErr) < self.tolerance:
return 0, None
else:
if (headErr > 0 and headErr < 180) or headErr < -180:
return speed, 'left'
else: #Turn right
return speed, 'right'
def goDartHeading(self, head, speed, duration):
# print('goDartHeading')
self.setHeadingProp(head)
t0 = time.time()
t1 = time.time()
while t1 - t0 < duration:
left_speed = speed
right_speed = speed
turnSpeed, direction = self.giveHeadingProp(head, speed / 20)
# print(turnSpeed)
if direction:
if direction == 'left':
left_speed -= turnSpeed
right_speed += turnSpeed
elif direction == 'right':
left_speed += turnSpeed
right_speed -= turnSpeed
# print(left_speed,right_speed)
self.motor(left_speed, 'left')
self.motor(right_speed, 'right')
t1 = time.time()
self.motor(0, 'left')
self.motor(0, 'right')
# self.stop()
# print('fin heading')
def goLineOdo(self, speed, duration):
t0 = 0
t1 = 0
t0 = time.time()
while t1 - t0 <= duration:
self.motor(speed, 'left')
self.motor(speed, 'right')
leftOdo = self.get_odometers()[0]
rightOdo = self.get_odometers()[1]
errOdo = abs(leftOdo - rightOdo)
if errOdo < 6:
time.sleep(0.2)
else:
if leftOdo > rightOdo:
self.motor(speed - 7, 'left')
else:
self.motor(speed - 7, 'right')
# print(errOdo)
t1 = time.time()
def goLineEmpirique(self, speed, duration):
t0 = 0
t1 = 0
t0 = time.time()
correction = 16 * speed / 40
while t1 - t0 <= duration:
self.motor(speed + correction, 'left')
self.motor(speed, 'right')
t1 = time.time()
def obstcleAVoid(self, d=0.5):
# print(self.get_sonar('front'))
if self.get_sonar('front') <= d:
self.motor(0, 'left')
self.motor(0, 'right')
# time.sleep(0.1)
if self.get_sonar('right') <= d / 2 and self.get_sonar(
'left') >= d / 2:
# self.rotation(90,-1,1.45)
print('Left')
return 'Left'
elif self.get_sonar('left') <= d / 2 and self.get_sonar(
'right') >= d / 2:
# self.rotation(90,1,1.45)
print('Right')
return 'Right'
else:
# self.rotation(90,1,1.45)
print('DefaultCase')
return 'Right'
return False
class Dart():
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
# 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.__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)
@property
def s(self):
return self.__s
@property
def vMotorLeft(self):
return self.__vMotorLeft
@property
def vMotorRight(self):
return self.__vMotorRight
@property
def vSonarFront(self):
return self.__vSonarFront
@property
def vSonarRear(self):
return self.__vSonarRear
@property
def vSonarLeft(self):
return self.__vSonarLeft
@property
def vSonarRight(self):
return self.__vSonarRight
@property
def vEncoderLeft(self):
return self.__vEncoderLeft
@property
def vEncoderRight(self):
return self.__vEncoderRight
@property
def simulation_alive(self):
return self.__simulation_alive
@property
def vHeading(self):
return self.__vHeading
@property
def angles(self):
return self.__razor.angles
def vrep_com_socket(vdart, s):
print(s)
RECV_BUFFER = 4096 # buffer size
while True:
#wait to accept a connection - blocking call
conn, addr = s.accept()
print('Connected with ' + addr[0] + ':' + str(addr[1]))
#print (conn)
while True:
#print ("socket read",conn)
data = conn.recv(RECV_BUFFER)
#print (len(data))
hd0, hd1, sz, lft, vSonarFront, vSonarRear, vSonarLeft, vSonarRight, vEncoderLeft, vEncoderRight, vHeading, simulationTime = struct.unpack(
'<ccHHffffffff', data)
#print (hd0,hd1,sz,lft,vSonarFront, vSonarRear,vSonarLeft, vSonarRight, vEncoderLeft, vEncoderRight, vHeading, simulationTime)
#print (vEncoderLeft, vEncoderRight, vHeading)
vdart.vrep_update_sim_param(vEncoderLeft, vEncoderRight,
vSonarLeft, vSonarRight,
vSonarFront, vSonarRear, vHeading)
vMotorLeft = vdart.vMotorLeft
vMotorRight = vdart.vMotorRight
strSend = struct.pack('<BBHHff', data[0], data[1], 8, 0,
vMotorLeft, vMotorRight)
conn.send(strSend)
if not vdart.simulation_alive:
break
time.sleep(0.010)
if not vdart.simulation_alive:
break
def vrep_update_sim_param(self, vEncoderLeft, vEncoderRight, vSonarLeft,
vSonarRight, vSonarFront, vSonarRear, vHeading):
self.__vEncoderLeft = vEncoderLeft
self.__vEncoderRight = vEncoderRight
self.__vSonarLeft = vSonarLeft
self.__vSonarRight = vSonarRight
self.__vSonarFront = vSonarFront
self.__vSonarRear = vSonarRear
self.__trex.status["left_encoder"] = vEncoderLeft
self.__trex.status["right_encoder"] = vEncoderRight
self.__vMotorLeft = self.__trex.command["left_motor_speed"]
self.__vMotorRight = self.__trex.command["right_motor_speed"]
self.__vHeading = vHeading
# insert here your functions to control the robot using motors, sonars
# encoders and heading
def stop(self):
print("Game Over")
# add a command to stop the motors
self.set_speed(0, 0)
# stop the connection to the simulator
self.__simulation_alive = False
def set_speed(self, speed_left, speed_right):
self.__trex.command["left_motor_speed"] = speed_left
self.__trex.command["right_motor_speed"] = speed_right
def get_odometers(self):
vLeft = self.__trex.status['left_encoder']
vRight = self.__trex.status['right_encoder']
return vLeft, vRight
def get_distance(self, direction):
return (self.__sonars.get_distance(direction))
def get_angles(self):
#return(self.__razor.angles()[0])
return (self.__vHeading)
def goCap(self, trigo):
self.set_speed(50, -50)
while (self.get_angles() - trigo > 2
or self.get_angles() - trigo < -2):
continue
self.stop()
def center(self, a, b, s):
if s <= -180:
s = s + 360
elif s > 180:
s = s - 360
#a entre -180 et 180
#b entre -180 et 180
return (s)
def setHeading(self, head):
#head entre 0 et 360
headErrMax = 1
headOK = False
while not headOK:
headMes = self.get_angles()
headErr = head - headMes
headErr = self.center(head, headMes, headErr)
#headErr = headErr - 180
#headErr centré en 0
vit = abs(headErr) / 4 + 30
if headErr >= 0:
self.set_speed(vit, -vit)
else:
self.set_speed(-vit, vit)
if abs(headErr) < headErrMax:
print("ca marche Michel")
headOK = True
self.set_speed(0, 0)
else:
#print(self.__vHeading)
#print(abs(headErr) - headErrMax)
time.sleep(0.1)
def goLineHeading(self, head, speed, duration):
self.setHeading(head)
time.sleep(0.1)
capInitial = head
t0 = time.time()
t1 = time.time()
while t1 - t0 < duration:
t1 = time.time()
capFinal = self.get_angles()
diff = capInitial - capFinal
diff = self.center(capInitial, capFinal, diff)
#print(capInitial-capFinal)
if capInitial - capFinal > 0:
self.set_speed(speed + 3, speed)
else:
self.set_speed(speed, speed + 3)
self.set_speed(0, 0)
def goLineOdo(self, speed, duration):
self.setHeading(50)
t0 = time.time()
t1 = time.time()
gauche0, droite0 = self.get_odometers()
while t1 - t0 < duration:
t1 = time.time()
gauche1, droite1 = self.get_odometers()
time.sleep(0.1)
#print((gauche1 - gauche0) - (droite1-droite0) )
if (gauche1 - gauche0) - (droite1 - droite0) > 1:
self.set_speed(speed, speed + 3)
elif (gauche1 - gauche0) - (droite1 - droite0) < -1:
self.set_speed(speed + 3, speed)
gauche0, droite0 = gauche1, droite1
def obstacleAvoid(self):
self.set_speed(0, 0)
self.setHeading(self.get_angles() + 90)
""" A perfectionner plus tard """
def goCurveOdo(self, speedTan, radius, sign, duration):
t0 = time.time()
t1 = time.time()
while t1 - t0 < duration:
if sign == 1:
self.goLineHeading(self.setHeading(1, 1), (50, -50), 0.1)
else:
self.goLineHeading(self.setHeading(1, 1), (-50, 50), 0.1)