class Camera:
def __init__(self):
config = configparser.ConfigParser()
config.read("./config.ini")
HIntfNum = config.getint("camera", "HIntfNum")
HInitPosition = config.getint("camera", "HInitPosition")
HMinPosition = config.getint("camera", "HMinPosition")
HMaxPosition = config.getint("camera", "HMaxPosition")
HSpeed = config.getint("camera", "HSpeed")
# Vertical direction control parameters
VIntfNum = config.getint("camera", "VIntfNum")
VInitPosition = config.getint("camera", "VInitPosition")
VMinPosition = config.getint("camera", "VMinPosition")
VMaxPosition = config.getint("camera", "VMaxPosition")
VSpeed = config.getint("camera", "VSpeed")
self.HCameraControl = Steering(HIntfNum, HInitPosition, HMinPosition,
HMaxPosition, HSpeed)
self.VCameraControl = Steering(VIntfNum, VInitPosition, VMinPosition,
VMaxPosition, VSpeed)
def cameraRotate(self, direction):
if direction == "A":
self.HCameraControl.forwardRotation()
elif direction == "D":
self.HCameraControl.reverseRotation()
elif direction == "W":
self.VCameraControl.forwardRotation()
elif direction == "S":
self.VCameraControl.reverseRotation()
elif direction == "R":
self.HCameraControl.reset()
self.VCameraControl.reset()
else:
print(
"Your input for camera direction is wrong, please input: D, A, W, S or RESET!"
)
class Sense():
def __init__(self):
config = configparser.ConfigParser()
config.read("config.ini")
trigger = config.getint("ultrasonic", "Trigger")
echo = config.getint("ultrasonic", "Echo")
servoNum = config.getint("steering", "servoNum")
InitPosition = config.getint("steering", "InitPosition")
minPosition = config.getint("steering", "MinPosition")
maxPosition = config.getint("steering", "maxPosition")
speed = config.getint("steering", "speed")
self.IN_1 = config.getint("car", "IN1")
self.IN_2 = config.getint("car", "IN2")
self.IN_3 = config.getint("car", "IN3")
self.IN_4 = config.getint("car", "IN4")
self.Sonic = Ultrasonic(trigger, echo)
self.Servo = Steering(servoNum, InitPosition, minPosition, maxPosition,
speed)
self.motor = Motor(self.IN_1, self.IN_2, self.IN_3, self.IN_4)
#get obstacles type
def detection(self):
distance = self.ask_distance()
#NO Obstacles or 35cm safe distance type 0
if distance >= 30:
return "Fgo"
#Obstacles ahead
#self.Infrared.check_distance() == "Warning"
else:
l_d = self.ask_distance_l()
r_d = self.ask_distance_r()
#Obstacles ahead&&right =safe type a
if ((l_d >= 30) and (r_d >= 30)):
if (l_d > r_d):
return "Lgo"
else:
return "Rgo"
#Obstacles ahead&&right R>L type b
elif ((l_d <= 30) and (r_d > 30)):
return "Rgo"
#Obstacles ahead&&right L>R type c
elif ((l_d > 30) and (r_d <= 30)):
return "Lgo"
#Obstacles ahead&&right L&R<safe type d
elif ((l_d < 30) and (r_d < 30)):
return "Bgo"
def ask_distance(self):
self.Servo.reset()
return self.Sonic.get_distance()
def ask_distance_l(self):
self.motor.stop()
self.Servo.turnleft()
time.sleep(0.1)
distance = self.Sonic.get_distance()
self.Servo.reset()
return distance
def ask_distance_r(self):
self.motor.stop()
self.Servo.turnright()
time.sleep(0.1)
distance = self.Sonic.get_distance()
self.Servo.reset()
return distance
class Main():
ub = UltraBorg3.UltraBorg()
tb = ThunderBorg3.ThunderBorg()
MODE_STRAIGHT_LINE_SPEED = 0
MODE_OVER_THE_RAINBOW = 1
MODE_MAZE_SOLVE = 2
MODE_DERANGED_GOLF = 3
MODE_DUCK_SHOOT = 4
MODE_OBSTACLE_COURSE = 5
MODE_PI_NOON = 6
MODE_TEST = 99
mode = MODE_OBSTACLE_COURSE
def __init__(self):
self.ub.Init()
self.tb.Init()
self.t1 = datetime.now()
self.tickSpeed = 0.05
self.us = Ultrasonics(self.ub)
self.motors = Motors(self.tb, self.ub, self.tickSpeed)
self.steering = Steering(self.tb, self.ub, self.tickSpeed)
self.teleLogger = Telemetry("telemetry", "csv").get()
self.ptc = PanTiltController(self.ub, 270, 135)
self.joystick_controller = JoystickController(self.tb, self.ub)
battMin, battMax = self.tb.GetBatteryMonitoringLimits()
battCurrent = self.tb.GetBatteryReading()
print('Battery monitoring settings:')
print(' Minimum (red) %02.2f V' % (battMin))
print(' Half-way (yellow) %02.2f V' % ((battMin + battMax) / 2))
print(' Maximum (green) %02.2f V' % (battMax))
print()
print(' Current voltage %02.2f V' % (battCurrent))
print(' Mode %s' % (self.mode))
print()
def log(self):
if (self.motors.speed != 0):
self.teleLogger.info(
'%(left)f, %(front)f, %(right)f, %(back)f, %(distanceMoved)f, %(forwardSpeed)f, %(direction)s, %(degree)f, %(ratio)f',
{
"left": self.us.left,
"front": self.us.front,
"right": self.us.right,
"back": self.us.back,
"distanceMoved": self.motors.distanceMoved,
"forwardSpeed": self.motors.forwardSpeed,
"direction": self.steering.going,
"degree": self.steering.steeringPosition,
"ratio": self.steering.sideRatio
})
def run(self):
try:
if (self.mode == self.MODE_STRAIGHT_LINE_SPEED):
print("Straight line speed")
self.modeStraightLineSpeed()
elif (self.mode == self.MODE_OVER_THE_RAINBOW):
print("Rainbow")
self.modeOverTheRainbow()
elif (self.mode == self.MODE_DERANGED_GOLF
or self.mode == self.MODE_PI_NOON
or self.mode == self.MODE_OBSTACLE_COURSE):
print("Joystick control: " + str(self.mode))
self.joystick_controller.run()
else:
# Wait for Buttons
print('Pan -62 = ' + str(self.ptc.pan(-62)))
time.sleep(1)
print('Pan 0 = ' + str(self.ptc.pan(0)))
time.sleep(1)
#
# print('Pan 62 = ' + str(self.ptc.pan(62)))
# time.sleep(1)
#
# print('Pan absolute 1.0 = ' + str(self.ptc.pan_absolute(1.0)))
# time.sleep(1)
#
# print('Pan absolute -1.0 = ' + str(self.ptc.pan_absolute(-1.0)))
# time.sleep(1)
#
# print('Pan absolute 0.0 = ' + str(self.ptc.pan_absolute(0.0)))
# time.sleep(1)
#
# print('Tilt -30 = ' + str(self.ptc.tilt(-30)))
# time.sleep(1)
# print('Tilt 0 = ' + str(self.ptc.tilt(0)))
# time.sleep(1)
#
# print('Tilt 30 = ' + str(self.ptc.tilt(30)))
# time.sleep(1)
#
# print('Tilt absolute 0.6 = ' + str(self.ptc.tilt_absolute(0.6)))
# time.sleep(1)
#
# print('Tilt absolute -0.6 = ' +
# str(self.ptc.tilt_absolute(-0.6)))
# time.sleep(1)
#
# print('Tilt absolute 0.0 = ' + str(self.ptc.tilt_absolute(0.0)))
# time.sleep(1)
except KeyboardInterrupt:
# User has pressed CTRL+C
self.ub.SetServoPosition2(0)
t2 = datetime.now()
delta = t2 - self.t1
print("Run complete in : " + str(delta.total_seconds()))
print('Done')
if (self.motors):
self.motors.shutdown()
except Exception as e:
tb = traceback.format_exc()
e = sys.exc_info()[0]
print(tb)
if (self.motors):
self.motors.shutdown()
def modeStraightLineSpeed(self):
self.straight_line_speed = StraightLineVision(self.steering,
self.motors)
self.teleLogger.info(
'left, front, right, back, distanceMoved, forwardSpeed, direction, steering position, ratio'
)
self.steering.reset()
slVa = VisionAttributes()
slVa.startTiltAngle = 0.6
slVa.startPanAngle = 0
slVa.targetMinSize = 1000
slVa.targetMaxSize = 18000
slVa.minPanAngle = -0.5
slVa.maxPanAngle = 0.5
slVa.colour = Vision.COLOUR_WHITE
slVa.targetColorPattern = Vision.COLOUR_WHITE
slVa.topSpeed = 0.6
slVa.topSpinSpeed = 1.0
self.ptc.tilt(0.5)
slsPtc = PanTiltController(self.ub, 270, 135)
slsPtc.initPanServo(5000, 1000)
self.straight_line_speed.initialise(slVa, slsPtc)
self.motors.stop()
prev_block_position = None
t1 = datetime.now()
self.straight_line_speed.track(self.straight_line_speed.COLOUR_WHITE)
t2 = datetime.now()
delta = t2 - self.t1
print("Run complete in: " + str(delta.total_seconds()))
def modeOverTheRainbow(self):
self.vision = Vision(self.steering, self.motors)
slVa = VisionAttributes()
slVa.startTiltAngle = 0.12
slVa.startPanAngle = -1.00
slVa.targetMinSize = 20
slVa.targetMaxSize = 2200
slVa.minPanAngle = -1.0
slVa.maxPanAngle = 1.0
slVa.targetColorPattern = Vision.COLOUR_WHITE
slVa.topSpeed = 1.0
slVa.topSpinSpeed = 1.0
self.motors.move(-1, -1, 0.3)
time.sleep(0.8)
self.motors.stop()
rainbowPtc = PanTiltController(self.ub, 270, 135)
rainbowPtc.initPanServo(5000, 1000)
self.vision.initialise(slVa, rainbowPtc)
time.sleep(0.5)
self.vision.scan()
print("Scan Complete")
index = 0
prev_position = 0
for ball_position in self.vision.ball_positions:
ball_position = self.vision.ball_positions[0]
print("Size: " + str(ball_position.size) + ', x :' +
str(ball_position.x) + ', y :' + str(ball_position.y) +
', pan-position :' + str(ball_position.pan_position) +
', angle : ' + str(ball_position.pan_position * 135) +
', Colour:' + str(ball_position.colour))
if (index > 0):
prev_position = self.vision.ball_positions[index -
1].pan_position
index = index + 1
self.vision.goto_ball_position(ball_position, prev_position)
class Camera:
def __init__(self):
'''
Read config file to init camera's parameter
'''
config = configparser.ConfigParser()
config.read("config.ini")
self.statu=""
# Horiazonal direction control parameters
HIntfNum = config.getint("camera", "HIntfNum")
HInitPosition = config.getint("camera", "HInitPosition")
HMinPosition = config.getint("camera", "HMinPosition")
HMaxPosition = config.getint("camera", "HMaxPosition")
HSpeed = config.getint("camera", "HSpeed")
# Vertical direction control parameters
VIntfNum = config.getint("camera", "VIntfNum")
VInitPosition = config.getint("camera", "VInitPosition")
VMinPosition = config.getint("camera", "VMinPosition")
VMaxPosition = config.getint("camera", "VMaxPosition")
VSpeed = config.getint("camera", "VSpeed")
self.HCameraControl = Steering(
HIntfNum, HInitPosition, HMinPosition, HMaxPosition, HSpeed)
self.VCameraControl = Steering(
VIntfNum, VInitPosition, VMinPosition, VMaxPosition, VSpeed)
def cameraRotate(self, direction):
'''
This method is used to contorl the camera's rotating
The value of parameter direction and its meaning as follow:
HR - Turn right
HL - Turn left
VU - Turn upward
VD - Turn downword
'''
if direction == "HL":
self.HCameraControl.forwardRotation()
config = configparser.ConfigParser()
config.read("config.ini")
H = self.HCameraControl.position
config.set("camera", "HInitPosition", str(H))
with open("config.ini", "w+") as f:
config.write(f)
self.statu="LEFT"
elif direction == "HR":
self.HCameraControl.reverseRotation()
config = configparser.ConfigParser()
config.read("config.ini")
H = self.HCameraControl.position
config.set("camera", "HInitPosition", str(H))
with open("config.ini", "w+") as f:
config.write(f)
self.statu = "RIGHT"
elif direction == "VU":
self.VCameraControl.forwardRotation()
config = configparser.ConfigParser()
config.read("config.ini")
V = self.VCameraControl.position
config.set("camera", "VInitPosition", str(V))
with open("config.ini", "w+") as f:
config.write(f)
self.statu = "UP"
elif direction == "VD":
self.VCameraControl.reverseRotation()
config = configparser.ConfigParser()
config.read("config.ini")
V = self.VCameraControl.position
config.set("camera", "VInitPosition", str(V))
with open("config.ini", "w+") as f:
config.write(f)
self.statu = "DOWN"
elif direction == "RESET":
self.HCameraControl.reset()
self.VCameraControl.reset()
config = configparser.ConfigParser()
config.read("config.ini")
H = 80
V = 50
config.set("camera", "HInitPosition", str(H))
config.set("camera", "VInitPosition", str(V))
with open("config.ini", "w+") as f:
config.write(f)
self.statu = "RESET"
else:
print(
"Your input for camera direction is wrong, please input: HR, HL, VU, VD or RESET!")
def __del__(self):
print(self.statu+" over and "+"camera has been deleted")