def followTheRightWall(self,rb,dist,speed,max_time = 120):
setPoint = dist
nominalSpeed = speed
derivOk = False
fltFront = sonar_filter.SonarFilter()
fltFront.set_iir_a(0.7)
fltFront.reset_iir()
fltRight = sonar_filter.SonarFilter()
fltRight.set_iir_a(0.7)
fltRight.reset_iir()
kp = 10
kd = 1000
deltaSpeedMax = 10
distObstacle = 0.5
loopIterationTime = 0.05
global_t0 = time.time()
while True:
t0 = time.time()
distWall_raw = rb.get_sonar("right")
distWall = fltRight.median_filter(distWall_raw)
distWall = fltRight.iir_filter(distWall)
ControlError_Right = setPoint - distWall
if derivOk :
derivError = ControlError_Right - lastError
deltaSpeed = kp * ControlError_Right + kd * derivError
else :
deltaSpeed = kp * ControlError_Right
#print("deltaSpeed = {0}".format(deltaSpeed))
if deltaSpeed > deltaSpeedMax:
deltaSpeed = deltaSpeedMax
if deltaSpeed < - deltaSpeedMax:
deltaSpeed = - deltaSpeedMax
rb.set_speed(nominalSpeed - deltaSpeed, nominalSpeed + deltaSpeed)
lastError = ControlError_Right
derivOk = True
distFront_raw = rb.get_sonar("front")
distFront = fltFront.median_filter(distFront_raw)
distFront = fltFront.iir_filter(distFront)
#print(distFront_raw,distFront,distObstacle)
ControlError_Front = distObstacle - distFront
global_time = time.time() - global_t0
print(global_time)
if (distFront_raw != 0.0 and ControlError_Front > 0) or (distWall_raw == 0.0) or (global_time > max_time) :
rb.stop()
break
execTime = time.time() - t0
deltaTime = loopIterationTime - execTime
if deltaTime > 0:
time.sleep(deltaTime)
def followWallsRight(self, rb, setPoint=0.5, nominalSpeed=25):
kp = 10
kd = 1000
lastError = 0
deriveOk = False
filter_sonar = flt.SonarFilter()
while True:
#distFront = rb.get_sonar('front')
#distFront = filter_sonar.median_filter(distFront)
distWall = rb.get_sonar('right')
distWall = filter_sonar.median_filter(distWall)
print(distWall)
controlError = setPoint - distWall
if deriveOk:
deriveError = controlError - lastError
deltaSpeed = kp * controlError + kd * deriveError
else:
deltaSpeed = kp * controlError
deltaSpeedMax = 20
if deltaSpeed > deltaSpeedMax:
deltaSpeed = deltaSpeedMax
elif deltaSpeed < -deltaSpeedMax:
deltaSpeed = -deltaSpeedMax
rb.set_speed(nominalSpeed - deltaSpeed, nominalSpeed + deltaSpeed)
lastError = controlError
deriveOk = True
if distWall == 0:
rb.stop()
break
import rob1a_v01 as rob1a
import numpy as np
import matplotlib.pyplot as plt
import time
import sonar_filter
if __name__ == "__main__":
rb = rob1a.Rob1A()
flt = sonar_filter.SonarFilter()
rb.log_file_on()
# if you want to change the parameters of the filter
# uncomment and modify the following lines (depending on your filter choice)
# flt.set_ma_order(N) # set MA filter order (if you choose MA)
# flt.set_iir_a(0.9) # set a1 coefficient (if you choose IIR)
# get raw data
n = 100
tt = np.zeros(n) # time axis
ts = np.zeros(n) # raw data
tf = np.zeros(n) # filtered data
loop_time = 0.100 # 100 ms (or 10 Hz)
tStart = time.time()
for i in range(n):
t0 = time.time()
tt[i] = time.time() - tStart
ts[i] = rb.get_sonar("front")
# select your filter here by removing comment sign
#tf[i] = flt.ma_filter(ts[i])
#tf[i] = flt.iir_filter(ts[i])
lastError = controlError
derivOk = True
t1 = time.time()
if i > 8:
distWallFront = rb.get_sonar("front") - (0.03 + 0.10081346333)
i = i + 1
print(distWallLeft)
#traitement pour l'arret
time.sleep(abs(0.5 - (t1 - t0)))
if __name__ == "__main__":
rb = rob1a.Rob1A() # create a robot (instance of Rob1A class)
ctrl = robot_control.RobotControl() # create a robot controller
flt = sonar_filter.SonarFilter() #create filter
# put here your code to solve the challenge
# ..
# on fait que le robot avance de 0.5 metre
ctrl.goLineOdometer(rb, 0.5, 70) #il va a 80 pour cent de sa vitesse max
time.sleep(0.5)
#on le fait tourner de 90degres a droite
ctrl.inPlaceTurnRight(rb, 90)
time.sleep(0.5)
#on le fait avancer de 0.5metres a 80%
ctrl.goLineOdometer(rb, 0.5, 70)
time.sleep(0.5)
#on le fait tournee sur la gauche
ctrl.inPlaceTurnLeft(rb, 90)