def __init__(self,
xy_lf_pin,
xy_rf_pin,
xy_lb_pin,
xy_rb_pin,
z_lf_pin,
z_rf_pin,
z_lb_pin,
z_rb_pin,
arm_pin,
initialize_motors=True,
ssc_control=True):
with open('p2t.json', 'r') as json_file:
p2t = json.load(json_file)
self.xy_lf = ContinuousRotationServo(xy_lf_pin,
ssc_control=ssc_control,
p2t=p2t)
self.xy_rf = ContinuousRotationServo(xy_rf_pin,
ssc_control=ssc_control,
p2t=p2t)
self.xy_lb = ContinuousRotationServo(xy_lb_pin,
ssc_control=ssc_control,
p2t=p2t)
self.xy_rb = ContinuousRotationServo(xy_rb_pin,
ssc_control=ssc_control,
p2t=p2t)
self.z_lf = ContinuousRotationServo(z_lf_pin,
ssc_control=ssc_control,
p2t=p2t)
self.z_rf = ContinuousRotationServo(z_rf_pin,
ssc_control=ssc_control,
p2t=p2t)
self.z_lb = ContinuousRotationServo(z_lb_pin,
ssc_control=ssc_control,
p2t=p2t)
self.z_rb = ContinuousRotationServo(z_rb_pin,
ssc_control=ssc_control,
p2t=p2t)
self.arm = StandardServo(arm_pin)
self.imu = Imu()
self.z_motors_list = [self.z_lf, self.z_rf, self.z_lb, self.z_rb]
self.xy_motors_list = [self.xy_lf, self.xy_rf, self.xy_lb, self.xy_rb]
self.all_motors_list = self.z_motors_list + self.xy_motors_list
self.arm_status = False
self.open_arm()
# PID
self.old_err = np.zeros((2, 2))
self.imu.start()
if initialize_motors:
self.initialize_motors()
self._initialize_imu()
sleep(2)
def __init__(self,
xy_lf_pin,
xy_rf_pin,
xy_lb_pin,
xy_rb_pin,
z_lf_pin,
z_rf_pin,
z_lb_pin,
z_rb_pin,
arm_pin,
initialize_motors=True,
ssc_control=True):
with open('p2t.json', 'r') as json_file:
p2t = json.load(json_file)
self.ssc_control = ssc_control
self.motors = {
"xy_lf": ContinuousRotationServo(xy_lf_pin, p2t=p2t),
"xy_rf": ContinuousRotationServo(xy_rf_pin, p2t=p2t),
"xy_lb": ContinuousRotationServo(xy_lb_pin, p2t=p2t),
"xy_rb": ContinuousRotationServo(xy_rb_pin, p2t=p2t),
"z_lf": ContinuousRotationServo(z_lf_pin, p2t=p2t),
"z_rf": ContinuousRotationServo(z_rf_pin, p2t=p2t),
"z_lb": ContinuousRotationServo(z_lb_pin, p2t=p2t),
"z_rb": ContinuousRotationServo(z_rb_pin, p2t=p2t)
}
self.motor_prev_powers = {
"xy_lf": 0,
"xy_rf": 0,
"xy_lb": 0,
"xy_rb": 0,
"z_lf": 0,
"z_rf": 0,
"z_lb": 0,
"z_rb": 0
}
self.arm = StandardServo(arm_pin)
self.arm_status = False
self.open_arm()
# PID
self.old_err = np.zeros((2, 2))
self.imu = Imu()
self.imu.start()
if initialize_motors:
self.initialize_motors()
self._initialize_imu()
sleep(2)
def __init__(self, red_led_pin, green_led_pin, left_servo_pin,
right_servo_pin, left_motor_forward, left_motor_backward,
right_motor_forward, right_motor_backward):
# Input
self.imu = Imu()
self.camera = Camera()
# Output
self.red_led = Led(red_led_pin)
self.green_led = Led(green_led_pin)
self.left_servo = Servo(left_servo_pin, min_angle=-90, max_angle=90)
self.right_servo = Servo(right_servo_pin, min_angle=-90, max_angle=90)
self.left_motor = Motor(forward=left_motor_forward,
backward=left_motor_backward)
self.right_servo = Motor(forward=right_motor_forward,
backward=right_motor_backward)
def __init__(self, m1, m2, jib=None, dist=None, verbose=False):
self.m1 = m1
self.m2 = m2
self.imu = Imu()
self.dist = dist
self.jib = jib
self.motorlog = []
# CURRENT STATE
self.state = 'stop'
self.power = 0
self.steerpower = 0
self.minpower = 30
self.minsteerpower = 20
# DEBUG
self.verbose = verbose
print "Hi, I'm Rover... hopefully I won't roll over!"
def __init__(self):
self.imu = Imu()
self.navigation = Navigation()
INFO('process id:', os.getpid())
if __name__ == '__main__':
info('main line')
# 共有メモリの準備
shmem = Value(Point, 0)
shmem.preparingRestart = False
shmem.relyStation = False
shmem.soundPhase = 0
# モータ制御インスタンスの生成
motorController = MotorController()
# 画像処理インスタンスの生成
imageProcessing = ImageProcessing()
# 慣性計測インスタンスの生成
imu = Imu()
# スタート時に一度ジャイロセンサの補正をしておく
imu.calibrate()
# websocketサーバの生成
# server = Server(9001)
sound = Sound()
p_motorContl = Process(target=motorController.target, args=(shmem, ))
p_imageProcessing = Process(target=imageProcessing.target, args=(shmem, ))
p_imu = Process(target=imu.target, args=(shmem, ))
# p_server = Process(target=server.target, args=())
p_sound = Process(target=sound.target, args=(shmem, ))
p_motorContl.start()
DEBUG('p_motorContl started')
p_imageProcessing.start()
def navigate(self):
lastOrientationDegree = 0;
turn_degrees_needed = 0;
turn_degrees_accum = 0;
imu = Imu();
#clean angle;
imu.get_delta_theta();
#Condition distance more than 2 meters.
while distance > 2 and self.stopNavigation != False:
#print("degrees: ", imu.NORTH_YAW);
#print("coords: ", imu.get_gps_coords());
#print("orientation degrees", orientationDegree);
if(lastOrientationDegree != orientationDegree):
turn_degrees_needed = orientationDegree;
turn_degrees_accum = 0;
#clean angle;
imu.get_delta_theta();
lastOrientationDegree = orientationDegree;
#If same direction, keep route
#while math.fabs(turn_degrees_needed) > 10:
imu_angle = imu.get_delta_theta()['z']%360;
if(imu_angle > 180):
imu_angle = imu_angle - 360;
#print("grados imu: ", imu_angle);
#threshold
if(math.fabs(imu_angle) > 1):
turn_degrees_accum += imu_angle;
#print("grados acc: ", turn_degrees_accum);
turn_degrees_needed = (orientationDegree + turn_degrees_accum)%360;
if(turn_degrees_needed > 180):
turn_degrees_needed = turn_degrees_needed - 360;
elif (turn_degrees_needed < -180):
turn_degrees_needed = turn_degrees_needed + 360;
#print("grados a voltear: ", turn_degrees_needed);
if(math.fabs(turn_degrees_needed) < 10):
print("Tengo un margen menor a 10 grados");
velocity = destiny['distance'] * 10;
if (velocity > 300):
velocity = 200;
motors.move(velocity, velocity);
else:
#girar
if(turn_degrees_needed > 0):
print("Going to move left")
motors.move(70, -70);
else:
print("Going to move right")
motors.move(-70, 70);
#ir derecho;
#recorrer 2 metros
destiny = imu.get_degrees_and_distance_to_gps_coords(latitude2, longitud2);
#time.sleep(1);
motors.move(0,0);
print("End thread Navigation");
# mc.set_pwm(mc.esc_three, test_pwm)
mc.set_pwm(mc.esc_four, test_pwm)
inp = raw_input()
if inp == 'stop':
keep_testing = False
elif inp == 'd':
test_pwm -= 50
elif inp == 'u':
test_pwm += 50
else:
pass
"""
self.mc.motors_stop()
if __name__ == '__main__':
# Initialize node
rospy.init_node('picopter')
try:
picopter = Quadcopter(1.1, 0.3556 / 2, 0.28575 / 2, 0.008465,
0.0154223, 0.023367, 1.13e-8, 8.300e-8)
bno = Imu()
mc = MotorController()
qc = QuadController(picopter)
fp = FlightPlanner()
se = StateEstimator(picopter, bno)
picopter_fly = PicopterFly(bno, mc, qc, fp, se, picopter)
except rospy.ROSInterruptException:
pass
#!/usr/bin/env python3
import argparse
from imu import Imu
def handle_imu_data(imu_data):
print("Got data from %f" % imu_data.system_timestamp)
import argparse
parser = argparse.ArgumentParser(description='Read data from Yostlabs IMU')
parser.add_argument('port_name',
help='Device file for serial port (e.g., /dev/ttyUSBS0)')
parser.add_argument('--output', help='Save output to file')
parser.add_argument('--raw-output', help='Save raw output to file')
args = parser.parse_args()
imu = Imu("imu", args.port_name)
imu.add_callback(handle_imu_data)
imu.run()
def navigate(self, destiny, lat, lon):
lastOrientationDegree = 0
turn_degrees_needed = 0
turn_degrees_accum = 0
distance = destiny['distance']
orientationDegree = destiny['degree']
imu = Imu()
#clean angle
imu.get_delta_theta()
#Condition distance more than 2 meters.
while distance > 2 and self.stopNavigation != False:
#print("degrees: ", imu.NORTH_YAW)
#print("coords: ", imu.get_gps_coords())
#print("orientation degrees", orientationDegree)
if lastOrientationDegree != orientationDegree:
turn_degrees_needed = orientationDegree
turn_degrees_accum = 0
#clean angle
imu.get_delta_theta()
lastOrientationDegree = orientationDegree
#If same direction, keep route
#while math.fabs(turn_degrees_needed) > 10:
imu_angle = imu.get_delta_theta()['z'] % 360
if imu_angle > 180:
imu_angle = imu_angle - 360
#print("grados imu: ", imu_angle)
#threshold
if math.fabs(imu_angle) > 1:
turn_degrees_accum += imu_angle
#print("grados acc: ", turn_degrees_accum)
turn_degrees_needed = (orientationDegree +
turn_degrees_accum) % 360
if turn_degrees_needed > 180:
turn_degrees_needed = turn_degrees_needed - 360
elif turn_degrees_needed < -180:
turn_degrees_needed = turn_degrees_needed + 360
#print("grados a voltear: ", turn_degrees_needed)
if math.fabs(turn_degrees_needed) < 10:
print("Tengo un margen menor a 10 grados")
velocity = distance * 10
if velocity > 250:
velocity = 200
motors.move(velocity, velocity)
else:
#girar
if turn_degrees_needed > 0:
print("Going to move left")
motors.move(70, -70)
else:
print("Going to move right")
motors.move(-70, 70)
#ir derecho
#recorrer 2 metros
destiny = imu.get_degrees_and_distance_to_gps_coords(lat, lon)
#time.sleep(1)
motors.move(0, 0)
