def utilsMotors():
motors = Motors()
while True:
print "---------------------------"
print "utils for Motors"
print "---------------------------"
print """
1 --> calibrate ESC
2 --> test motor
0 --> go back
"""
if choice2 == None:
choice = raw_input('please set action and press enter\n')
else: choice = choice2
if choice == '1':
choice = raw_input('please set the motor id to ccalibrate\n')
if not choice == 'all' : choice = int(choice)
motors.calibrate(choice)
elif choice == '2':
choice = raw_input('please set the motor id to test\n')
motors.test(int(choice))
elif choice == '0':
return
else:
print "This choice is not reconized, try again"
def __init__(self):
self.motors = Motors()
#self.marg = MARG()
# not sure if I want this here, but will add for now
self.server = Server()
self.prev_msg = b''
def __init__(self, verbose=False):
self.verbose = verbose
self.status_counter = 0
self.state = self.STATE_WAITING
self.currenttime = 0
self.flex_fish_limit = self.FLEX_FISH_LIMIT
self.player = Player(self, verbose)
self.adc_sensors = AdcSensors(self, verbose)
self.motors = Motors(self, verbose)
self.web_connection = WebConnection(self, verbose)
self.gps_tracker = GpsTracker(self, verbose)
self.video = Video(self, verbose)
# Initial valus for settings
# Speed: (0-1). 1 = Full speed ahead
# Turn = -1 - +1 = +1 Only left motor on (turn to right)
# 0 Both motors same speed
# -1 Only right motor on (turn to left)
self.speed = 0.0
self.turn = 0.0
# speed style examples:
# - Constant speed = (low_speed_percent = 100)
# - Stop and go jigging with 6 sec motor on and 4 sec stop. low_speed_percent = 0,speed_change_cycle = 10, speed_motors_full_percent = 60
# - Trolling with 10 sec half speed and 5 sec full speed. low_speed_percent = 50, speed_change_cycle = 15, speed_motors_full_percent = 66.66
self.speed_change_cycle = 0
self.speed_motors_full_percent = 100
self.low_speed_percent = 0
# Play music or not
self.play_music = False
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 test1():
ZumoButton().wait_for_press()
m = Motors()
m.turn_right(90)
m.turn_left(180)
m.turn_right(360)
m.turn_left(360)
def operationalize(self):
#Oppretter motors-objekt
m = Motors()
#henter inn recommendation fra arbitrator og plugger inn i motors
m.set_value(self.values, 0.5)
#Går gjennom listen og bruker den til å utføre de forskjellige handlingene fra motors
for i in range(len(self.motors)):
self.motors[i].set_value(self.values, 0.5)
def __init__(self):
self.motor = Motors()
self.value = -1
self.speed = 0.35
self.funcs = {
"left": self.left,
"right": self.right,
"random": self.random,
"rewind": self.rewind
}
def __init__(self, a_star=None):
self.a_star = a_star or AStar()
self.encoders = Encoders(self.a_star)
self.odometer = Odometer(self.encoders)
self.motors = Motors(self.a_star, self.odometer)
self.camera = Camera()
self.log = logging.getLogger('romi')
self.motors.stop()
def main():
gamepad = Gamepad()
motors = Motors()
iks = []
for i in range(4):
iks.append(DeltaIK())
controller = ManualController(iks)
angles = 8 * [0.0]
kPs = 8 * [0.1]
frametime = 1.0 / 30.0
print("Ready.")
while True:
try:
s = time.time()
if not gamepad.process_events():
print("Controller disconnected, waiting for reconnect...")
while not gamepad.reconnect():
time.sleep(0.5)
print("Controller reconnected.")
if gamepad.is_a_down():
print("Shutdown button (A) has been pressed, shutting down...")
break
pad_y = gamepad.get_left_thumbstick_y()
controller.step(pad_y, frametime)
# Map IK leg results to motors
for i in range(4):
angles[legMap[i]
[0]] = legDirs[i][0] * (iks[i].angle -
(np.pi / 2.0 - iks[i].A))
angles[legMap[i]
[1]] = legDirs[i][1] * (iks[i].angle +
(np.pi / 2.0 - iks[i].A))
motors.sendCommands(angles, kPs)
time.sleep(max(0, frametime - (time.time() - s)))
except Exception as e:
print(e)
break
print("-- Program at End --")
def shoot_panorama(shots=5):
camera = Camera()
motors = Motors()
s = 1
im = IMR.Imager(image=camera.update()).scale(s, s)
rotation_time = 3 / shots # At a speed of 0.5(of max), it takes about 3 seconds to rotate 360 degrees
for i in range(shots - 1):
motors.right(0.5, rotation_time)
im = im.concat_horiz(IMR.Imager(image=camera.update()))
im.dump_image("/root/Oving6_Plab/basic_robot/bilder/test.png")
return im
def calc():
m = Motors()
while True:
ZumoButton().wait_for_press()
sleep(1)
m.set_value([-1, 1], 90 * 0.00228)
sleep(1)
m.set_value([-1, 1], 90 * 0.00228)
sleep(1)
m.set_value([-1, 1], 90 * 0.00228)
sleep(1)
m.set_value([-1, 1], 90 * 0.00228)
def __init__(self):
# Name of the controller
self.name = "PAUL controller for Raspberry Pi"
# Current Speed of the robot
self.speed = 0
# Speed that the robot will travel up (note negative speed goes up)
self.up_speed = -70
#Speed that the robot will travel down (note negative speed goes down)
self.down_speed = 70
# Threshold reading on top distance sensors before triggering
self.top_threshold = 1.9
# Threshold reading on bottom distance sensors before triggering
self.bottom_threshold = 2.6
# Note: The following should only be set if distance sensors are not attached,
# they should be set to 0 if sensors are attached.
self.top_ds_reading = 0
self.bottom_ds_reading = 0
# Create an instance of the Motors class used in SDP
self.mc = Motors()
#Sensors
#define array of digital inputs to store readings from the bump sensors
self.bump_sensors = [DigitalInput() for i in range (0,6)]
#addressing the channels for the bump sensors
#0-2 are top sensors
#3-5 are bottom channels
for i in range(0,6):
self.bump_sensors[i].setChannel(i)
self.bump_sensors[i].openWaitForAttachment(5000)
#define array of analogue inputs to store readings from the ir sensors
self.ir_sensors = [VoltageInput() for i in range (0,6)]
#addressing the channels for the ir sensors
#0-2 are top sensors
#3-5 are bottom channels
for i in range(0,6):
self.ir_sensors[i].setChannel(i)
#this line will basically call the event handler when the readings change
self.ir_sensors[i].openWaitForAttachment(5000)
#Light
# connect to pin 12(slot PWM)
PIN = 18
# For Grove - WS2813 RGB LED Strip Waterproof - 30 LED/m
# there is 30 RGB LEDs.
COUNT = 60
self.strip = GroveWS2813RgbStrip(PIN, COUNT)
def test():
ZumoButton().wait_for_press()
ultra = Ultrasonic()
m = Motors()
ultra.update()
tall = ultra.get_value()
print("tall: ", tall)
while tall < 5.0:
m.backward(0.2, 1)
print(tall)
ultra.update()
tall = ultra.get_value()
print(tall)
class motob:
"""Motob klasse har en motor"""
def __init__(self):
"""Create empty list as value"""
self.motor = Motors()
# ("l",45,+0.5)
self.value_drive = ["l", 0, 0] # a list [direction, degrees, speed]
self.value_halt = False # The value of the halt flag
#self.dir_list = ['l','r','f','b']
def update(self, mot_roc):
"""sette en ny value"""
self.value_drive = mot_roc[0]
self.value_halt = mot_roc[1]
def operationalize(self, dur=0.05):
"""apply value, r: Right, l:Left, f:Forward, b:Backward"""
#dur = 3
turn_speed = self.value_drive[1] / 100
if turn_speed > 1:
turn_speed = 1
# cond_left_right = False
# if self.value_drive[1] == 0:
# If we are turning 0 degrees, cond_left_right = True
# cond_left_right = True
#print("I am in loop")
if self.value_drive[
0] == 'l' and self.value_drive[1] > 0 and not self.value_halt:
# turn left and we are turning, but why not call left?
self.motor.set_value((-self.value_drive[2], self.value_drive[2]),
turn_speed) # 10 eller dur
#self.motor.left(self.value_drive[2], dur)
elif self.value_drive[
0] == 'r' and self.value_drive[1] > 0 and not self.value_halt:
# turn right, and we are turning, try motors.right??
self.motor.set_value((self.value_drive[2], -self.value_drive[2]),
turn_speed) # 10 eller dur
#self.motor.right(self.value_drive[2], dur)
elif self.value_drive[0] == 'f' and self.value_drive[
1] == 0 and not self.value_halt:
# Drive forward
self.motor.forward(abs(self.value_drive[2]), dur)
#self.motor.set_value((self.value_drive[2], self.value_drive[2]), self.value_drive[2])
elif self.value_drive[0] == 'b' and self.value_drive[
1] == 0 and not self.value_halt:
# Drive backwards
self.motor.backward(abs(self.value_drive[2]), dur)
#self.motor.set_value((-self.value_drive[2], -self.value_drive[2]), self.value_drive[2])
elif self.value_halt:
# If we are done, the haltflag is True, stop
self.motor.stop()
class Motob:
"""
The motor object (motob) manifests an interface between a behavior and one or more motors (a.k.a. actuators).
It contains (at least) the following instance variables:
1. motors - a list of the motors whose settings will be determined by the motob.
2. value - a holder of the most recent motor recommendation sent to the motob.
Its primary methods are:
1. update - receive a new motor recommendation, load it into the value slot, and operationalize it.
2. operationalize - convert a motor recommendation into one or more motor settings, which are sent to
the corresponding motor(s)
"""
def __init__(self):
self.motor = Motors()
self.value = None
def update(self, recommendation):
self.value = recommendation
self.operationalize()
def operationalize(self):
"""
convert motor recommendation into motor setting
send to motor
format value: [direction, speed, duration]
:return:
"""
for element in self.value:
if element[0] == "f":
self.motor.forward(element[1], element[2])
elif element[0] == "b":
self.motor.backward(element[1], element[2])
elif element[0] == "l":
self.motor.left(element[1], element[2])
elif element[0] == "r":
self.motor.right(element[1], element[2])
def vsvingsakte():
speed = 0.5
ZumoButton().wait_for_press()
motors = Motors()
motors.set_value((speed / 2, speed / 2), 1)
motors.set_value((speed, -speed), 0.1)
motors.set_value((speed / 2, speed / 2), 1)
def vsvingrask():
speed = 0.5
ZumoButton().wait_for_press()
motors = Motors()
motors.set_value((speed / 2, speed / 2), 1)
motors.set_value((0.3, -0.3), 1)
motors.set_value((speed / 2, speed / 2), 1)
def __init__(self):
"""
position: init position dict with keys:
- "angle": a X-axis relative angle.
- "point": the first position.
"""
# self._us_sensors = RangeSensor(4)
self._motors = Motors(5)
self._kinematic = Kinematics(6)
self._us = RangeSensor(4)
logging.info('Building the graph map.')
# self._graph = build_graph(robot_diagonal)
logging.info('Finished to build the graph map.')
self._blocking_servo = -1
class Move:
def __init__(self):
# initialize the status
self.dist = 1
self.dist_prev = 2
self.act_prev = 'r'
# init motors, establish bluetooth
self.motors = Motors()
self.motors.arm_move((90,90))
def targetDist(self,targetCenter, imgCenter):
# range from 0 to 1
h = float(imgCenter[0]-targetCenter[0])/imgCenter[0]
v = float(imgCenter[1]-targetCenter[1])/imgCenter[1]
return h
def findmarker(self, img):
# return > 0 to end finding process
print self.dist
if abs(self.dist)>abs(self.dist_prev):
# the result got worse
if self.act_prev == 'r':
self.act = 'l'
if self.act_prev == 'l':
self.act = 'r'
if abs(self.dist)<=abs(self.dist_prev):
# the result got better or same, keep doing
self.act = self.act_prev
self.motors.turn(self.act)
import time
time.sleep(0.1)
self.act_prev = self.act
self.dist_prev = self.dist
mlist = marker().find(img, debug = 0, show = 0)
#default dist, which means the not found response
if len(mlist)==1:
num, pos = mlist[0]
center = tuple(np.int0( np.mean( pos.reshape(4,2), axis = 0)))
img_center = tuple( img.shape[:2])
img_center = (img_center[1]/2, img_center[0]/2)
self.dist = self.targetDist(center, img_center)
else:
self.dist = 2
return 0
def start():
bbcon = BBCON()
arb = Arbitrator(bbcon)
motor = Motors()
reflect_sens = ReflectanceSensors(False)
cam = Camera()
ir = IR()
ultra = Ultrasonic()
bbcon.add_motobs(motor)
bbcon.set_arb(arb)
bbcon.add_behaviour(AvoidObj(bbcon, ultra, ir))
bbcon.add_behaviour(Behaviour_line_follower(bbcon, reflect_sens))
bbcon.add_behaviour(fub(bb=bbcon))
#behaviour avoid blue, has to be added last, do not change this, will screw up bbcon code
bbcon.add_behaviour(Behaviour_avoid_blue(bb=bbcon, cam=cam, ultra=ultra))
bbcon.add_sensob(reflect_sens)
bbcon.add_sensob(ir)
bbcon.add_sensob(ultra)
#cam has to be added last, will screw up bbcon code if not
bbcon.add_sensob(cam)
butt = ZumoButton()
butt.wait_for_press()
print("Start zumo")
while True:
bbcon.run_one_timestep()
def lineCollision():
reflect = ReflectanceSensors()
ZumoButton().wait_for_press()
motor = Motors()
stuck = CheckStuck()
ultra = Ultrasonic()
proxim = IRProximitySensor()
sensobCol = Sensob()
sensobCol.set_sensors([ultra, proxim])
motob = Motob(motor=motor)
sensobLine = Sensob()
sensobLine.set_sensors([reflect])
arb = Arbitrator(motob=motob)
bbcon = BBCON(arbitrator=arb, motob=motob)
bbcon.set_checkStucker(stuck)
line = LineFollow(1, [sensobLine])
col = CollisionAvoidance(1, [sensobCol])
bbcon.add_behavior(line)
bbcon.add_behavior(col)
bbcon.add_sensob(sensobCol)
bbcon.add_sensob(sensobLine)
count = 0
while count < 20:
bbcon.run_one_timestep()
count += 1
def __init__(self):
self.arbit = Arbitrary()
self.value = None
self.motorList = []
self.motorList.append(Motors())
def __init__(self):
# Initialize arbitrator
self.arbitrator = Arbitrator()
# Initialize motobs (single object for both motors on the Zumo)
self.motobs.append(Motob(Motors()))
# Initialize sensors
self.sensors = {
'ultrasonic': Ultrasonic(0.05),
'IR': IRProximitySensor(),
'reflectance': ReflectanceSensors(False, 0, 900),
'camera': Camera(),
}
self.active_sensors = [self.sensors['ultrasonic'], self.sensors['IR'], self.sensors['reflectance']]
# Initialize sensobs
self.sensobs = {
'distance': DistanceSensob([self.sensors['ultrasonic']]),
'line_pos': LinePosSensob([self.sensors['reflectance']]),
'proximity': ProximitySensob([self.sensors['IR']]),
'red_search': RedSearchSensob([self.sensors['camera']]),
}
self.active_sensobs = [self.sensobs['distance'], self.sensobs['line_pos'], self.sensobs['proximity']]
time.sleep(1)
class Motob:
def __init__(self):
self.motors = Motors()
def update(self, motor_recommendation: MotorOperation):
print(motor_recommendation)
if motor_recommendation == MotorOperation.FORWARDS:
self.motors.forward()
elif motor_recommendation == MotorOperation.BACKWARDS:
self.motors.backward()
elif motor_recommendation == MotorOperation.TURN_LEFT:
self.motors.left(1, 2.5)
elif motor_recommendation == MotorOperation.TURN_RIGHT:
self.motors.right(1, 2.5)
else:
self.motors.forward(0)
def lineTest():
reflect = ReflectanceSensors()
ZumoButton().wait_for_press()
motor = Motors()
stuck = CheckStuck()
camera = Camera()
motob = Motob(motor)
arbitrator = Arbitrator(motob=motob)
sensob = Sensob()
sensob.set_sensors([reflect])
bbcon = BBCON(arbitrator=arbitrator, motob=motob)
bbcon.add_sensob(sensob)
bbcon.set_checkStucker(stuck)
b = LineFollow(1, [sensob])
bbcon.add_behavior(b)
timesteps = 0
while timesteps < 30:
bbcon.run_one_timestep()
timesteps += 1
def trackTest():
ZumoButton().wait_for_press()
motor = Motors()
ultra = Ultrasonic()
camera = Camera()
stuck = CheckStuck()
motob = Motob(motor)
arbitrator = Arbitrator(motob=motob)
sensob = Sensob()
sensob.set_sensors([ultra, camera])
bbcon = BBCON(arbitrator=arbitrator, motob=motob)
b = TrackObject(priority=1, sensobs=[sensob])
bbcon.set_checkStucker(stuck)
bbcon.add_behavior(b)
bbcon.activate_behavior(0)
bbcon.add_sensob(sensob)
timesteps = 0
while timesteps < 25:
bbcon.run_one_timestep()
timesteps += 1
def __init__(self, position):
"""
position: init position dict with keys:
- "angle": a X-axis relative angle.
- "point": the first position.
"""
self._position = position
# self._us_sensors = RangeSensor(4)
self._motors = Motors(5)
self._kinematic = Kinematics(6)
logging.info('Building the graph map.')
robot_diagonal = math.sqrt(self.DIMENSION['length'] +
self.DIMENSION['width']**2)
self._graph = build_graph(robot_diagonal)
logging.info('Finished to build the graph map.')
def __init__(self):
# initialize the status
self.dist = 1
self.dist_prev = 2
self.act_prev = 'r'
# init motors, establish bluetooth
self.motors = Motors()
self.motors.arm_move((90,90))
class Server():
def __init__(self, host, port):
self.host = host
print(host)
self.port = port
self.motors = Motors()
def start(self):
self.sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self.sock.bind((self.host, self.port))
self.sock.listen(5)
return self.connection()
def connection(self):
conn, addr = self.sock.accept()
print(addr)
while True:
data = conn.recv(1024)
if data.decode("utf-8") == "w":
print('move forward')
self.motors.forward()
elif data.decode("utf-8") == "s":
print("move back")
self.motors.back()
elif data.decode("utf-8") == "a":
print("move left")
self.motors.left()
elif data.decode("utf-8") == "d":
print("move right")
self.motors.right()
elif data.decode("utf-8") == "space":
print("stopping")
self.motors.stop()
elif data.decode("utf-8") == "m":
print("server OFF")
break
elif data.decode("utf-8") == "q":
print("Program will close...")
self.sock.close()
return False
self.sock.close()
return True
def restart(self):
self.connection()
def shoot_panorama(camera=Camera(),motors=Motors(),shots=5):
s = 1
im = IMR.Imager(image=camera.update()).scale(s,s)
rotation_time = 3/shots # At a speed of 0.5(of max), it takes about 3 seconds to rotate 360 degrees
for i in range(shots-1):
motors.right(0.5,rotation_time)
im = im.concat_horiz(IMR.Imager(image=camera.update()))
return im
def tourist(steps=25,shots=5,speed=.25):
ZumoButton().wait_for_press()
rs = ReflectanceSensors(); m = Motors(); c = Camera()
for i in range(steps):
random_step(m,speed=speed,duration=0.5)
vals = rs.update()
if sum(vals) < 1: # very dark area
im = shoot_panorama(c,m,shots)
im.dump_image('vacation_pic'+str(i)+'.jpeg')
async def update_motors():
global cmd
m = Motors()
m.set(0, 0)
while True:
if time.time() - cmd['time'] > p.motor_command_timeout:
m.set(0, 0)
else:
m.set(cmd['forward'], cmd['turn'])
await asyncio.sleep(0.05)
def __init__(self, host, port):
self.host = host
self.port = port
self.socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self.socket.bind((host, port))
self.socket.listen(1)
self.client_socket = self.socket.accept()[0]
Motors()
self.listen()
def __init__(self, leftMotorPort=None, rightMotorPort=None, leftTouchPort=None, rightTouchPort=None, sonarPort=None): BrickPiSetup() # setup the serial port for communication if(sonarPort is not None): self.sonar = Sonar(sonarPort) if(leftTouchPort is not None): self.leftTouch = Touch(leftTouchPort) if(rightTouchPort is not None): self.rightTouch = Touch(rightTouchPort) if(leftMotorPort is not None and rightMotorPort is not None): self.motors = Motors(leftMotorPort, rightMotorPort) BrickPiSetupSensors() #Send the properties of sensors to BrickPi
def __init__(self):
"""Initialize Motob object.
Parameters
----------
action : Holder of the action whose settings will be determined by the motob.
duration : Holder of the most recent duration the of the action.
"""
self.m = Motors()
self.action = None
self.duration = None
def __init__(self):
if pi is True:
self.board = Arduino()
self.board.connect()
self.move = Motors()
GPIO.setup(GREEN_LED_GPIO,GPIO.OUT)
GPIO.setup(RED_LED_GPIO,GPIO.OUT)
GPIO.setup(BUTTON, GPIO.IN, pull_up_down=GPIO.PUD_UP)
# load the sample hash
with open(rfid_hash) as fh:
self.rfid_hash = json.load(fh)
class Robot: def __init__(self, leftMotorPort=None, rightMotorPort=None, leftTouchPort=None, rightTouchPort=None, sonarPort=None): BrickPiSetup() # setup the serial port for communication if(sonarPort is not None): self.sonar = Sonar(sonarPort) if(leftTouchPort is not None): self.leftTouch = Touch(leftTouchPort) if(rightTouchPort is not None): self.rightTouch = Touch(rightTouchPort) if(leftMotorPort is not None and rightMotorPort is not None): self.motors = Motors(leftMotorPort, rightMotorPort) BrickPiSetupSensors() #Send the properties of sensors to BrickPi def followWall(self, prefer_wall_distance): """ Follow wall within 'distance' cm """ print "Follow wall within ", prefer_wall_distance, " cm." acc_err = 0 last_err = 0 last_dist = 0 while(True): acc_err, last_err, last_diff_vel = self.followWallStep(prefer_wall_distance, acc_err, last_err, last_dist) def followWallStep(self, prefer_wall_distance, acc_err=0, last_err=0, last_z = 0): # read sonar z = self.sonar.getSmoothSonarDistance(0.05) print "Distance from the wall ", z err = (prefer_wall_distance - z) err = limitTo(err, -10, 10) derror = err - last_err acc_err += err #acc_err = limitTo(acc_err, -10, 10) # HACK: shouldn't need to do this if(err < 0): kp = FOLLOW_WALL_KP_FAR else: kp = FOLLOW_WALL_KP_NEAR I_adjust = limitTo(FOLLOW_WALL_KI*acc_err, -5, 5) diff_vel = kp*err + I_adjust + FOLLOW_WALL_KD * derror print "err : ", err, acc_err if(err*last_err <= 0): acc_err = 0 print kp*err, I_adjust, FOLLOW_WALL_KD * derror print "RERR : ", abs(err) - abs(last_err) ERR_T = 3 #if(abs(last_err) - abs(err) > ERR_T): # diff_vel = 0 # set moving speed new_left_speed = FWD_VEL + FWD_SIGN*int(round(diff_vel)) new_right_speed = FWD_VEL - FWD_SIGN*int(round(diff_vel)) self.motors.setSpeed(new_left_speed, new_right_speed) time.sleep(0.01) last_z = z return (acc_err, err, last_z) def keepDistanceFront(self, distance): """ Keeps the robot at some distance from the wall """ print "Keep distance : ", distance acc_err = 0 last_err = 0 while True: # get sonar measurement for 0.05 seconds z = self.sonar.getSmoothSonarDistance(0.05) # set the speed of the robot err = z - distance acc_err += err derror = err - last_err print "Current distance : ", z speed = KEEP_DISTANCE_FRONT_KP * err + KEEP_DISTANCE_FRONT_KI * acc_err + KEEP_DISTANCE_FRONT_KD * derror print speed, err, acc_err, derror #if(abs(err) > 3 and speed != 0 and abs(speed) < LOWEST_VEL): # direction = speed/abs(speed) # speed = direction*LOWEST_VEL if(err*last_err < 0): acc_err = 0 print "mul", err*last_err self.motors.setSpeed(speed) last_err = err time.sleep(0.01) # def stop(self): # self.motors.stop() # def forward(self, *args, **kwargs): # self.motors.forward(*args, **kwargs) # def turn(self, *args, **kwargs): # self.motors.turn(*args, **kwargs) # def left90deg(self): # self.motors.left90deg() # def right90deg(self): # self.motors.right90deg() # def setSpeed(self, *args, **kwargs): # self.motors.setSpeed(*args, **kwargs) def __getattr__(self, name): """ If a method like stop does not exist, try it on the motors """ def _missing(*args, **kwargs): if self.motors is not None: func = getattr(self.motors, name) func(*args, **kwargs) return _missing
def dancer():
ZumoButton().wait_for_press()
m = Motors()
m.forward(.2,3)
m.backward(.2,3)
m.right(.5,3)
m.left(.5,3)
m.backward(.3,2.5)
m.set_value([.5,.1],10)
m.set_value([-.5,-.1],10)
def __init__(self):
# No communications or arming yet
self.comms = None
self.armed = False
# Do basic Tk initialization
self.root = Tk()
self.root.configure(bg=BACKGROUND_COLOR)
self.root.resizable(False, False)
self.root.title('Hackflight Ground Control Station')
left = (self.root.winfo_screenwidth() - DISPLAY_WIDTH) / 2
top = (self.root.winfo_screenheight() - DISPLAY_HEIGHT) / 2
self.root.geometry('%dx%d+%d+%d' % (DISPLAY_WIDTH, DISPLAY_HEIGHT, left, top))
self.frame = Frame(self.root)
self.root.wm_iconbitmap(bitmap = "@media/icon.xbm")
self.root.tk.call('wm', 'iconphoto', self.root._w, PhotoImage('icon.png'))
self.root.protocol('WM_DELETE_WINDOW', self.quit)
# Create panes for two rows of widgets
self.pane1 = self._add_pane()
self.pane2 = self._add_pane()
# Add a buttons
self.button_connect = self._add_button('Connect', self.pane1, self._connect_callback)
self.button_setup = self._add_button('Setup', self.pane2, self._setup_callback)
self.button_motors = self._add_button('Motors', self.pane2, self._motors_button_callback)
self.button_receiver = self._add_button('Receiver', self.pane2, self._receiver_button_callback)
self.button_messages = self._add_button('Messages', self.pane2, self._messages_button_callback)
#self.button_maps = self._add_button('Maps', self.pane2, self._maps_button_callback, disabled=False)
# Prepare for adding ports as they are detected by our timer task
self.portsvar = StringVar(self.root)
self.portsmenu = None
self.connected = False
self.ports = []
# Finalize Tk stuff
self.frame.pack()
self.canvas = Canvas(self.root, width=DISPLAY_WIDTH, height=DISPLAY_HEIGHT, background='black')
self.canvas.pack()
# Add widgets for motor-testing dialog; hide them immediately
self.motors = Motors(self)
self.motors.stop()
# Create receiver dialog
self.receiver = Receiver(self)
# Create messages dialog
self.messages = Messages(self)
# Create setup dialog
self.setup = Setup(self)
self._schedule_connection_task()
# Create a maps dialog
#self.maps = Maps(self, yoffset=-30)
# Create a splash image
self.splashimage = PhotoImage(file='media/splash.png')
self._show_splash()
# Create a message parser
self.parser = MSP_Parser()
# Set up parser's request strings
self.attitude_request = self.parser.serialize_ATTITUDE_Request()
self.rc_request = self.parser.serialize_RC_Request()
# No messages yet
self.yaw_pitch_roll = 0,0,0
self.rxchannels = 0,0,0,0,0
# A hack to support display in Setup dialog
self.active_axis = 0
from arduino import Commands, Arduino
from time import sleep
from motors import Motors
from mission import Mission
board = Arduino()
board.connect()
move = Motors()
mission = Mission()
mission.startMission()
board.connect()
print("Board connected")
sleep(0.5)
del1 = 5 # This is standard delay 1 in seconds
del2 = 3 # This is standard delay 2 in seconds
speed = 50 # This takes a value from 0 to 254
print("Will now go forward speed 50 for 1S")
sleep(0.8)
move.forward(50)
sleep(1)
move.stop()
print("Will now go backwards speed 50 for 1S")
sleep(1)
move.backward(50)
sleep(1)
move.stop()
print("robot will now move forward by 60 cm on button press")
sleep(1)
from arduino import Commands, Arduino
from time import sleep
from motors import Motors
from mission import Mission
board = Arduino()
board.connect()
move = Motors()
mission = Mission()
mission.startMission()
while True:
move.position(10,100)
sleep(4)
move.rotate(-90,100)
sleep(4)
move.position(10,100)
sleep(4)
move.rotate(-90,100)
sleep(4)
move.position(10,100)
sleep(4)
move.rotate(-90,100)
sleep(4)
move.position(10,100)
sleep(4)
move.rotate(-90,100)
sleep(4)
move.position(-10,100)
sleep(4)
move.rotate(90,100)
class GCS:
def __init__(self):
# No communications or arming yet
self.comms = None
self.armed = False
# Do basic Tk initialization
self.root = Tk()
self.root.configure(bg=BACKGROUND_COLOR)
self.root.resizable(False, False)
self.root.title('Hackflight Ground Control Station')
left = (self.root.winfo_screenwidth() - DISPLAY_WIDTH) / 2
top = (self.root.winfo_screenheight() - DISPLAY_HEIGHT) / 2
self.root.geometry('%dx%d+%d+%d' % (DISPLAY_WIDTH, DISPLAY_HEIGHT, left, top))
self.frame = Frame(self.root)
self.root.wm_iconbitmap(bitmap = "@media/icon.xbm")
self.root.tk.call('wm', 'iconphoto', self.root._w, PhotoImage('icon.png'))
self.root.protocol('WM_DELETE_WINDOW', self.quit)
# Create panes for two rows of widgets
self.pane1 = self._add_pane()
self.pane2 = self._add_pane()
# Add a buttons
self.button_connect = self._add_button('Connect', self.pane1, self._connect_callback)
self.button_setup = self._add_button('Setup', self.pane2, self._setup_callback)
self.button_motors = self._add_button('Motors', self.pane2, self._motors_button_callback)
self.button_receiver = self._add_button('Receiver', self.pane2, self._receiver_button_callback)
self.button_messages = self._add_button('Messages', self.pane2, self._messages_button_callback)
#self.button_maps = self._add_button('Maps', self.pane2, self._maps_button_callback, disabled=False)
# Prepare for adding ports as they are detected by our timer task
self.portsvar = StringVar(self.root)
self.portsmenu = None
self.connected = False
self.ports = []
# Finalize Tk stuff
self.frame.pack()
self.canvas = Canvas(self.root, width=DISPLAY_WIDTH, height=DISPLAY_HEIGHT, background='black')
self.canvas.pack()
# Add widgets for motor-testing dialog; hide them immediately
self.motors = Motors(self)
self.motors.stop()
# Create receiver dialog
self.receiver = Receiver(self)
# Create messages dialog
self.messages = Messages(self)
# Create setup dialog
self.setup = Setup(self)
self._schedule_connection_task()
# Create a maps dialog
#self.maps = Maps(self, yoffset=-30)
# Create a splash image
self.splashimage = PhotoImage(file='media/splash.png')
self._show_splash()
# Create a message parser
self.parser = MSP_Parser()
# Set up parser's request strings
self.attitude_request = self.parser.serialize_ATTITUDE_Request()
self.rc_request = self.parser.serialize_RC_Request()
# No messages yet
self.yaw_pitch_roll = 0,0,0
self.rxchannels = 0,0,0,0,0
# A hack to support display in Setup dialog
self.active_axis = 0
def quit(self):
self.motors.stop()
self.root.destroy()
def hide(self, widget):
widget.place(x=-9999)
def getChannels(self):
return self.rxchannels
def getYawPitchRoll(self):
# configure button to show connected
self._enable_buttons()
self.button_connect['text'] = 'Disconnect'
self._enable_button(self.button_connect)
return self.yaw_pitch_roll
def checkArmed(self):
if self.armed:
self._show_armed(self.root)
self._show_armed(self.pane1)
self._show_armed(self.pane2)
self._disable_button(self.button_motors)
else:
self._show_disarmed(self.root)
self._show_disarmed(self.pane1)
self._show_disarmed(self.pane2)
def scheduleTask(self, delay_msec, task):
self.root.after(delay_msec, task)
def _add_pane(self):
pane = PanedWindow(self.frame, bg=BACKGROUND_COLOR)
pane.pack(fill=BOTH, expand=1)
return pane
def _add_button(self, label, parent, callback, disabled=True):
button = Button(parent, text=label, command=callback)
button.pack(side=LEFT)
button.config(state = 'disabled' if disabled else 'normal')
return button
# Callback for Setup button
def _setup_callback(self):
self._clear()
self.motors.stop()
self.receiver.stop()
self.messages.stop()
#self.maps.stop()
self.parser.set_ATTITUDE_Handler(self._handle_attitude)
self.setup.start()
def _start(self):
self.parser.set_ATTITUDE_Handler(self._handle_attitude)
self._send_attitude_request()
self.setup.start()
self.parser.set_RC_Handler(self._handle_rc)
self._send_rc_request()
# Sends Attitude request to FC
def _send_attitude_request(self):
self.comms.send_request(self.attitude_request)
# Sends RC request to FC
def _send_rc_request(self):
self.comms.send_request(self.rc_request)
# Callback for Motors button
def _motors_button_callback(self):
self._clear()
self.setup.stop()
self.parser.set_ATTITUDE_Handler(self._handle_attitude)
self.receiver.stop()
self.messages.stop()
#self.maps.stop()
self.motors.start()
def _clear(self):
self.canvas.delete(ALL)
# Callback for Receiver button
def _receiver_button_callback(self):
self._clear()
self.setup.stop()
self.motors.stop()
self.messages.stop()
#self.maps.stop()
self.receiver.start()
# Callback for Messages button
def _messages_button_callback(self):
self._clear()
self.setup.stop()
self.motors.stop()
#self.maps.stop()
self.receiver.stop()
self.messages.start()
def _getting_messages(self):
return self.button_connect['text'] == 'Disconnect'
# Callback for Maps button
def _maps_button_callback(self):
self._clear()
if self._getting_messages():
self.receiver.stop()
self.messages.stop()
self.setup.stop()
self.motors.stop()
#self.maps.start()
# Callback for Connect / Disconnect button
def _connect_callback(self):
if self.connected:
self.setup.stop()
#self.maps.stop()
self.motors.stop()
self.messages.stop()
self.receiver.stop()
if not self.comms is None:
self.comms.stop()
self._clear()
self._disable_buttons()
self.button_connect['text'] = 'Connect'
self._show_splash()
else:
#self.maps.stop()
self.comms = Comms(self)
self.comms.start()
self.button_connect['text'] = 'Connecting ...'
self._disable_button(self.button_connect)
self._hide_splash()
self.scheduleTask(CONNECTION_DELAY_MSEC, self._start)
self.connected = not self.connected
# Gets available ports
def _getports(self):
allports = comports()
ports = []
for port in allports:
portname = port[0]
if 'ttyACM' in portname or 'ttyUSB' in portname or 'COM' in portname:
ports.append(portname)
return ports
# Checks for changes in port status (hot-plugging USB cables)
def _connection_task(self):
ports = self._getports()
if ports != self.ports:
if self.portsmenu is None:
self.portsmenu = OptionMenu(self.pane1, self.portsvar, *ports)
else:
for port in ports:
self.portsmenu['menu'].add_command(label=port)
self.portsmenu.pack(side=LEFT)
if ports == []:
self._disable_button(self.button_connect)
self._disable_buttons()
else:
self.portsvar.set(ports[0]) # default value
self._enable_button(self.button_connect)
self.ports = ports
self._schedule_connection_task()
# Mutually recursive with above
def _schedule_connection_task(self):
self.root.after(USB_UPDATE_MSEC, self._connection_task)
def _disable_buttons(self):
self._disable_button(self.button_setup)
self._disable_button(self.button_motors)
self._disable_button(self.button_receiver)
self._disable_button(self.button_messages)
def _enable_buttons(self):
self._enable_button(self.button_setup)
self._enable_button(self.button_motors)
self._enable_button(self.button_receiver)
self._enable_button(self.button_messages)
def _enable_button(self, button):
button['state'] = 'normal'
def _disable_button(self, button):
button['state'] = 'disabled'
def sendMotorMessage(self, index, value):
values = [1000]*4
values[index-1] = value
self.comms.send_message(self.parser.serialize_SET_MOTOR, values)
def _show_splash(self):
self.splash = self.canvas.create_image((400,250), image=self.splashimage)
def _hide_splash(self):
self.canvas.delete(self.splash)
def _show_armed(self, widget):
widget.configure(bg='red')
def _show_disarmed(self, widget):
widget.configure(bg=BACKGROUND_COLOR)
if self._getting_messages():
self._enable_button(self.button_motors)
def _handle_calibrate_response(self):
self.setup.showCalibrated()
def _handle_params_response(self, pitchroll_kp_percent, yaw_kp_percent):
# Only handle parms from firmware on a fresh connection
if self.newconnect:
self.setup.setParams(pitchroll_kp_percent, yaw_kp_percent)
self.newconnect = False
def _handle_attitude(self, x, y, z):
self.yaw_pitch_roll = z, -y/10., x/10.
self.messages.setCurrentMessage('Yaw/Pitch/Roll: %+3.3f %+3.3f %+3.3f' % self.yaw_pitch_roll)
# As soon as we handle the callback from one request, send another request
self._send_attitude_request()
def _handle_rc(self, c1, c2, c3, c4, c5, c6, c7, c8):
self.rxchannels = c1, c2, c3, c4, c5
# As soon as we handle the callback from one request, send another request
self._send_rc_request()
self.messages.setCurrentMessage('Receiver: %04d %04d %04d %04d %04d' % (c1, c2, c3, c4, c5))
def _handle_arm_status(self, armed):
self.armed = armed
self.messages.setCurrentMessage('ArmStatus: %s' % ('ARMED' if armed else 'Disarmed'))
def _handle_battery_status(self, volts, amps):
self.messages.setCurrentMessage('BatteryStatus: %3.3f volts, %3.3f amps' % (volts, amps))
class Motob:
"""Interface between a behavior and one or more motors."""
def __init__(self):
"""Initialize Motob object.
Parameters
----------
action : Holder of the action whose settings will be determined by the motob.
duration : Holder of the most recent duration the of the action.
"""
self.m = Motors()
self.action = None
self.duration = None
def update(self, recommendation):
# print('Motor recommendation:', recommendation)
"""Update object.
Receive a new motor recommendation.
Load it into the instance variables.
Operationalize it.
Example
-------
recommendation = ('L', 1)
"""
self.m.stop()
self.action = recommendation[0]
self.duration = recommendation[1]
self.operationalize(self.action, self.duration)
def operationalize(self, action, duration):
"""Convert motor recommendation into motor setting and send to corresponding motor(s).
Parameters
----------
action : Action the motors will perform.
dur : Duration of the operation.
Example of actions
------------------
F = Drive forward
B = Drive backward
Z = Boost forward
X = Flee
T = Turn around 180 degrees
L = Turn left while driving forward
R = Turn right while driving forward
S = Stop
"""
if action == 'F':
self.m.set_value((0.5, 0.5), duration)
elif action == 'B':
self.m.backward(0.25, duration)
elif action == 'Z':
self.m.set_value((1, 1), duration)
elif action == 'X':
self.m.flee() # Turn around, must be tuned for 180 degree turn.
elif action == 'T':
self.m.turn()
elif action == 'L':
self.m.left((0.1, 0.25), duration)
elif action == 'R':
self.m.right((0.25, 0.1), duration)
elif action == 'S':
self.m.stop()
def __init__(self):
self.motor = Motors()
# these 3 lines import some libraries that the software uses
from arduino import Arduino
from motors import Motors
from time import sleep
# these 3 lines get the robot setup and ready to go
board = Arduino()
board.connect()
move = Motors()
# the rest of the program shows how to move the robot around
print("turn Left")
move.turnLeft(90) # 90 is the angle in degree
sleep(3) # 3 is the number of seconds to wait
move.stop()
sleep(1)
print("turn Right")
move.turnRight(90)
sleep(3)
move.stop()
from arduino import Commands, Arduino
from motors import Motors
from mission import Mission
import time
board = Arduino()
board.connect()
move = Motors()
mission = Mission()
mission.startMission()
start_time = time.time()
print("--- %s seconds ---" % (time.time() - start_time))
move.position(20,200)
there=int(move.getAtPosition())
print("initial position status is :")
print(there)
print("now moving to requested position")
while (there==0):
print("waiting for desination, status is:")
time.sleep(0.2)
there=int(move.getAtPosition())
print(there)
print("Arrived there")
print("--- %s seconds ---" % (time.time() - start_time))
print("now going to rotate and see how that goes")
move.rotate(-90,200)
print("initial position status is :")
there=int(move.getAtPosition())
print(there)
print("now turning by requested amount")
while (there==0):
def dancer():
ZumoButton().wait_for_press()
m = Motors()
print("hei")
m.forward(0.2, 3)
m.backward(0.2, 3)
m.right(0.5, 3)
m.left(0.5, 3)
m.backward(0.3, 2.5)
m.set_value([0.5, 0.1], 10)
m.set_value([-0.5, -0.1], 10)
print("hei2")
def explorer(dist=10):
ZumoButton().wait_for_press()
m = Motors()
u = Ultrasonic()
while u.update() > dist:
m.forward(0.2, 0.2)
m.backward(0.1, 0.5)
m.left(0.5, 3)
m.right(0.5, 3.5)
sleep(2)
while u.update() < dist * 5:
m.backward(0.2, 0.2)
m.left(0.75, 5)
import time
from arduino import Commands, Arduino
#from arduino import Commands, Arduino
from ultrasound import Ultrasound
from time import sleep
from motors import Motors
from mission import Mission
import RPi.GPIO as GPIO
import picamera
RED_LED_GPIO = 26
GREEN_LED_GPIO = 29
BUTTON = 36
GPIO.setmode(GPIO.BOARD)
board = Arduino()
move = Motors()
mission = Mission()
#variables to be set up for the test
power_f = 90
power_t = 90
del1 = 5
del2 = 3
speed = 50
def buttonWait():
GPIO.output(GREEN_LED_GPIO,False)
GPIO.output(RED_LED_GPIO,True)
print("waiting for button...")
while GPIO.input(BUTTON) == False:
class Motob():
def __init__(self):
self.motor = Motors()
self.action = None
self.duration = None
def update(self, motor_recommendation):
if motor_recommendation is None:
return
self.action, self.duration = motor_recommendation
self.operationalize(self.action, self.duration)
def operationalize(self, action, duration):
if action == 'F':
self.motor.set_value((0.2, 0.2), duration)
elif action == 'B':
self.motor.set_value((-0.2, -0.2), duration)
elif action == 'L':
self.motor.set_value((-0.4, 0.4), duration)
elif action == 'R':
self.motor.set_value((0.4, -0.4), duration)
elif action == 'S':
self.motor.stop()
elif action == 'LL':
self.motor.set_value((-0.6, 0.6), duration)
def stop(self):
self.motor.stop()
class Motob:
def __init__(self):
self.motor = Motors()
def update(self, action, sleep_time = 0.33):
if action == "forward":
self.motor.forward()
sleep(sleep_time)
elif action == "backward":
self.motor.backward()
sleep(sleep_time * 6)
self.motor.left()
sleep(sleep_time / 10)
elif action == "left":
self.motor.left()
sleep(sleep_time / 10)
elif action == "right":
self.motor.right()
sleep(sleep_time / 10)
elif action == "charge":
self.motor.set_value((400, 400))
elif action == "brake":
self.motor.stop()
def __init__(self):
self.motor = Motors()
self.action = None
self.duration = None
from arduino import Commands, Arduino
from time import sleep
from motors import Motors
from mission import Mission
board = Arduino()
board.connect()
move = Motors()
mission = Mission()
power_f = 100
power_t = 100
mission.startMission()
while True:
print("Forward")
move.forward(power_f)
sleep(4)
move.stop()
print("turn left")
move.leftMotor(power_f, 0)
move.rightMotor(power_f, 1)
sleep(1)
move.stop()
sleep(1)
print("turn right")
move.leftMotor(power_f, 1)
from arduino import Commands, Arduino
from time import sleep
from motors import Motors
from mission import Mission
mission = Mission()
board = Arduino()
board.connect()
move = Motors()
print("ok")
move.stop()
sleep(2)
print("sampling")
location = mission.getLocation()
sample = mission.takeSample(location)
print(sample)
mission.endMission()
class Mission():
# tested
def __init__(self):
if pi is True:
self.board = Arduino()
self.board.connect()
self.move = Motors()
GPIO.setup(GREEN_LED_GPIO,GPIO.OUT)
GPIO.setup(RED_LED_GPIO,GPIO.OUT)
GPIO.setup(BUTTON, GPIO.IN, pull_up_down=GPIO.PUD_UP)
# load the sample hash
with open(rfid_hash) as fh:
self.rfid_hash = json.load(fh)
# won't test
def startMission(self):
if pi is True:
GPIO.output(GREEN_LED_GPIO,False)
GPIO.output(RED_LED_GPIO,True)
print("waiting for button...")
while GPIO.input(BUTTON) == False:
time.sleep(0.1)
GPIO.output(RED_LED_GPIO,False)
# won't test
def endMission(self):
if pi is True:
GPIO.output(GREEN_LED_GPIO,True)
# tested
def deleteData(self):
try:
os.unlink(data_file)
except OSError:
pass
# tested
# stores a dict as json on the disk
# appends to the data file
def saveData(self, sample):
with open(data_file,'a') as fh:
fh.write(json.dumps(sample) + "\n")
# tested
# returns a list of dicts
def loadData(self):
data = []
try:
with open(data_file,) as fh:
for sample in fh.readlines():
data.append(json.loads(sample))
except IOError:
pass
return data
# won't test
# fetches an RFID
def getLocation(self):
if pi is True:
rfid = ""
# keep moving until robot gets a location
while True:
rfid = self.board.sendCommand(Commands.READ_RFID,0,0)
# when I catalogued the RFIDs I missed the last char!
rfid = rfid[0:11]
if len(rfid) == 11:
break
self.move.forward(70)
time.sleep(0.1)
self.move.stop()
time.sleep(1)
return rfid
# tested
# indexes into the sample database with the RFID and returns sample as dict
def takeSample(self, location):
try:
return self.rfid_hash[location]
except KeyError:
raise Exception("unknown location [%s]" % location)
# tested
# uploads a sample dict with a team name (string)
def uploadSample(self, sample, team):
# fetch team ID from nane
r = requests.get(mc_url + '/api/team/' + team)
if r.status_code == 400:
raise Exception(r.text)
team_id = json.loads(r.text)['id']
sample['team'] = str(team_id)
# posts it
r = requests.post(mc_url + '/api/sample', json=sample)
if r.status_code == 400:
return r.text
#exeception stops the program
#raise Exception(r.text)
new_sample = json.loads(r.text)
print("uploaded sample %d" % new_sample['id'])
return new_sample
def getAllSamples(self):
r = requests.get(mc_url + '/api/samples')
samples = json.loads(r.text)['samples']
return samples
