Beispiel #1
0
def piSetUp():

	GPIO.setmode(GPIO.BOARD)
	
	TIC1 = TIC()
	TIC1.TICstart()

	motorL = motor("left", [32, 31, 33])
        encoderA = encoder("left", [26, 29], 50, motorL)
        
        motorR = motor("right", [35, 36, 37])
        encoderB = encoder("left", [38, 40], 50, motorR)
        
        proxSensA= psensor("Front",[16,18], 50)
        proxSensB= psensor("Left",[12,22], 35)
        proxSensC= psensor("Right",[7,13], 35)
        
        rob = robot(motorL, motorR, encoderA, encoderB, proxSensA, proxSensB, proxSensC)
        rob.pinSetup()
        
        tnow = time.time()
        rob.obsDetect()

        while(1):
            rob.run()
            TIC1.run()
Beispiel #2
0
def par_test():

    print "Number of active threads before starting: " + str(
        threading.activeCount())
    print "Active threads before starting: " + str(threading.enumerate())

    pi = pigpio.pi()
    sonar_queue = Queue.Queue()
    encoder_queue = Queue.Queue()

    sonar = ranger(pi=pi, trigger=17, echo=27, queue=sonar_queue)
    enc = encoder(pi=pi, signal_pin=2, queue=encoder_queue)
    motor_left = motor(pi=pi, forward_pin=26, back_pin=19)
    motor_right = motor(pi=pi, forward_pin=16, back_pin=20)
    car = Car(pi=pi,
              motor_left=motor_left,
              motor_right=motor_right,
              encoder_left=enc,
              encoder_right=enc,
              sonar=sonar,
              encoder_queue=encoder_queue,
              sonar_queue=sonar_queue)

    result = car.parallel_test()

    print "Counts: " + str(result[0])
    print "Sonar Readings: " + str(result[1])
    print "Encoder Readings: " + str(result[2])

    car.cleanup()
Beispiel #3
0
    def _twist_callback(self, msg):
        self._cmd_lin_vel = msg.linear.x
        self._cmd_ang_vel = msg.angular.z

        print "x: " + str(self._cmd_lin_vel)
        print "yaw: " + str(self._cmd_ang_vel)

        # Disable both motors
        if self._cmd_lin_vel == 0 and self._cmd_ang_vel == 0:
            motor.motorStop()
        else:
            # Forward driving
            if self._cmd_lin_vel > 0:
                self._left_motor_dir = 0
                self._right_motor_dir = 0
            # Reverse driving
            elif self._cmd_lin_vel < 0:
                self._left_motor_dir = 1
                self._right_motor_dir = 1
            # CCW Rotation
            elif self._cmd_ang_vel < 0:
                self._left_motor_dir = 1
                self._right_motor_dir = 0
            # CW Rotation
            elif self._cmd_ang_vel > 0:
                self._left_motor_dir = 0
                self._right_motor_dir = 1
            motor.motor1(1, self._left_motor_dir, 100)
            motor.motor(1, self._right_motor_dir, 100)
Beispiel #4
0
    def init_rect_model(self, width=50, height=50, x_offset=0, y_offset=0):
        '''
        ul : upper left      dl : down left
        ur : upper right     dr : down right
        
        MODEL : 
        
        ul----------ur
        |            |
        |            |
        |    BOT     |
        |            |
        |            |
        dl-----------dr
        '''
        #Setting up motors :
        self.ul_motor = motor()
        self.ur_motor = motor()
        self.dl_motor = motor()
        self.dr_motor = motor()

        #setting coordinates:
        self.width = width
        self.height = height
        "These parameters will remain constant."

        #Offsets:
        "This is the factor which will change the position of the bot."
        self.x_offset = x_offset
        self.y_offset = y_offset
Beispiel #5
0
    def __init__(self):
        #オブジェクトの生成
        self.rightmotor = motor.motor(ct.const.RIGHT_MOTOR_IN1_PIN,
                                      ct.const.RIGHT_MOTOR_IN2_PIN,
                                      ct.const.RIGHT_MOTOR_VREF_PIN)
        self.leftmotor = motor.motor(ct.const.LEFT_MOTOR_IN1_PIN,
                                     ct.const.LEFT_MOTOR_IN2_PIN,
                                     ct.const.LEFT_MOTOR_VREF_PIN)
        self.gps = gps.GPS()
        self.bno055 = bno055.BNO055()
        self.radio = radio.radio()
        self.RED_LED = led.led(ct.const.RED_LED_PIN)
        self.BLUE_LED = led.led(ct.const.BLUE_LED_PIN)
        self.GREEN_LED = led.led(ct.const.GREEN_LED_PIN)

        #開始時間の記録
        self.startTime = time.time()
        self.timer = 0
        self.landstate = 0  #landing stateの中でモータを一定時間回すためにlandのなかでもステート管理するため
        self.v_right = 100
        self.v_left = 100

        #変数
        self.state = 0
        self.laststate = 0
        self.landstate = 0

        #stateに入っている時刻の初期化
        self.preparingTime = 0
        self.flyingTime = 0
        self.droppingTime = 0
        self.landingTime = 0
        self.pre_motorTime = 0
        self.waitingTime = 0
        self.runningTime = 0
        self.goalTime = 0

        #state管理用変数初期化
        self.countPreLoop = 0
        self.countFlyLoop = 0
        self.countDropLoop = 0
        self.countGoal = 0
        self.countAreaLoopEnd = 0  # 終了判定用
        self.countAreaLoopStart = 0  # 開始判定用
        self.countAreaLoopLose = 0  # 見失い判定用
        self.countgrass = 0

        #GPIO設定
        GPIO.setmode(GPIO.BCM)  #GPIOの設定
        GPIO.setup(ct.const.FLIGHTPIN_PIN, GPIO.IN)  #フライトピン用

        date = datetime.datetime.now()
        self.filename = '{0:%Y%m%d}'.format(date)
        self.filename_hm = '{0:%Y%m%d%H%M}'.format(date)

        if not os.path.isdir(
                '/home/pi/Desktop/wolvez2021/Testcode/EtoEtest/%s/' %
            (self.filename)):
            os.mkdir('/home/pi/Desktop/wolvez2021/Testcode/EtoEtest/%s/' %
                     (self.filename))
Beispiel #6
0
def handle_message(event):
    text = event.text

    if text == "あけ":
        motor.motor("open")

        profile = line_bot_api.get_profile(event.source.user_id)
        line_bot_api.push_message(
            "U90270fbcc310d31bb0c7bdbaa1e4b01c",
            TextSendMessage(text="{}が開けました".format(profile.display_name)))

        acc.Sensing_acceleration()

        line_bot_api.reply_message(event.reply_token,
                                   TextSendMessage(text=event.message.text))

    elif text == "しめ":
        motor.motor("close")
        acc.Sensing_acceleration()
        line_bot_api.reply_message(event.reply_token,
                                   TextSendMessage(text=event.message.text))

    else:
        line_bot_api.reply_message(event.reply_token,
                                   TextSendMessage(text=event.message.text))
Beispiel #7
0
    def __init__(self):
        self.pi = pigpio.pi()

        self.FL = motor(15, 14, 18, self.pi)
        self.FR = motor(20, 16, 21, self.pi)

        self.BL = motor(3, 2, 4, self.pi)
        self.BR = motor(5, 6, 13, self.pi)
Beispiel #8
0
    def __init__(self):
        # ---------- SETTINGS ---------- #
        self.MIN_CYCLE_PERIOD = 10  # Minimum time (in ms) between program iterations

        # -------- PORT MAPPING -------- #
        self.RED_LED_PORT = 28  # GP28 Pin 34
        self.GREEN_LED_PORT = 27  # GP27 Pin 32
        self.BUZZER_PORT = 22  # GP22 Pin 29

        self.WHEEL_1_PORT = 8  # GP8 Pin 11
        self.WHEEL_2_PORT = 12  # GP12 Pin 16
        self.WHEEL_3_PORT = 14  # GP14 Pin 19

        self.ENC_1A_PORT = 2  # GP2 Pin 4
        self.ENC_1B_PORT = 3  # GP3 Pin 5
        self.ENC_2A_PORT = 4  # GP4 Pin 6
        self.ENC_2B_PORT = 5  # GP5 Pin 7
        self.ENC_3A_PORT = 6  # GP6 Pin 9
        self.ENC_3B_PORT = 7  # GP7 Pin 10

        self.I2C_SDA = 0  # GP0 Pin 1
        self.I2C_SCL = 1  # GP1 Pin 2

        # ------------ VARS ------------ #
        self.enabled = False
        self.connected = False
        self.i2caddr = 0x0
        self.sqrt3 = 3**0.5 / 2  # Finding square roots (and division) is time consuming. Let's only do it once

        # --------- INITIALIZE --------- #
        self.red_led = machine.Pin(self.RED_LED_PORT, machine.Pin.OUT)
        self.red_led.value(0)
        self.green_led = machine.Pin(self.GREEN_LED_PORT, machine.Pin.OUT)
        self.green_led.value(0)
        self.buzzer = machine.Pin(self.BUZZER_PORT, machine.Pin.OUT)
        self.buzzer.value(0)

        self.wheel1 = motor(self.WHEEL_1_PORT)
        self.wheel2 = motor(self.WHEEL_2_PORT)
        self.wheel3 = motor(self.WHEEL_3_PORT)

        self.encoder1 = encoder(self.ENC_1A_PORT, self.ENC_1B_PORT)
        self.encoder2 = encoder(self.ENC_2A_PORT, self.ENC_2B_PORT)
        self.encoder3 = encoder(self.ENC_3A_PORT, self.ENC_3B_PORT)

        self.kiwi_encoder = kiwi_encoders(self.encoder1, self.encoder2,
                                          self.encoder3)

        sda = machine.Pin(self.I2C_SDA)
        scl = machine.Pin(self.I2C_SCL)
        self.i2c = machine.I2C(0, sda=sda, scl=scl, freq=400000)
        self.wdt = None
Beispiel #9
0
	def __init__(self):
		# setting motors
		self.rate_pid = []
		self.motor_pid = []
		for i in range(0, 3):
			self.rate_pid.append(PIDcontrol(config.ANGLE_KP, config.ANGLE_KI, config.ANGLE_KD, config.ANGLE_MAX, config.ANGLE_MIN))
			self.motor_pid.append(PIDcontrol(config.RATE_KP, config.RATE_KI, config.RATE_KD, config.RATE_MAX, config.RATE_MIN))

		self.motor_standard = 10
		self.angle_standard = [0,3,0]
		self.motors = {'left' : motor('left', 25, simulation=False),
				'right' : motor('right', 23, simulation=False),
				'rear' : motor('rear', 24, simulation=False),
				'front' : motor('front', 22, simulation=False) }
Beispiel #10
0
def handle_beacon(event):
    #print(event.source.userid)
    motor.motor("open")

    profile = line_bot_api.get_profile(event.source.user_id)
    line_bot_api.push_message(
        "U90270fbcc310d31bb0c7bdbaa1e4b01c",
        TextSendMessage(text="{}が開けました".format(profile.display_name)))

    acc.Sensing_acceleration()

    line_bot_api.reply_message(
        event.reply_token,
        TextSendMessage(
            text=
            'Got beacon event. hwid={}, device_message(hex string)={}, event_type={}'
            .format(event.beacon.hwid, event.beacon.dm, event.beacon.type)))
Beispiel #11
0
def straightRun():

	GPIO.setmode(GPIO.BOARD)
	
	motorL = motor("left", [32, 31, 33])
        encoderA = encoder("left", [26, 29], 50, motorL)
            
        motorR = motor("right", [35, 36, 37])
        encoderB = encoder("left", [38, 40], 50, motorR)
	
	proxSensA= psensor("Front",[16,18], 50)
        proxSensB= psensor("Left",[12,22], 35)
        proxSensC= psensor("Right",[7,13], 35)
	
	rob = robot(motorL, motorR, encoderA, encoderB, proxSensA, proxSensB, proxSensC)
        rob.pinSetup()
	
	while(1):
            rob.direct("forward", 50)
def pid_controller(P=1, I=1, D=1):
	
	
	T=1		#Periodo
	numlecturas = 10
	#lecturas=[0]*numlecturas
	
	
	#Inicialización
	ui_prev = 0
	e_prev = 0
	e_acumulado = 0
	ti = time.clock() #tiempo inicial
	Referencia = 0
	
	#Actuadores
	motor1=motor.motor(1)
	motor2=motor.motor(2)
	
	#IMU
	imu1=IMU.IMU()
	
	while True:
		# Error between the desired and actual output
		ta=time.clock()
		sensor = 0
		for i in range(numlecturas):
			sensor += imu1.SetAcc[1]
			i+=1
		sensor /= numlecturas
		
		if ta - ti >= T:
			e_actual = sensor - Referencia
			e_acumulado += e_actual
			diferencia_e = e_actual - e_prev
			
			control_PID = e_actual * P + e_acumulado * I + diferencia_e * D
			
			

			# Adjust previous values
			e_prev = e_actual
			ti = time.clock()
Beispiel #13
0
def motor_mod():
    try:
        from motor import main as motor
    except Exception as e:
        logging.warning('motor.py脚本不存在, 或者motor.py脚本不存在main模块 ')
    else:
        try:
            optInfo=motor()
        except Exception as e:
            logging.error('获取motor数据失败!  ')
        else:
            return optInfo
Beispiel #14
0
    def __init__(self, name, no_of_propellers):
        self.name = name
        self.propellers = []
        self.__counters = [0] * no_of_propellers
        props = []
        for index in range(0, no_of_propellers):
            propeller_instance = motor.motor(TEXT_PROPELLER + str(index), 17, simulation=False)
            propeller_instance.setW(0)
            self.propellers.append(propeller_instance)
            props.append((TEXT_PROPELLER + '-' + str(index), ''))

        self.__keys = OrderedDict(props)
Beispiel #15
0
def pid_controller(P=1, I=1, D=1):

    T = 1  #Periodo
    numlecturas = 10
    #lecturas=[0]*numlecturas

    #Inicialización
    ui_prev = 0
    e_prev = 0
    e_acumulado = 0
    ti = time.clock()  #tiempo inicial
    Referencia = 0

    #Actuadores
    motor1 = motor.motor(1)
    motor2 = motor.motor(2)

    #IMU
    imu1 = IMU.IMU()

    while True:
        # Error between the desired and actual output
        ta = time.clock()
        sensor = 0
        for i in range(numlecturas):
            sensor += imu1.SetAcc[1]
            i += 1
        sensor /= numlecturas

        if ta - ti >= T:
            e_actual = sensor - Referencia
            e_acumulado += e_actual
            diferencia_e = e_actual - e_prev

            control_PID = e_actual * P + e_acumulado * I + diferencia_e * D

            # Adjust previous values
            e_prev = e_actual
            ti = time.clock()
Beispiel #16
0
def motorTest():
    motorL = motor("left", [32, 31, 33])
    encoderA = encoder("left", [26, 29], 50, motorL)

    motorL.pinSetup()
    encoderA.pinSetup()

    motorL.setVolt(55)

    tnow = time.time()

    while (tnow + 3 > time.time()):
        encoderA.RPMcalc()
    def attempt_connection(self):
        if self.m is not None:
            del (self.m)
            self.m = None
        try:
            self.connect_status('Connecting...')
            self.m = motor.motor(port=self.motor_port.get(),
                                 calibration_file='3d3a_stage_calibration.dat')
            self.connect_status('Connected')
            self.m.safe_start_off()

        except (motor.serial.SerialException, AttributeError) as e:
            print('Could not connect to motor.')
            self.m = None
            self.connect_status('Could not connect')
def post_motorsteps(hat: int, position: int, reverse: int, numsteps: int):
    """
        Allows a single motor to be controlled by address
    """
    log.debug(
        "post_motorsteps - Starting. hat={0} position={1} reverse={2} numsteps={3}"
        .format(hat, position, reverse, numsteps))
    try:
        hat = int('0x6{0}'.format(hat), 16)
    except Exception as e:
        log.error("{0}".format(str(e)))
    test_motor = motor(hat, position)
    for step in range(numsteps):
        log.debug("Stepping hat {0} position {1} reverse {2}".format(
            hat, position, bool(int(reverse))))
        test_motor.step(bool(int(reverse)))
Beispiel #19
0
    def __init__(self, shared): 
        threading.Thread.__init__(self) 
        self.shared = shared
        self.minESC = 0
        self.neutralESC = 35
        self.maxESC = 100
        self.shared.set('neutralESC', self.neutralESC)
        self.shared.set('rpm', self.neutralESC)

        # Set Pin 24 as ESC
        self.servoPin = 24
        self.mymotor = motor('m1', self.servoPin, simulation=False)
        self.mymotor.start()
        self.mymotor.setW(self.maxESC)
        time.sleep(2)
        self.mymotor.setW(self.neutralESC)
        self.oldspeedValue = self.neutralESC
        self._stopevent = threading.Event( ) 
Beispiel #20
0
    def __init__(self, LMUP, LMCENTER, LMDOWN, motorPin1, motorPin2):
        self.motor = motor.motor(motorPin1, motorPin2)
        self.LMUP = LMUP
        self.LMCENTER = LMCENTER
        self.LMDOWN = LMDOWN
        self.virtualPosition = FLOATING

        #DECLAIT LIMIT SWITICHES AS INPUT

        GPIO.setmode(GPIO.BOARD)
        GPIO.setup(LMUP, GPIO.IN, pull_up_down=GPIO.PUD_UP)
        GPIO.setup(LMDOWN, GPIO.IN, pull_up_down=GPIO.PUD_UP)
        GPIO.setup(LMCENTER, GPIO.IN, pull_up_down=GPIO.PUD_UP)

        #inintal position of hand well be center
        self.virtualPosition = self.getCurrentPosition()
        self.lastPosition = self.getCurrentPosition()
        self.center()
Beispiel #21
0
def main():
    deviceDrivers={}
    thing = motor(10)
    while(1):
        thing.writePercent_Master(50)
        
    while(1):
        if userInput == "f":
            deviceDrivers[LEFTWHEEL].writePercent_Master(50)
            deviceDrivers[RIGHTWHEEL].writePercent_Master(750)
            time.sleep(2)
        if userInput == "b":
            deviceDrivers[LEFTWHEEL].writePercent_Master(-75)
            deviceDrivers[RIGHTWHEEL].writePercent_Master(-75)
            time.sleep(2)
        if userInput == "tr":
            deviceDrivers[LEFTWHEEL].writePercent_Master(75)
            deviceDrivers[RIGHTWHEEL].writePercent_Master(25)
            time.sleep(2)
        if userInput == "tl":
            deviceDrivers[LEFTWHEEL].writePercent_Master(25)
            deviceDrivers[RIGHTWHEEL].writePercent_Master(75)
            time.sleep(3)
        if userInput == "90r":
            deviceDrivers[LEFTWHEEL].writePercent_Master(75)
            deviceDrivers[RIGTHWHEEL].writePercent_Master(-75)
            time.sleep(3)
        if userInput == "90l":
            deviceDrivers[LEFTWHEEL].writePercent(-75)
            deviceDrivers[RIGHTWHEEL].writePercent(75)
        if userInput == "c":
            deviceDrivers[CONVEYORBELT].writePercent_Master(CONVEYORBELTSPEED)
        else:
            deviceDrivers[CONVEYORBELT].writePercent_Master(0)
        if userInput == "t":
            deviceDrivers[CONVERYORTILT].writePercent_Master(CONVERYORTILTSPEEDBACK)
            time.sleep(5)
        if userInput == "d":
            deviceDrivers[HOPPERMOTOR].writePercent_Master(HOPPERSPEED)
            time.sleep(10)
            deviceDrivers[HOPPERMOTOR].writePercent_Master(0)
        userInput = input("Enter a command: ")
    time.sleep(.1)
Beispiel #22
0
def main():

    motor1 = motor(7, 11, 15)

    if len(sys.argv) > 2 and sys.argv[1] == 's':
        motor1.stop()
        print("Motor Stopped")

        return 0

    motor1.backward()

    time.sleep(2.5)

    motor1.speed(50)
    motor1.forward()

    time.sleep(2.5)

    motor1.stop()
Beispiel #23
0
	def __init__(self):
		
		self.Tdes=36.25
		self.periodo=10.0
		self.maxTemp = 36.4  #Emu    #37.8   #Gen
		self.minTemp = 36.1  #Emu    #37.6   #Gen
		self.maxHumi = 29    #Emu    #55     #Gen
		self.minHumi = 26    #Emu    #50     #Gen

		self.stop=1

		self.st1='Bombilla ceramica Off'
		self.st2='Ventiladores Off'
		self.st3='Humidificador Off'
		self.SHT15 = BB_SHT15_lib.BB_SHT15()
		time.sleep(0.5)
		self.motorDC = motor.motor()
		time.sleep(0.5)
		self.iTemp = self.SHT15.temperature()
		time.sleep(0.5)
		self.iHumi = self.SHT15.humidity()

		LOW = 0
		HIGH = 1
		self.motorDC.preexecute() # Starting PWM for the EN of the motors
		self.motorDC.motorSpeed(39)
		GPIO.setup("P8_22", GPIO.OUT) # Motor giro Derecha
		GPIO.cleanup()
		GPIO.setup("P8_23", GPIO.OUT) # Motor giro Izquierda
		GPIO.cleanup()

		GPIO.setup("P8_26", GPIO.OUT) # RELE TEMP
		GPIO.cleanup()
		GPIO.setup("P8_27", GPIO.OUT) # RELE HUMI
		GPIO.cleanup()
		GPIO.setup("P8_28", GPIO.OUT) # RELE HUMI
		GPIO.cleanup()
		self.lastiTemp=0
		self.estado=0
		time.sleep(0.5)
    def __init__(self, mode='Ar'):

        self.mode = mode

        self.motor = motor()

        GPIO.setmode(GPIO.BCM)
        GPIO.setwarnings(False)

        GPIO.setup(self.LED, GPIO.OUT)

        # BLOB
        self.blob_detector = cv.SimpleBlobDetector_create(params)

        # ARUCO
        self.aruco_parameters = aruco.DetectorParameters_create()
        self.dictionary = cv.aruco.Dictionary_get(cv.aruco.DICT_6X6_250)
        self.aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250)

        # Serial
        try:
            self.ser = serial.Serial(USB_PORT, 115200, timeout=1)
            self.ser.flush()
        except:
            self.ser = None

        # Button
        GPIO.setup(
            self.BUTTON, GPIO.IN, pull_up_down=GPIO.PUD_DOWN
        )  # Set pin 10 to be an input pin and set initial value to be pulled low (off)
        GPIO.add_event_detect(
            self.BUTTON, GPIO.BOTH,
            callback=self.rising_callback)  # Setup event on pin 10 rising edge

        self.limit_switch = False

        self.tr = None
Beispiel #25
0
    def __init__(self, config = None):

        self.hostname = gethostname()
        self.load_config(config)
        self.setup_logging()
        self.log.info("Initializing '%s' using config '%s'" % (self.hostname, self.config['config']) )

        # TODO: move details into a drive_set(?)
        self.motors = {}
        for i in [1,2,3]:
            self.motors[i] = motor.motor(i)
        self.motor_enable = bbb.GPIO(117)

        self.accel = lsm.accel()
        self.mag = lsm.mag()

        self.dms_mux = dms.DmsMux(self.config['dms_mux'])
        self.dms = self.dms_mux.sensors

        self.battery = battery.Battery(self.config['battery'])
        self.lightsensor = lightsensor.LightSensor(self.config['lightsensor'])
        self.camera = camera.Camera()

        self.log.debug("Initization complete")
enc_B_chan_B = pyb.Pin('Y8',
                       pyb.Pin.AF_PP,
                       pull=pyb.Pin.PULL_UP,
                       af=pyb.Pin.AF2_TIM4)

#Spec encoder counts per revolution
cpr = 2249

#Putting the Pyboard Channels into encoder mode
enc_timer_left = pyb.Timer(2, prescaler=0, period=65535)
enc_timer_right = pyb.Timer(4, prescaler=0, period=65535)
left_encoder = enc_timer_left.channel(1, pyb.Timer.ENC_AB)
right_encoder = enc_timer_right.channel(2, pyb.Timer.ENC_AB)

#Motor Object:
motor_left = motor(PWMA, DIRA, TIMA, CHANA)
motor_right = motor(PWMB, DIRB, TIMB, CHANB)

kp_motor = 1
ki_motor = 0
kd_motor = 0

#Creating PID object for both tracks
left_pid = PID(kp_motor, ki_motor, kd_motor, 100000, 3.5, -3.5)
right_pid = PID(kp_motor, ki_motor, kd_motor, 100000, 3.5, -3.5)

#starting count
initial_count = 0


def arduino_map(x, in_min, in_max, out_min, out_max):
Beispiel #27
0
# [email protected]
# solenerotech.wordpress.com

# solenerotech 2013.09.06

from motor import motor

mymotor = motor("m1", 27, simulation=False)
# where 17 is  GPIO17 = pin 11

print("***Disconnect ESC power")
print("***then press ENTER")
res = raw_input()
mymotor.start()
mymotor.setW(100)

# NOTE:the angular motor speed W can vary from 0 (min) to 100 (max)
# the scaling to pwm is done inside motor class
print("***Connect ESC Power")
print("***Wait beep-beep")

print("***then press ENTER")
res = raw_input()
mymotor.setW(0)
print("***Wait N beep for battery cell")
print("***Wait beeeeeep for ready")
print("***then press ENTER")
res = raw_input()
print("increase > a | decrease > z | save Wh > n | set Wh > h|quit > 9")

cycling = True
firebase_admin.initialize_app(cred)
db = firestore.client()
with open('weather_station.txt', mode='w') as csv_file:
    csv_writer = csv.writer(csv_file,
                            delimiter=',',
                            quotechar='"',
                            quoting=csv.QUOTE_MINIMAL)
    csv_writer.writerow(["temp", "hum", "soil", "light", "rain", "Density"])
while True:
    i += 1
    temp, hum = temp_hum()
    while (temp == "error"):
        temp, hum = temp_hum()
#    if temp>50:
#       motor()
    motor()
    s = arduino()
    if i > 1:
        rain = s["rain"]
        soil = s["Soil"]
        lum = s["Light Intensity"]
    else:
        rain, soil, lum = 0, 0, 0

    doc_ref = db.collection(u'Monitoring_Data').document(str(i))
    doc_ref.set({
        u'Temperature': temp,
        u'humidity': hum,
        u'date': SERVER_TIMESTAMP,
        u'moisture': soil,
        u'light': lum,
Beispiel #29
0
#[email protected]
#solenerotech.wordpress.com

#solenerotech 2013.09.06

from motor import motor

mymotor1 = motor('m1', 17, simulation=False)
mymotor2 = motor('m1', 18, simulation=False)
#where 17 is  GPIO17 = pin 11

print('***Disconnect ESC power')
print('***then press ENTER')
res = raw_input()
mymotor1.start()
mymotor2.start()
mymotor1.setW(100)
mymotor2.setW(100)

#NOTE:the angular motor speed W can vary from 0 (min) to 100 (max)
#the scaling to pwm is done inside motor class
print('***Connect ESC Power')
print('***Wait beep-beep')

print('***then press ENTER')
res = raw_input()
mymotor1.setW(0)
mymotor2.setW(0)
print('***Wait N beep for battery cell')
print('***Wait beeeeeep for ready')
print('***then press ENTER')
Beispiel #30
0
initEsc = False
gpio = 0
parser = argparse.ArgumentParser()
parser.add_argument("gpio", type=int, help="define gpio: 18-23-24-25  pin:12-16-18-22")
parser.add_argument("-i", dest="initEsc", action="store_true", help="Initialize ESC")
args = parser.parse_args()
initEsc = args.initEsc
gpio = args.gpio

logger = setupLogger("myQ")

print("gpio: " + str(gpio))
print("initEsc: " + str(initEsc))

mymotor = motor("m1", gpio, simulation=False)
# where 18 is  GPIO18 = pin 12
# GPIO23 = pin 16
# GPIO24 = pin 18
# GPIO25 = pin 22


mySensor = sensor()
mySensor.start()

print("***Press ENTER to start")
res = raw_input()
mymotor.start()

# TODO the next line code is necessary to INITIALIZE the ESC to a desired MAX PWM
# I suggest to run this line at least once, for each esc
Beispiel #31
0
from motor import motor

motor1 = motor('m1', 17, simulation=False)
motor2 = motor('m1', 18, simulation=False)
motor3 = motor('m1', 25, simulation=False)
motor4 = motor('m1', 22, simulation=False)

motors = [motor1, motor2, motor3, motor4]

print('***Disconnect ESC power')
print('***then press ENTER')
res = raw_input()
try:
        for mitour in motors:
                mitour.start()
                mitour.setW(100)

	print('***Connect ESC Power')
	print('***Wait beep-beep')

        res = raw_input()
	for mitour in motors:
                mitour.start()
                mitour.setW(0)
	print('***Wait N beep for battery cell')
	print('***Wait beeeeeep for ready')
	print('***then press ENTER')
	res = raw_input()
	
	for mitour in motors:
Beispiel #32
0
from led import led
from time import sleep             # lets us have a delay  
from pid import pid
from mpu6050 import mpu6050
import RPi.GPIO as GPIO            # import RPi.GPIO module  


# set up inputs 
GPIO.setmode(GPIO.BCM)
GPIO.setup(18, GPIO.IN, pull_up_down=GPIO.PUD_UP) # blue button
GPIO.setup(23, GPIO.IN, pull_up_down=GPIO.PUD_UP) # yellow button




motorA = motor(17,27)
motorB = motor(22,10)

gyro=mpu6050()
gyro.setFilter(2)

greenLed = led(24)
yellowLed = led(25)

greenLed.on()
yellowLed.off()

# regulator settings
Ta = 0.05
Time =120 / Ta
regulator = pid()
Beispiel #33
0
from motor import motor


def Stopall():
	'''Stop all the motors, with pulswidth 1000 us'''
	for mymotor in Motorlist:
		mymotor.setW(0)

def PowerOffAll():
	'''Power off all the motors'''
	for mymotor in Motorlist:
		mymotor.stop()

Motorlist=[]   

mymotor1 = motor('m1', 17, simulation=False)
Motorlist.append(mymotor1)
#where 17 is  GPIO17 = pin 11
# Front-Right

mymotor2 = motor('m2', 27, simulation=False)
Motorlist.append(mymotor2)
#where 21 is  GPIO27 = pin 13

mymotor3 = motor('m3', 22, simulation=False)
Motorlist.append(mymotor3)
#where 22 is  GPIO22 = pin 15
# Back-Right

mymotor4 = motor('m4', 23, simulation=False)
Motorlist.append(mymotor4)

import sys		# to read in cmd args
import threading	# run concurrent motors
import threader		# really more of a 'parser' -- needs a rename
import time
import returnHome
import heating
 
from printHead import PrintHead
pos = PrintHead()
print("Initial Printer Position ", pos.x, pos.y, pos.z)

from motor import motor
motors = [None]*4
motors[0] = motor("x", Vars.xDIR, Vars.xSTEP)
motors[1] = motor("y", Vars.yDIR, Vars.ySTEP)
motors[2] = motor("z", Vars.zDIR, Vars.zSTEP)
motors[3] = motor("e", Vars.eDIR, Vars.eSTEP)

try:
	temperature = threading.Thread(name='temp', target=heating.run)
	temperature.setDaemon(True)
	temperature.start()


	import ply.yacc as yacc
	parser = yacc.yacc()

	f = open(sys.argv[1], 'r')
	num = 0
Beispiel #35
0
#!/usr/bin/python
import time
from motor import motor
from RPIO import PWM

PWM.setup()
PWM.init_channel(0)
#where 17 is  GPIO17 = pin 11
# First we specify which gpio pins our motors are on and set our pwm accordingly
mymotor1 = motor('m1', 23, simulation=False)
mymotor2 = motor('m2', 17, simulation=False)
mymotor3 = motor('m3', 24, simulation=False)
mymotor4 = motor('m4', 4, simulation=False)
print('Motors set, press ENTER')
res = raw_input()

# Here we set each motor to 7, most esc's handle pairing by quickly
# increasing and decreasing throttle, this implements that.
mymotor1.start()
mymotor1.setW(7)

mymotor2.start()
mymotor2.setW(7)

mymotor3.start()
mymotor3.setW(7)

mymotor4.start()
mymotor4.setW(7)
#NOTE:the angular motor speed W can vary from 0 (min) to 100 (max)
Beispiel #36
0
 def __init__(self, punchMotorPins, moveMotorPins, lifePins, hitPin):
     self.punchMotor = motor(punchMotorPins)
     self.moveMotor = motor(moveMotorPins)
     self.lives = lives(lifePins)
     self.hitButton = button(hitPin)
    print("MPUD doesn't seem to be started???")
    exit()

pi = pigpio.pi()


# When MPUD is running, reads the values from SHM location
def getMPUVals():
    global shm
    shm.seek(0, 0)
    output = shm.readline()
    a, b, c = output.split()
    return float(a), float(b), float(c)


m1 = motor(pi, 22, 13, 15)
m2 = motor(pi, 24, 18, 16)
m1.rotate(0)
m2.rotate(0)


def botMoveForward(power):
    m1.rotate(power)
    m2.rotate(power)


def botMoveBackward(power):
    # power = power * 1.5
    m1.rotate(power)
    m2.rotate(power)
Beispiel #38
0
#[email protected]
#solenerotech.wordpress.com

#solenerotech 2013.09.06

from motor import motor

mymotor = motor('m1', 17, simulation=False)
#where 17 is  GPIO17 = pin 11

print('***Disconnect ESC power')
print('***then press ENTER')
res = raw_input()
mymotor.start()
mymotor.setW(100)

#NOTE:the angular motor speed W can vary from 0 (min) to 100 (max)
#the scaling to pwm is done inside motor class
print('***Connect ESC Power')
print('***Wait beep-beep')

print('***then press ENTER')
res = raw_input()
mymotor.setW(0)
print('***Wait N beep for battery cell')
print('***Wait beeeeeep for ready')
print('***then press ENTER')
res = raw_74input()
print ('increase > a | decrease > z | save Wh > n | set Wh > h|quit > 9')

cycling = True
from motor import motor
import time
import os

motor1 = motor("m1", 17, simulation=False)
motor2 = motor("m2", 22, simulation=False)
motor3 = motor("m3", 23, simulation=False)
motor4 = motor("m4", 24, simulation=False)
motors = [motor1, motor2, motor3, motor4]
try:
    for m in motors:
        m.start()
        m.setW(100)
    time.sleep(5)
    for m in motors:
        m.start()
        m.setW(0)
    time.sleep(5)
    for m in motors:
        m.start()
        m.setW(10)
    time.sleep(5)
    for m in motors:
        m.start()
        m.setW(0)

except error:
    print "Error"
Beispiel #40
0
 def __init__(self):
   self.left_motor  = motor.motor(7,11)
   self.right_motor = motor.motor(12,13)
   self.actions = {'forward':False, 'reverse':False, 'left':False, 'right':False, 'stop':False}
Beispiel #41
0
	def __init__(self, motor_x_gpio, motor_y_gpio, motor_shoot_gpio):
		self.motor_x = motor(motor_x_gpio)
		self.motor_y = motor(motor_y_gpio)
		self.motor_shoot = motor(motor_shoot_gpio)
deviation = 4.0
error = 0.0
previousError = 0.0
totalError = 0.0

pwmLeft = 0.0
pwmRight = 0.0

maxSpeed = 80

import motor
import irSensor

import PID

motor = motor.motor()
lineSensor = irSensor.irSensor(11, 9, 10, 22, 27)
lineSensor.setSampleTime(0.01)
pid = PID.PID(Kp, Ki, Kd)
pid.setPoint(4.0)
pid.setSampleTime(0.01)

try:
    while input_state:

        input_state = GPIO.input(buttonPin)

        activeSensor = 0.0
        totalSensor = 0

        #Get sensor data
Beispiel #43
0

import sys		# to read in cmd args
import threading	# run concurrent motors
import threader		# really more of a 'parser' -- needs a rename
import time
import returnHome
import heating
 
from printHead import PrintHead
pos = PrintHead()
print("Initial Printer Position ", pos.x, pos.y, pos.z)

from motor import motor
motors = [None]*4
motors[0] = motor("x", Vars.xDIR, Vars.xSTEP, Vars.xMaxFeedrate, Vars.xSteps)
motors[1] = motor("y", Vars.yDIR, Vars.ySTEP, Vars.yMaxFeedrate, Vars.ySteps)
motors[2] = motor("z", Vars.zDIR, Vars.zSTEP, Vars.zMaxFeedrate, Vars.zSteps)
motors[3] = motor("e", Vars.eDIR, Vars.eSTEP, Vars.eMaxFeedrate, Vars.eSteps, 1)

try:
	temperature = threading.Thread(name='temp', target=heating.run)
	temperature.setDaemon(True)
	temperature.start()


	import ply.yacc as yacc
	parser = yacc.yacc()

	f = open(sys.argv[1], 'r')
	num = 0
import copy
import sys
from gripper import open_close_gripper
from motor import motor
from art import art

log = logging.getLogger('Thor_Main')
hdlr = logging.FileHandler('/var/log/thor_control.log')
formatter = logging.Formatter('%(asctime)s %(name)s %(levelname)s %(message)s')
hdlr.setFormatter(formatter)
log.addHandler(hdlr)
log.setLevel(logging.DEBUG)

# Define motors 1-7 from the bottom of the bot to the top.
# The definition is a motor hat, and a hat position
m1 = motor(0x61, 2)
m2 = motor(0x63, 2)
m3 = motor(0x61, 1)
m4 = motor(0x63, 1)
m5 = motor(0x60, 2)
m6 = motor(0x62, 1)
m7 = motor(0x60, 1)

# Define the articulations.  These are a list of lists
# that inculde a motor and a default reverse spec
# Also include a number of degrees per step as a float
art1 = art([[m1, 0]], 1.8)
art2 = art([[m2, 0], [m3, 0]], 0.35)
art3 = art([[m4, 0]], 1.0)
art4 = art([[m5, 0]], 1.0)
art5 = art([[m6, 0], [m7, 0]], 1.0)
Beispiel #45
0
def main():
    from threading import Thread
    _logger = logger()
    def log(string):
        if _logger.q:
            _logger.q.put('%s%s' % (datetime.now().strftime('%X'), string))
    _logger.start()
    setup()
    m = motor()
    sv = SensorVote(new_vote_callback = lambda x:direction(m, x) or log('[NEW-VOTE] %s' % x))
    #t = Tracer.Tracer()
    #sv.add_voter(t)
    l = Light.Light()
    sv.add_voter(l)
    h = HumanOverride.HumanOverride()
    sv.add_voter(h)

    spd = speed()
    spd.start_monitor()

    class hb(Thread):
        def __init__(self):
            Thread.__init__(self)
            self.exit = False

        def run(self):
            while not self.exit:
                try:
                    time.sleep(1.5)
                    res = sv.vote_full()
                    s = spd.get_speed()
                    log('[%s] %s %s' % ('CAUTION' if res[1] else 'HEARTBEAT', res[0], s))
                    direction(m, res[0])
                    if sum(s) == 0 and m.status != motor.STATUS_STOP:
                        log('[OBSTACLE] !!!!!')
                        m.do_reverse()
                except KeyboardInterrupt:
                    return
    m.set_speed(int(raw_input('Speed > ') or '10'))
    b = hb()
    b.setDaemon(True)
    b.start()
    #m.go(start = True)

    while True:
        try:
            inp = raw_input('Override >')
            h.do_override(inp)
            if inp == 'b':
                m.back(start = True)
            elif inp == 'g':
                m.go(start = True)
            elif inp == 's':
                m.stop()
        except KeyboardInterrupt:
            break
    b.exit = True
    _logger.q = None
    b.join()
    _logger.join()
    cleanup(m)
Beispiel #46
0
from sys import path
from matplotlib import use as mpluse
mpluse('Agg')
import matplotlib.pyplot as plt
path.append("./../bin/")

import motor
import resx
from time import sleep
m = motor.motor()
m.start_poll(name="test.csv", controlled=False)
m.update_setpoint(100)
m.set_dc(25)
print "warming up motor"
sleep(2)
m.start_control()

Ku = 4

m.pidc.tuning = (Ku, 0.0, 0.0)
m.pidc.tuning = (1.8, 2.85, 0.0)

errs_ = list()
times = list()

print "control started\ttuning: {}".format(m.pidc.tuning)
m.update_setpoint(100)

print "waiting"
for i in range(0, 1):
    #print "speed  {:.3f}  gd {:.3f}  lav  {} err {}".format(m.speed, resx.get_strain((m.speed * 2 * 3.14) / 60.0), m.ldc, m.pidc.lerr)
Beispiel #47
0
        return math.sqrt((self.spray.x - self.pinA.x)**2/((self.spray.x - self.pinA.x)**2 + (self.spray.y - self.pinA.y)**2))
    def sinB(self):
        if self.pinB.x == self.spray.x:
            return 0
        return math.sqrt((self.spray.x - self.pinB.x)**2/((self.spray.x - self.pinB.x)**2 + (self.spray.y - self.pinB.y)**2))
    def cosA(self):
        return math.sqrt(1 - self.sinA()**2)
    def cosB(self):
        return math.sqrt(1 - self.sinB()**2)
    
    def pinForceA(self):
        if 0 == self.sinB():
            return 0
        return self.spray.weight*self.sinB()/(self.sinB()*self.cosA()+self.cosB()*self.sinA())
    def pinForceB(self):
        if 0 == self.sinA():
            return 0
        return self.spray.weight*self.sinA()/(self.sinB()*self.cosA()+self.cosB()*self.sinA())
        
if __name__ == "__main__":
    m1 = motor(12, 6.38, 32, 1.8)
    m2 = motor(12, 6.38, 32, 1.8)
    sp = spray(20, 0.5)
    
    ps = pinSystem(m1, m2, sp)
    ps.loadPosition(0, 0, 10, 100)
    
    for i in range(10):
        for j in range(10):
            ps.spray.setPosition(j, i)
            print "%.2f,%.2f"%(ps.pinForceA(), ps.pinForceB())
def do(deviceName, action):
    action = "on"
    if deviceName == "up_status":
        mymotor1 = motor('m1', pwm1, simulation=False)
        mymotor1.start()
        mymotor.setW(100)
        mymotor2 = motor('m2', pwm2, simulation=False)
        mymotor2.start()
        mymotor.setW(100)
        mymotor3 = motor('m3', pwm3, simulation=False)
        mymotor3.start()
        mynotor.setW(100)
        mymotor4 = motor('m4', pwm4, simulation=False)
        mymotor4.start()
        mymotor.setW(100)

    if deviceName == "down_status":
        i = 100
        for i in range(100):
            i = i - 10
            mymotor1 = motor('m1', pwm1, simulation=False)
            mymotor1.stop()
            mymotor2.setW(i)
            mymotor2 = motor('m2', pwm2, simulation=False)
            mymotor2.stop()
            mymotor2.setW(i)
            mymotor3 = motor('m3', pwm3, simulation=False)
            mymotor3.stop()
            mymotor3.setW(i)
            mymotor4 = motor('m4', pwm4, simulation=False)
            mymotor4.stop()
            mymotor4.setW(i)
    if deviceName == "left_status":
        mymotor1 = motor('m1', pwm1, simulation=False)
        mymotor1.stop()
        mymotor3 = motor('m3', pwm3, simulation=False)
        mymotor3.stop()
        for i in range(5000):
            j = i + 1
        mymotor1.start()
        mymotor1.setW(100)
        mymotor3.start()
        mymotor3.setW(100)

    if deviceName == "right_status":
        mymotor2 = motor('m1', pwm2, simulation=False)
        mymotor2.stop()
        mymotor4 = motor('m4', pwm4, simulation=False)
        mymotor4.stop()
        for i in range(5000):
            j = i + 1
        mymotor4.start()
        mymotor4.setW(100)
        mymotor3.start()
        mymotor3.setW(100)
    return render_template('index.html')
Beispiel #49
0
    def __init__(self,
                 name,
                 pin0=18,
                 pin1=23,
                 pin2=24,
                 pin3=25,
                 simulation=True):

        #GPIO: 18 23 24 25
        #pin : 12 16 18 22

        self.logger = logging.getLogger('myQ.quadcptr')
        self.name = name
        self.simulation = simulation

        self.version = 1

        self.motor = [motor('M' + str(i), 0) for i in xrange(4)]
        self.motor[0] = motor('M0',
                              pin0,
                              kv=1000,
                              WMin=0,
                              WMax=100,
                              simulation=self.simulation)
        self.motor[1] = motor('M1',
                              pin1,
                              kv=1000,
                              WMin=0,
                              WMax=100,
                              simulation=self.simulation)
        self.motor[2] = motor('M2',
                              pin2,
                              kv=1000,
                              WMin=0,
                              WMax=100,
                              simulation=self.simulation)
        self.motor[3] = motor('M3',
                              pin3,
                              kv=1000,
                              WMin=0,
                              WMax=100,
                              simulation=self.simulation)

        self.sensor = sensor(simulation=self.simulation)

        self.pidR = pid()
        self.pidP = pid()
        self.pidY = pid()
        self.pidR_rate = pid()
        self.pidP_rate = pid()
        self.pidY_rate = pid()

        self.ip = '192.168.1.1'

        self.netscan = netscan(self.ip)

        self.webserver = webserver(self)

        self.display = display(self)

        self.rc = rc(self.display.screen)

        self.imulog = False
        self.savelog = False
        self.calibIMU = False
        self.debuglev = 0
        self.netscanning = False

        #for quadricopter phisics calculations- not used yet
        self.prop = prop(9, 4.7, 1)
        self.voltage = 12  # [V]
        self.mass = 2  # [Kg]
        self.barLenght = 0.23  # [mm]
        self.barMass = 0.15  # [kg]

        self.datalog = ''
        self.initLog()
Beispiel #50
0
#[email protected]
#solenerotech.wordpress.com

#solenerotech 2013.09.06

from motor import motor

mymotor = motor('m1', 17, simulation=False)
#where 17 is  GPIO17 = pin 11

print('***Disconnect ESC power')
print('***then press ENTER')
res = raw_input()
mymotor.start()
mymotor.setW(500)

#NOTE:the angular motor speed W can vary from 0 (min) to 100 (max)
#the scaling to pwm is done inside motor class
print('***Connect ESC Power')
print('***Wait beep-beep')

print('***then press ENTER')
res = raw_input()
mymotor.setW(0)
print('***Wait N beep for battery cell')
print('***Wait beeeeeep for ready')
print('***then press ENTER')
res = raw_input()
print ('increase > a | decrease > z | save Wh > n | set Wh > h|quit > 9')

cycling = True
Beispiel #51
0
#[email protected]
#solenerotech.wordpress.com

#solenerotech 2013.09.06

from motor import motor
import time

mymotor = motor('m1', 22, simulation=False)
#where 17 is  GPIO17 = pin 11

print('***Disconnect ESC power')
print('***then press ENTER')
res = raw_input()
mymotor.start()
mymotor.setW(100)

#NOTE:the angular motor speed W can vary from 0 (min) to 100 (max)
#the scaling to pwm is done inside motor class
print('***Connect ESC Power')
print('***Wait beep-beep')

print('***then press ENTER')
res = raw_input()
mymotor.setW(0)
print('***Wait N beep for battery cell')
print('***Wait beeeeeep for ready')
print('***then press ENTER')
res = raw_input()
print ('increase > a | decrease > z | save Wh > n | set Wh > h|quit > 9 | set yourself > m')