def turnRightTestCase():
count = 0
velocityData.append(myBoat.getVelocity())
timeData.append(count * samplePeriod)
headingData.append(myBoat.getHeading())
count = count + 1
for i in range(40):
myBoat.updateSpeed(0.0, 10.0, 0.0)
velocityData.append(myBoat.getVelocity())
headingData.append(myBoat.getHeading() % (2 * math.pi))
timeData.append(count * samplePeriod)
logThisLine(count)
count = count + 1
for i in range(40):
myBoat.updateSpeed(10.0, 10.0, 0.0)
velocityData.append(myBoat.getVelocity())
headingData.append(myBoat.getHeading() % (2 * math.pi))
timeData.append(count * samplePeriod)
logThisLine(count)
count = count + 1
plt.figure(2)
plt.plot(timeData, headingData)
plt.ylabel('Heading (radians)')
plt.xlabel('Time (s)')
plt.show()
def backwardTestCase():
count = 0
velocityData.append(myBoat.getVelocity())
timeData.append(count * samplePeriod)
count = count + 1
for i in range(40):
myBoat.updateSpeed(-10.0, -10.0, 0.0)
velocityData.append(myBoat.getVelocity())
timeData.append(count * samplePeriod)
logThisLine(count)
count = count + 1
for i in range(40):
myBoat.updateSpeed(0.0, 0.0, 0.0)
velocityData.append(myBoat.getVelocity())
timeData.append(count * samplePeriod)
logThisLine(count)
count = count + 1
plt.plot(timeData, velocityData)
plt.ylabel('Velocity (m/s)')
plt.xlabel('Time (s)')
plt.figure(1)
plt.show()
def control_loop(num):
n = 0
startTime = time.time()
checkValue = 1
while 1 > 0:
if (time.time() > startTime):
startTime += 0.01
global count
global headingData
global velocityData
global timeData
headingData.append(myBoat.getHeading())
velocityData.append(myBoat.getVelocity())
timeData.append(count * samplePeriod)
count = count + 1
tempComplex = complex(
XCoord - myBoat.getXCoordinate(), YCoord -
myBoat.getYCoordinate()) #rename to destinationX, destinationY
tempPolar = cmath.polar(tempComplex)
PIDAngle = PIDA.regulator(tempPolar[1] - myBoat.getHeading())
angle = tempPolar[1]
if angle < 0:
angle = angle + 2 * math.pi
if checkValue > tempPolar[0]:
checkValue = 1.5
print("We are in the close distance step")
#More vector calculus YAY!
vf = tempPolar[0] * math.cos(tempPolar[1] -
myBoat.getHeading())
vs = tempPolar[0] * math.sin(tempPolar[1] -
myBoat.getHeading())
closeXspeed = PIDX.regulator(vf)
closeYspeed = PIDY.regulator(vs)
myBoat.updateSpeed(closeXspeed, closeXspeed, closeYspeed,
"#2C2020")
else:
myBoat.updateSpeed(FarDistanceModeSpeed + PIDAngle,
FarDistanceModeSpeed - PIDAngle, 0)
checkValue = 1
n = n + 1
if n > 400:
break
