initial commit

.gitignore

+V-REP_PRO_EDU_V3_6_2_Ubuntu18_04

README.md

+# 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 ...**

py/dartv2b.py

+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()
+    
+
+
+

py/drivers/dartv2b_basis.py

+#!/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