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()
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()
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)
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
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))
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))
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)
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
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) }
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)))
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()
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
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)
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()
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)))
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( )
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()
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)
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()
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
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):
# [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,
#[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')
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
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:
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()
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
#!/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)
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)
#[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"
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}
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
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)
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)
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)
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')
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()
#[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
#[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')
