def __init__(self):
     self._front = Motor(self._frontDriver)
     self._rear = Motor(self._rearDriver)
     self._front.setDirCw()
     self._front.setSpeed(0)
     self._rear.setDirCcw()
     self._rear.setSpeed(0)

def navigate():
	'''
		Function will call exploration() function to move robot, but will keep
		track of previous moves
	'''

	# Test output
	print("Navigating ...")

	# Create motor and buzzer objects
	motor = Motor()
	buzzer = Buzzer()

	# Beep to indicate begining of navigate step
	buzzer.play(4)

	try:
		# Enter the exploration loop
		for i in xrange(params.p['MAX_ITER']):

			# Execute explore function and save results
			explore(motor, buzzer)

			# Wait between moves
			time.sleep(params.p['WAIT_TIME'])
	except Exception:
		pass
	finally:
		motor.stop_bot()

class RoboArm(object):
    def __init__(self):
        self.joints = [
            PWMJoint(
                5, BASE_MIN_ANGLE, BASE_MIN_PWM, BASE_MAX_ANGLE, 
                BASE_MAX_PWM, BASE_COF1, BASE_COF0),
            PWMJoint(
                6, SHOULDER_MIN_ANGLE, SHOULDER_MIN_PWM, SHOULDER_MAX_ANGLE, 
                SHOULDER_MAX_PWM, SHOULDER_COF1, SHOULDER_COF0),
            PWMJoint(
                7, ELBOW_MIN_ANGLE, ELBOW_MIN_PWM, ELBOW_MAX_ANGLE, 
                ELBOW_MAX_PWM, ELBOW_COF1, ELBOW_COF0),
            DyMiJoint(
                1, WRIST_MIN_ANGLE, WRIST_MIN_PWM, WRIST_MAX_ANGLE, 
                WRIST_MAX_PWM, WRIST_COF1, WRIST_COF0)]
        self.motor = Motor(4)

    def move_arm(self, out_pos):
        self.motor.set_vel(out_pos.pos7)
        arm_pos = [0.] * 6
        for i in range(0, 4):
            legal, arm_pos[i] = joints[i].get_pos(arm_pose[i])
            if not legal:
                rospy.logerr('angle over bound for joint %d' % i)
            joints[i].set_pos(arm_pos[i])
    
    def close(self):
        for joint in self.joints:
            joint.close()

def main():
	Volvomotor=Motor(0,100,0,"Volvo","Stop")
	state=raw_input("Motor state: ")
	Volvomotor.changestate(state)
	Volvomotor.accelerate()
	print "Current speed: "+str(Volvomotor.rpm)+"rpm"
	return 0

def main():
    log_file = "main_log.log"
    create_timed_rotating_log("log/" + log_file)
    logger = logging.getLogger("BasicLogger")
    logger.info("----- Starting Logging Session -----")
    config = configparser.ConfigParser()
    config.read('config.ini')

    # How to use the config values. REMOVE when done with setup of this!
    print(config.sections())
    print('scale of this whole thing from config file is: ' + config['grid']['scale'])
    # To get the values as integers:
    i = int(config['grid']['max_position_z'])
    print(i+2)

    # subprocess.call("../gcodepull.sh", shell=True)

    #opens the file named in the varibles file
    length = range(FileOperator.OpenFile()- 3)
    Motor.setup()
    start = 2
    for row in length:
            # for the appropiated length each row is worked through 
            # and the needet steps are sent to the stepper motors
            next_row = row + start
            delta_step = FileOperator.NextMove(next_row)
            corrected_coords = FileOperator.MoveCorrect(delta_step)
            Motor.move(corrected_coords)
            print('finished')
    GPIO.cleanup()

    def __init__(self, config):
        Thread.__init__(self)
        self.angle = 0
        self.K = 0.98
        self.logDataSetBuffer = []

        self.Kp = config.config.as_float('Kp')
        self.Ki = config.config.as_float('Ki')
        self.Kd = config.config.as_float('Kd')
        self.set_point = config.config.as_float('set_point')
        self.disable_motors = config.config.as_bool('disable_motors')
        
        self.integral_error = 0
        self.last_error = 0
        self.motor_left = Motor("L", port_motor_left_pwm, port_motor_left_backward, port_motor_left_forward)
        self.motor_right = Motor("R", port_motor_right_pwm, port_motor_right_backward, port_motor_right_forward)
        
        self.accelerometer = MPU6050()

        self.last_time = time()

        self.logger = logging.getLogger(__name__)

        self.setDaemon(True)
        self.latest_sensor = 0
        
        self.logger.info('Initialized ControlThread with the following settings')
        self.logger.info('disable_motors={}'.format(self.disable_motors))
        self.logger.info('set_point={:1.2f}'.format(self.set_point))
        self.logger.info('Kp={:1.1f}'.format(self.Kp))
        self.logger.info('Ki={:1.1f}'.format(self.Ki))
        self.logger.info('Kd={:1.1f}'.format(self.Kd))

    def __init__(self):
        self.motor_front_left = Motor(MotorConductor.FRONT, MotorConductor.LEFT)
        self.motor_front_right = Motor(MotorConductor.FRONT, MotorConductor.RIGHT)

        for ndx, motor in enumerate(self.get_motors()):
            motor.gear_ratio = Config.get_gear_ratio()
            motor.wheel_diameter = Config.get_wheel_diameter()
            motor.position_ratio = Config.get_position_ratio()[ndx]
            motor.speed_ratio = Config.get_speed_ratio()[ndx]

class CarServer(object):

    def __init__(self, motor_pins=(24, 25), servo_pin=0, port=2012):
        self.port = port

        # The motor and servo for driving
        self.motor = Motor(*motor_pins)
        self.servo = Servo(servo_pin)
        
        # The most recent coordinates from the accelerameter
        self.coords = (0, 0, 0)

        # Whether or not to continue running the server
        self._run = True

        self.start()

    def start(self):
        """ Initialize and start threads. """

        self._server_thread = threading.Thread(target=self._server_worker)
        self._server_thread.start()
        self._control_thread = threading.Thread(target=self._control_worker)
        self._control_thread.start()

    def stop(self):
        """ Shut down server and control threads. """
        self._run = False

    def _server_worker(self):
        HOST = ''  # Symbolic name meaning all available interfaces
        PORT = self.port
        s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        s.bind((HOST, PORT))
        s.listen(1)
        conn, addr = s.accept()

        print 'Connected by', addr
        while self._run:
            data = conn.recv(1024)
            if data:
                coords = data[1:].split(',')
                x, y, z = [float(n) for n in coords]
                self.coords = (x, y, z)
            conn.sendall(data)
        conn.close()

    def _control_worker(self):
        while self._run:
            x, y, z = self.coords
            forward_speed = -y/10
            turning_power = (x+10)/20

            self.motor.drive(forward_speed)
            self.servo.set(turning_power)

    def __init__(self, verbose=False):

        self.verbose = verbose

        capture = cv2.VideoCapture(0)
        capture.set(cv2.CAP_PROP_FRAME_WIDTH, 960)
        capture.set(cv2.CAP_PROP_FRAME_HEIGHT, 500)
        capture.set(cv2.CAP_PROP_BUFFERSIZE, 3)
        capture.set(cv2.CAP_PROP_FPS, 60)
        self.__capture_manager = CaptureManager(capture, 10)

        self.__io_manager = IOManager(IO_PINS, verbose)

        self.__image_analyzer = ImageAnalyzer(self.__capture_manager.current_frame_color, verbose)

        self.__coordinates_translator = CoordinatesTranslator(self.__image_analyzer, CONVEYOR_WIDTH)

        self.__packager = Packager(TRAY_SIZE, PADDING, verbose)

        self.__item_movement_motor = Motor(MOTOR_PINS['item_movement_control'],
                                           MOTOR_PINS['item_movement_direction'],
                                           ITEM_MOVEMENT_CONVEYOR_STEP_INTERVAL,
                                           ITEM_MOVEMENT_CONVEYOR_STEP_LIMIT,
                                           MOTOR_PINS['item_movement_limit_switch'],
                                           0.15,
                                           10,
                                           0.15,
                                           10,
                                           self.__io_manager,
                                           verbose)

        self.__io_manager.protect_pin(MOTOR_PINS['item_movement_control'])
        self.__io_manager.protect_pin(MOTOR_PINS['item_movement_direction'])
        self.__io_manager.protect_pin(MOTOR_PINS['item_movement_limit_switch'])

        self.__tray_movement_motor = Motor(MOTOR_PINS['tray_movement_control'],
                                           MOTOR_PINS['tray_movement_direction'],
                                           TRAY_MOVEMENT_CONVEYOR_STEP_INTERVAL,
                                           None,
                                           MOTOR_PINS['tray_movement_limit_switch'],
                                           0.15,
                                           10,
                                           0.15,
                                           10,
                                           self.__io_manager,
                                           verbose)

        self.__io_manager.protect_pin(MOTOR_PINS['tray_movement_control'])
        self.__io_manager.protect_pin(MOTOR_PINS['tray_movement_direction'])
        self.__io_manager.protect_pin(MOTOR_PINS['tray_movement_limit_switch'])

	self.current_loop_done = 1
	self.__calibrate_motors()

class Frank:

    def __init__(self):
        print "Set mode BCM"
        GPIO.setmode(GPIO.BCM)
        self.motorX = Motor([6, 13, 19, 26])
        #self.motorY = Motor([])
        #self.sensor = Sensor(4) 
        self.setUpScreen()

    def setUpScreen(self):
        self.screen = curses.initscr()
        # turn off input echoing
        curses.noecho()
        # respond to keys immediately (don't wait for enter)
        curses.cbreak()
        # map arrow keys to special values
        self.screen.keypad(True)

    def isArrowKey(self, char):
        return char in [curses.KEY_RIGHT,curses.KEY_LEFT, curses.KEY_UP, curses.KEY_DOWN]

    def start(self):
        try:
            char = self.screen.getch()
            while True :
                
                while self.isArrowKey():
                    if char == curses.KEY_RIGHT:
                        print 'right'
                        self.motorX.moveTo(Motor.RIGHT)
                    elif char == curses.KEY_LEFT:
                        print 'left '
                        self.motorX.moveTo(Motor.LEFT)
                    elif char == curses.KEY_UP :
                        print 'up'
                    elif char == curses.KEY_DOWN :
                        print 'down'
                    char = self.screen.getch()
                if char == ord('q'):
                    self.cleanUp()
                    break
                else:
                    char = self.screen.getch()

        finally:
            # shut down cleanly
            curses.nocbreak(); self.screen.keypad(0); curses.echo()
            curses.endwin()

    def cleanUp(self):
        GPIO.cleanup()

 def add_motor(self, line):
     """
     Add a new motor to the motor list
     :param line: motor parameters line as string from config file
     motor_number, motor_name, start_position, min_limit, max_limit
     :return:
     """
     line_list = line.split()
     new_motor = Motor(line_list[1])
     new_motor.motor_number = int(line_list[0])
     new_motor.goal_position = int(line_list[2])
     new_motor.min_limit = int(line_list[3])
     new_motor.max_limit = int(line_list[4])
     self.motor_list[line_list[1]] = new_motor

    def __init__(self):
        print "Set mode BCM"
        config = json.load(open("config.json"))
        GPIO.setmode(GPIO.BCM)
        self.motorX = Motor(config["motor_x"]["pins"])
        self.motorX.name = "X"
        self.motorX.delay = config["motor_x"]["delay"]
        self.motorX.steps_by_mm = config["motor_x"]["steps_by_mm"]

        self.motorY = Motor(config["motor_y"]["pins"])
        self.motorY.name = "Y"
        self.motorY.delay = config["motor_y"]["delay"]
        self.motorY.steps_by_mm = config["motor_y"]["steps_by_mm"]

        self.laser = Laser(config["laser"]["pin"])

    def handle_read(self):
       
        data0 = self.recv(160);
        if data0:            
            data = ConstBitStream(bytes=data0, length=160)
            # print "All: %s" % data.bin
 
            msize = data.read('intle:16')
            mtype = data.read('intle:16')
            mtime = data.read('intle:64')
 
            # RA: 
            ant_pos = data.bitpos
            ra = data.read('hex:32')
            data.bitpos = ant_pos
            ra_uint = data.read('uintle:32')
 
            # DEC:
            ant_pos = data.bitpos
            dec = data.read('hex:32')
            data.bitpos = ant_pos
            dec_int = data.read('intle:32')
             
            logging.debug("Size: %d, Type: %d, Time: %d, RA: %d (%s), DEC: %d (%s)" % (msize, mtype, mtime, ra_uint, ra, dec_int, dec))
                       
            (ra, dec, time) = coords.int_2_rads(ra_uint, dec_int, mtime)

            x = transformar_coordenadas(dec, ra)
            az,alt = x.get_azi_alt()

            #instancia o motor de azimute nos pinos 12, 16, 20 e 21 do RPi
            motor_az = Motor([31,33,35,37])
            motor_az.rpm = 5
            motor_az.mode = 2
            motor_az.move_to(az-self.az_anterior)
            self.az_anterior = az

            #instancia o motor de azimute nos pinos 32, 36, 38 e 40 do RPi
            motor_alt = Motor([32,36,38,40])
            motor_alt.rpm = 5
            motor_alt.mode = 2
            motor_alt.move_to(alt-self.alt_anterior)
            self.alt_anterior = alt

            logging.debug("Azimute: %d, Altitude: %d" % (az,alt))
            
            # envia as cordenadas para o Stellarium
            self.act_pos(ra, dec)

	def __init__(self, debugMode = False):
		GPIO.setmode(GPIO.BOARD)
		GPIO.setwarnings(False)

		if debugMode:
			level = logging.DEBUG
		else:
			level = logging.CRITICAL
		logging.basicConfig(stream=sys.stderr, level=level)

		self.motor 			= Motor(GPIO, logging)
		self.panandtilt = PanAndTilt(logging)
		self.distance 	= Distance(GPIO, logging)

		self.MoveDirectionsOptions = {
			'fwd': self.motor.forward,
			'stp': self.motor.stop,
			'lft': self.motor.leftTurn,
			'rgt': self.motor.rightTurn,
			'bwd': self.motor.backward,
			'lfm': self.motor.left,
			'rgm': self.motor.right,
		}

		self.LookDirectionsOptions = {
			'fnt': self.panandtilt.front,
			'lft': self.panandtilt.left,
			'rgt': self.panandtilt.right,
			'up': self.panandtilt.up,
			'dwn': self.panandtilt.down,
		}		

    def __init__(self, mode):
        self.mode = mode
        adafruitLoader = AdafruitLoader(self.mode)
        self.pwm = adafruitLoader.getPwmModule()
        self.bno = adafruitLoader.getBNO055Module()

        if not self.bno.begin():
            raise RuntimeError('Failed to initialize BNO055! Is the sensor connected?')

        # Set frequency to 60hz, good for servos.
        self.pwm.set_pwm_freq(60)

        self.motor1 = Motor(0, self.pwm, 276,457)
        self.motor2 = Motor(1, self.pwm, 276,457)
        self.motor3 = Motor(14, self.pwm, 276,457)
        self.motor4 = Motor(15, self.pwm, 276,457)

 def __init__(self, address, pin, maxthrottle=200, minthrottle=500, name="motor"):
     self.name = name
     self.address = address
     self.server = ZMQServer(address)
     self.motor = Motor(pin, minthrottle, maxthrottle)
     self.motor.setmin()
     self.quit = False
     self.engage()

def go(min = 20, max = 50, n_steps = 5, zenith = 0, samp_rate = 4400, acc_len = 1, n_accs = 10,
       port = '/dev/ttyUSB0', ip = '128.135.52.192', home=True, docal=True, indef=True):
    '''
    Main function that creates motor, spec, and hdf5 objects, calculates the computer's offset
    from ntp time, and calls snap_and_move() in order to sweep the horn through a range of
    elevations and write accumulations and metadata to disk. Closes file and creates a new file
    after each return to zenith.

    Inputs:
        Step size in degrees, zenith angle in degrees, min and max angles in degrees, sample 
        rate in MHz, accumulation length in seconds, number of accumulations, /dev address of 
        motor controller, output filename, and ip address of roach board. 

    Outputs: 
        None,  writes to disk and std out.
    '''
    m = Motor(port = port)
    angles = np.sign(min)*scan_range(min, max, n_steps)

    # Flag calibration data if not scanning indefinitely
    if not indef:
        calext='_cal'
    else:
        calext='_scan'

    while True:
        while True:
            try:
                s = Spec(ip = ip, samp_rate = samp_rate, acc_len = acc_len) #re-initialize roach
                break
            except Exception:
                pass
            
        dt = 0 #ts.offset()
        fname = '/'.join(os.path.abspath(io.__file__).split('/')[:-2])\
                + '/output/' + ts.true_time(dt) + calext + '.h5'
        if home:
            #print('Homing')
            m.abst(0)
            m.home()
        if docal:
            move_and_snap(m, s, fname, zenith, CALIBRATOR_POSITION + zenith, acc_len, n_accs, dt)
        for destination in tqdm.tqdm(angles, unit = 'steps'):
            move_and_snap(m, s, fname, zenith, destination, acc_len, n_accs, dt)
        if not indef:
            break

	def __init__(self):
		self.logging = False
		self.setDefaults()
		self.interface = brickpi.Interface()
		self.interface.initialize()
		self.events = Events()
		self.motorL = Motor(self.interface, self.events, 0)
		self.motorR = Motor(self.interface, self.events, 1)
		self.initMotorParams(self.motorL.motorParams)
		self.initMotorParams(self.motorR.motorParams)

		self.initConfig()
		self.touchSensorL = PushSensor('left',  self.interface, 0, self.events, brickpi.SensorType.SENSOR_TOUCH)
		self.touchSensorR = PushSensor('right', self.interface, 1, self.events, brickpi.SensorType.SENSOR_TOUCH)
		Bumper(self.events)
		self.ultraSonic = UltraSonicSensor(self.interface, 2, self.events, brickpi.SensorType.SENSOR_ULTRASONIC)
		self.setPID(self.pidk_p, self.pidk_i, self.pidk_d)

		self.events.add(EventType.SENSOR_TOUCH, self.sensorAction)

    def __init__(self, configuration):
        """Create a new instance of the clock class.
        """
        self._configuration = configuration
        self._logger = logging.getLogger(__name__)

        self._motor = Motor(self._configuration)
        self._blinker = Blinker(self._configuration)
        self._display = Display(self._configuration)
        self._database = Database(self._configuration)
        self._consumer = Consumer(self._configuration, self.on_message)

    def __init__(self, motor_pins=(24, 25), servo_pin=0, port=2012):
        self.port = port

        # The motor and servo for driving
        self.motor = Motor(*motor_pins)
        self.servo = Servo(servo_pin)
        
        # The most recent coordinates from the accelerameter
        self.coords = (0, 0, 0)

        # Whether or not to continue running the server
        self._run = True

        self.start()