Commit ccfd6e68 authored by Benoit ZERR's avatar Benoit ZERR
Browse files

initial commit

parents
V-REP_PRO_EDU_V3_6_2_Ubuntu18_04
# DartV2 Challenge 2021
## DartV2 - Lab1 - Part 3 - Low Level Control
### Several small programs are given in the **py** folder :
robot_cmd_1_qx.py : (x=1..9) examples of programs to solve questions 1 to 9 of the low level control lab.
fast_heading_calibration.py : a very ... very very simple 2D calibration of the compass by making the robot turning in places to get min and max values of the
horizontal magnetic field (these values, different for each dart, can be entered in the imu9 driver)
stop.py just stops the robot and indicate the battery voltage
manual_control.py controls manually the robot with the laptop keyboard (ESC to exit, all other keys are defined in the program)
### Executing a program on the real robot
Asuming that your group is grp-dum, the program to test is stop.py, the number of the dart is 7, the user and passwd are uv27 :
``` bash
$ scp -r py uv27@172.20.25.107:grp-dum
```
Then in another terminal, log in the dart using ssh with user uv27 (passwd uv27)
``` bash
$ ssh uv27@172.20.25.107
$ cd grp-dum/py
$ python3 stop.py
```
## DartV2 - Lab1 - Part 3 - Motion Control
**Now describe your work here ...**
import drivers.dartv2b_basis
import sys
import time
class DartV2(drivers.dartv2b_basis.DartV2Basis):
def __init__(self):
# get init from parent class
#drivers.dartv2b_basis.DartV2Basis.__init__(self)
super().__init__()
# define class variables
self.myVarDummy = 42
# place your new class variables here
# place your new functions here
def battery_voltage(self):
return self.encoders.battery_voltage()
def get_selected_odos(self,front_odos=False):
if front_odos:
odol,odor = self.get_front_odos()
else:
odol,odor = self.get_rear_odos()
return odol,odor
def get_all_odos(self):
odofl,odofr = self.get_front_odos()
odorl,odorr = self.get_rear_odos()
return odofl,odofr,odorl,odorr
def get_front_odos(self):
"""
DART wheels are 12 cm diameter
Virtual DART makes 200 ticks per wheel revolution
Read from T-REX (xx ticks per revolution)
return : odometer values (left and right) on 16 bits signed (-32768 to - 32767)
"""
odofl,odofr = self.get_front_encoders()
self.update_front_encoders (time.time(),odofl,odofr)
return odofl,odofr
def get_rear_odos(self):
odorl,odorr = self.get_rear_encoders()
while odorl<0:
time.sleep(0.0005)
print ("------ error I2C odorl")
odorl,dummy = self.get_rear_encoders()
while odorr<0:
time.sleep(0.0005)
print ("------ error I2C odorr")
dummy,odorr = self.get_rear_encoders()
self.update_rear_encoders (time.time(),odorl,odorr)
return odorl,odorr
def delta_odometers_without_jumps (self,odo_mem,odo_last):
deltaOdo = odo_last - odo_mem
if deltaOdo > 32767: # check if high positive jumps !!
deltaOdo -= 65536 # remove the jump (2^16)
if deltaOdo < -32767: # same for high negative jumps
deltaOdo += 65536 # remove the jump (2^16)
return deltaOdo
def delta_front_odometers(self,side="both"):
if side == "both":
deltaOdoLeft = self.delta_odometers_without_jumps (
self.encoders_front_left_mem,self.encoders_front_left_last)
deltaOdoRight = self.delta_odometers_without_jumps (
self.encoders_front_right_mem,self.encoders_front_right_last)
return [deltaOdoLeft,deltaOdoRight]
elif side == "left":
deltaOdoLeft = self.delta_odometers_without_jumps (
self.encoders_front_left_mem,self.encoders_front_left_last)
return deltaOdoLeft
if side == "right":
deltaOdoRight = self.delta_odometers_without_jumps (
self.encoders_front_right_mem,self.encoders_front_right_last)
return deltaOdoRight
def delta_rear_odometers(self,side="both"):
if side == "both":
deltaOdoLeft = self.delta_odometers_without_jumps (
self.encoders_rear_left_mem,self.encoders_rear_left_last)
deltaOdoRight = self.delta_odometers_without_jumps (
self.encoders_rear_right_mem,self.encoders_rear_right_last)
return [-deltaOdoLeft,-deltaOdoRight]
elif side == "left":
deltaOdoLeft = self.delta_odometers_without_jumps (
self.encoders_rear_left_mem,self.encoders_rear_left_last)
return -deltaOdoLeft
if side == "right":
deltaOdoRight = self.delta_odometers_without_jumps (
self.encoders_rear_right_mem,self.encoders_rear_right_last)
return -deltaOdoRight
def set_sonar_0_to_99(self,d):
if d == 0.0:
d = 99.9
return d
def get_cardinal_sonars(self):
df,dl,db,dr = self.sonars.read_4_sonars()
#print ("df,dl,db,dr",df,dl,db,dr)
df = self.set_sonar_0_to_99(df/100.0)
dl = self.set_sonar_0_to_99(dl/100.0)
db = self.set_sonar_0_to_99(db/100.0)
dr = self.set_sonar_0_to_99(dr/100.0)
self.update_cardinal_sonars(time.time(),df,dl,db,dr)
return df,dl,db,dr
def get_diagonal_sonars(self):
dl,dr = self.sonars.read_diag_all()
self.update_diagonal_sonars(time.time(),dl,dr)
return dl,dr
if __name__ == "__main__":
print ("start")
if len(sys.argv) < 3:
print ("needs to fix left and right speeds :")
print ("python3 dart speedLeft speedRight")
print ("exit!")
exit()
myDart = DartV2()
speedL = int(sys.argv[1])
speedR = int(sys.argv[2])
print ("Sonar version : ",myDart.sonars.get_version())
"""
mode = 2
dmax = 1.5
for isn in range(4):
#print ("Mode",isn+1,":","%2.2X"%(myDart.sonars.get_mode(isn+1)))
myDart.sonars.set_mode(isn+1,mode)
myDart.sonars.set_dist_max(isn+1,dmax)
myDart.sonars.set_dist_max(5,dmax)
for isn in range(4):
#print ("Mode",isn+1,":","%2.2X"%(myDart.sonars.get_mode(isn+1)))
pass
for isn in range(4):
print ("State",isn+1,":","%2.2X"%(myDart.sonars.get_state(isn+1)))
for itst in range(5):
time.sleep(0.5)
print ("Sonars cardinal",myDart.sonars.read_4_sonars())
print ("Sonars front diag",myDart.sonars.read_diag_both())
print ("Sonars front bytes",myDart.sonars.read_front_bytes())
print ("Sonars diag word",myDart.sonars.read_diag_left_word(),myDart.sonars.read_diag_right_word())
"""
print ("Encoder version : ",myDart.encoders.get_version())
print ("Battery voltage :",myDart.encoders.battery_voltage())
print ("Motor directions :",myDart.encoders.read_motors_direction())
myDart.encoders.reset_both()
encs_front_0 = myDart.get_front_encoders()
encs_rear_0 = myDart.encoders.read_encoders()
print ("Encoders (Front T-REX)) :",encs_front_0)
print ("Encoders :",encs_rear_0)
myDart.set_speed(speedL,speedR)
time.sleep(1.0)
print ("Encoders :",myDart.encoders.read_encoders())
#myDart.encoders.reset_left()
time.sleep(1.0)
print ("Encoders :",myDart.encoders.read_encoders())
#myDart.encoders.reset_right()
time.sleep(1.0)
print ("Encoders :",myDart.encoders.read_encoders())
#myDart.encoders.reset_both()
time.sleep(1.0)
print ("Encoders :",myDart.encoders.read_encoders())
print ("Encoders (Front T-REX)) :",myDart.get_front_encoders())
encs_front_0 = myDart.get_front_encoders()
encs_rear_0 = myDart.encoders.read_encoders()
print ("Encoders (Front T-REX)) :",encs_front_0)
print ("Encoders :",encs_rear_0)
"""
print (myDart.imu.read_mag_raw())
"""
myDart.set_speed(0,0)
time.sleep(0.05)
myDart.end()
#!/usr/bin/python
# -*- coding: utf-8 -*-
# Second Year ENSTA Bretagne SPID Project
# D : Brian Detourbet
# A : Elouan Autret
# A : Fahad Al Shaik
# R : Corentin Rifflart
# R : Clément Rodde
# T : Rémi Terrien
# Code modified in May 2016, by BenBlop (small changes to conform with
# new DART)
# Code modified in May 2017, by BenBlop (big changes to adapt to Irvin's
# drivers and to communicate with V-REP using sockets
# Code modified in April 2018 to adapt to Dart V2 (BenBlop)
# 4 sonars changed (accessed with I2C)
# 2 diagonal sonars added (I2C)
# Razor IMU (serial) changed to POLOLU IMU (I2C)
# 7 segments display added (I2C)
# Front encoders added (I2C)
# Code modified in Summer 2019 to use new microcode on DART microcontrollers
# change in sonars and seven segments display
# code taken from : /home/rob/Teach/old-vrep-sims/robmob-dartv2-code-2019/tests/dartv2.py
# v-rep 3.3.2, path :
# ./V-REP_PRO_EDU_V3_3_2_64_Linux
# 2017 simulator build path :
# /home/newubu/Lecture/robmob/dart/Simulateur/build/2017-05-10
# new version allowing to run the same code on real and virtual DARTV2
# - server is now on the lua side
# - no need to have to driver code , one for sim and one for real
# e.g. before vSonars.py was used on the sim and sonars.py on the real robot
# now sonars.py works for both sim and real
# - I2C bus simulation in i2csim.py uses a dict with all alllowed I2C commands
# the dict contains the function to execute on the sim
import os
import time
import math
import sys
import signal
import random
import pickle
import platform
class DartV2Basis():
# interrupt handler
def interrupt_handler(self,signal, frame):
print ('You pressed Ctrl+C! DART V2 will stop immediatly')
self.stop() # stop motors
self.end() # clean stop of simulator
sys.exit(0)
def __init__(self):
print ("Init Dartv2b Basis")
self.__dartSim = False
self.__debug = 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 gpio exists (not very robust)
# add test on processor type, real robot has armv7l
if (os.access("/sys/class/gpio/gpio266", os.F_OK)) \
and (platform.processor() == 'armv7l'):
print ("Work with real DART")
# if not the virtual robot is running in V-REP
else :
self.__dartSim = True
print ("Work with virtual DART on V-REP")
#print ("Python Sys Path",sys.path)
sys.path.append('./drivers')
if self.__dartSim:
sys.path.append('../vDartV2')
#print ("Python Sys Path Updated",sys.path)
# virtual DART requires multiprocessing and sockets
self.__vSimVar = None
if self.__dartSim:
import threading
import socket
import struct
# global simvar can be accessed with vsv.tSimVar dict
# eg simTime = vsv.tSimVar["vSimTime"]
sys.path.append('../vDartV2')
import vSimVar as vsv
self.__vSimVar = vsv.tSimVar
self.__vSimVar["vVoltageBin"] = self.battery_voltage_v2bin(self.__vSimVar["vVoltage"])
# Import modules to load the drivers
from drivers.trex import TrexIO
from drivers.sonars import SonarsIO
from drivers.encoders import EncodersIO
from drivers.imu9 import Imu9IO
self.__trex = TrexIO(sim=self.__dartSim)
#print (self.__vSimVar)
self.sonars = SonarsIO(sim=self.__dartSim, vsv=self.__vSimVar)
self.encoders = EncodersIO(sim=self.__dartSim, vsv=self.__vSimVar)
self.__trex.command["use_pid"] = 0 # 1
self.imu = Imu9IO(sim=self.__dartSim, vsv=self.__vSimVar)
# initiate communication thread with V-Rep
if self.__dartSim:
self.dtmx = -1.0
self.dtmn = 1e38
self.cnt_sock = 0
self.dta_sock = 0.0
self.upd_sock = False
import threading
import socket
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.server_address = (self.__HOST, self.__PORT)
srv = self.server_address
ev = self.vdart_ready
self.__simulation_alive = True
self.vrep = threading.Thread(target=self.vrep_com_socket,args=(srv,ev,))
self.vrep.start()
# wait for vdart to be ready
self.vdart_ready.wait()
print ("vDart ready ...")
# place your new class variables here
self.encoders_front_acq_time = time.time()
self.encoders_front_left_last = 0 # last value
self.encoders_front_right_last = 0
self.encoders_front_left_mem = 0 # memory value
self.encoders_front_right_mem = 0
enc_fr = self.get_front_encoders() # init last value
self.encoders_rear_acq_time = time.time()
self.encoders_rear_left_last = 0 # last value
self.encoders_rear_right_last = 0
self.encoders_rear_left_mem = 0 # memory value
self.encoders_rear_right_mem = 0
enc_rr = self.get_rear_encoders() # init last value
self.sonars_cardinal_acq_time = time.time()
self.sonars_cardinal_front = 99.9
self.sonars_cardinal_left = 99.9
self.sonars_cardinal_rear = 99.9
self.sonars_cardinal_right = 99.9
self.sonars_diagonal_acq_time = time.time()
self.sonars_diagonal_left = 99.9
self.sonars_diagonal_right = 99.9
self.speed_acq_time = time.time()
self.speed_left_last = 0
self.speed_right_last = 0
def dart_sim(self):
return self.__dartSim
def simulation_time(self):
simTime = time.time()
if self.__dartSim:
simTime = self.__vSimVar["vSimTime"]
return simTime
def vrep_com_socket(vdart,srv,ev):
import socket
import struct
# setup com format
comDataSize = 0
comData = []
comFmt = ""
#vDoLog
comFmt += "f"
comDataSize += 4
#vCmdSpeedLeft
comFmt += "f"
comDataSize += 4
#vCmdSpeedRight
comFmt += "f"
comDataSize += 4
#vCmdSpeedNew
comFmt += "f"
comDataSize += 4
#vEncoderRearLeftReset
comFmt += "f"
comDataSize += 4
#vEncoderRearRightReset
comFmt += "f"
comDataSize += 4
comFullFmt = '<BBHH'+comFmt # HH for data size in bytes
comDataSizeLow = comDataSize % 256
comDataSizeHigh = comDataSize // 256
# rx data :
# hd0,hd1,sz,lft
# simulationTime
# vSonarFront, vSonarRear,vSonarLeft, vSonarRight
# vSonarFrontLeft,vSonarFrontRight
# vEncoderFrontLeft, vEncoderFrontRight, vEncoderRearLeft, vEncoderRearRight
# vXRob,vYRob,vZRob,vXAcc,vYAcc,vZAcc,vXGyro,vYGyro,vZGyro
while True:
vDoLog = vdart.__vSimVar["vDoLog"]
vCmdSpeedLeft = vdart.__vSimVar["vCmdSpeedLeft"]
vCmdSpeedRight = vdart.__vSimVar["vCmdSpeedRight"]
vCmdSpeedNew = vdart.__vSimVar["vCmdSpeedNew"]
vEncoderRearLeftReset = vdart.__vSimVar["vEncoderRearLeftReset"]
vEncoderRearRightReset = vdart.__vSimVar["vEncoderRearRightReset"]
#print ("update vrep with sock")
#print (rob1a.simulation_alive,rob1a.speedLeft,rob1a.speedRight)
# Create a TCP/IP socket
t0 = time.time()
sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
try:
sock.connect(srv)
except:
print ("Simulation must be alive to execute your python program properly.")
print ("Type Ctrl-C to exit, start the simulation and then execute your python program.")
break
sock.settimeout(0.5)
#print (comFullFmt,ord(chr(59)),ord(chr(57)),
# comDataSizeLow,comDataSizeHigh,
# vDoLog,vCmdSpeedLeft,vCmdSpeedRight,vCmdSpeedNew,
# vEncoderRearLeftReset,vEncoderRearRightReset)
strSend = struct.pack(comFullFmt,ord(chr(59)),ord(chr(57)),
comDataSizeLow,comDataSizeHigh,
vDoLog,vCmdSpeedLeft,vCmdSpeedRight,vCmdSpeedNew,vEncoderRearLeftReset,vEncoderRearRightReset)
sock.sendall(strSend)
upd_sock = True
data = b''
try:
while len(data) < 6:
data += sock.recv(200)
except:
print ("socker error , duration is %f ms, try to reconnect !!!"%((time.time() - t0)*1000.0))
#sock.detach()
#sock.connect(srv)
#print ("socker error , type Ctrl-C to exit !!!")
#exit(0)
#print (len(data))
rxHeader = struct.unpack('<ccHH',data[0:6])
rxDataSize = 6 + rxHeader[2] + 256*rxHeader[3]
while True:
data_nw = sock.recv(2)
if len(data_nw) == 0:
#print (len(data_nw))
break
else:
data += data_nw
#print (len(data))
#print (rxHeader,rxDataSize,len(data))
if len(data) >= rxDataSize:
vrx = struct.unpack('<ccHHfffffffffffffffffffffff',data[0:rxDataSize])
vdart.vrep_update_sim_param(upd_sock,vrx[4:])
else:
print ("bad data length ",len(data))
sock.close()
vdart.cnt_sock = vdart.cnt_sock + 1
tsock = (time.time() - t0)*1000.0
vdart.dta_sock += tsock
if tsock > vdart.dtmx:
vdart.dtmx = tsock
if tsock < vdart.dtmn:
vdart.dtmn = tsock
dtm = vdart.dta_sock/float(vdart.cnt_sock)
#print ("tsock",tsock)
if vdart.__debug:
if (vdart.cnt_sock % 100) == 99:
print ("min,mean,max socket thread duration (ms) : ",vdart.dtmn,dtm,vdart.dtmx)
#time.sleep(0.2)
#print (dir(ev))
if vCmdSpeedNew != 0.0:
vdart.__vSimVar["vCmdSpeedNew"] = 0.0 # reset motor change cmd
ev.set()
#print (vdart.__simulation_alive)
if not vdart.__simulation_alive:
break
# update parameters (from new vales given by V-REP simulator)
def vrep_update_sim_param(self,upd_sock,vrx):
#print (upd_sock)
self.upd_sock = upd_sock
#print ("vrx",vrx)
simulationTime = vrx[0]
vSonarFront = vrx[1]
vSonarRear = vrx[2]
vSonarLeft = vrx[3]
vSonarRight = vrx[4]
vSonarFrontLeft = vrx[5]
vSonarFrontRight = vrx[6]
vEncoderFrontLeft = vrx[7]
vEncoderFrontRight = vrx[8]
vEncoderRearLeft = vrx[9]
vEncoderRearRight = vrx[10]
vHeading = vrx[11]
vXRob = vrx[12]
vYRob = vrx[13]
vZRob = vrx[14]
vXAcc = vrx[15]
vYAcc = vrx[16]
vZAcc = vrx[17]
vXGyro = vrx[18]
vYGyro = vrx[19]
vZGyro = vrx[20]
vEncoderRearLeftReset = vrx[21]
vEncoderRearRightReset = vrx[22]
#print ("update params in vSimVar from vrep values ...")
self.vSimTime = simulationTime
self.__vSimVar["vLocation"] = [vXRob,vYRob,vZRob]
self.__vSimVar["vAccelX"] = int(round(vXAcc*1000.0))
self.__vSimVar["vAccelY"] = int(round(vYAcc*1000.0))
self.__vSimVar["vAccelZ"] = int(round(vZAcc*1000.0))
self.__vSimVar["vGyroX"] = int(round(vXGyro*1000.0))
self.__vSimVar["vGyroY"] = int(round(vYGyro*1000.0))
self.__vSimVar["vGyroZ"] = int(round(vZGyro*1000.0))
self.__vSimVar["vMagX"] = int(round(math.cos(vHeading*math.pi/180.0)*1000.0))
self.__vSimVar["vMagY"] = int(round(math.sin(vHeading*math.pi/180.0)*1000.0))
self.__vSimVar["vMagZ"] = int(round(1000.0))
self.__vSimVar["vHeading"] = vHeading
self.__vSimVar["vEncoderFrontLeft"] = self.actual_front_encoders(vEncoderFrontLeft)
self.__vSimVar["vEncoderFrontRight"] = self.actual_front_encoders(vEncoderFrontRight)
self.__vSimVar["vEncoderRearLeft"] = self.actual_rear_encoders(vEncoderRearLeft)
self.__vSimVar["vEncoderRearRight"] = self.actual_rear_encoders(vEncoderRearRight)
if vEncoderRearLeftReset == -1.0:
self.__vSimVar["vEncoderRearLeftReset"] = 0.0
#print ("-- reset left")
if vEncoderRearRightReset == -1.0:
self.__vSimVar["vEncoderRearRightReset"] = 0.0
#print ("-- reset right")
self.__vSimVar["vSonarFrontLeft"] = self.actual_sonar(vSonarFrontLeft)
self.__vSimVar["vSonarFrontRight"] = self.actual_sonar(vSonarFrontRight)
self.__vSimVar["vSonarLeft"] = self.actual_sonar(vSonarLeft)
self.__vSimVar["vSonarRight"] = self.actual_sonar(vSonarRight)
self.__vSimVar["vSonarFront"] = self.actual_sonar(vSonarFront)
self.__vSimVar["vSonarRear"] = self.actual_sonar(vSonarRear)
#self.__trex.status["left_encoder"] = self.actual_front_encoders(vEncoderFrontLeft)
#self.__trex.status["right_encoder"] = self.actual_front_encoders(vEncoderFrontRight)
# battery level v->bin
def battery_voltage_v2bin_old(self,v):
vb = int(round((1024*0.23*v)/5.0))
return vb
def battery_voltage_v2bin(self,v):
vb = int(round((1024*v)/(5.0*4.3)))
return vb
# actual sonar functions
def actual_sonar(self,v):
v = round(v*100)
if random.random() < 0.005: # i2c failure probability of 0.5 %
v = -1
return v
# actual encoder functions
def actual_front_encoders(self,v): # 16 bits signed
iv = int(round(v+0.5)) % 65536 # put on 16 bits
iv0 = iv
if iv > 32767: # add the sign
iv -= 65536
return iv
def actual_rear_encoders(self,v): # 16 bits unsigned
iv = int(round(v+0.5)) % 65536 # put on 16 bits
while iv < 0:
iv += 65536
#if random.random() < 0.005: # i2c failure probability of 0.5 %
# iv = -1
return iv