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, 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!"
class GPSNavigation:
def __init__(self):
self.imu = Imu()
self.navigation = Navigation()
def update_nav(self, lat, lon):
destiny = self.imu.get_degrees_and_distance_to_gps_coords(lat, lon)
self.navigation.navigate(destiny, lat, lon)
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()
class RovMovement:
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 select_motors(self, search_list):
return {
key: value
for (key, value) in self.motors.items()
if not any(s not in key for s in search_list)
}
def initialize_motors(self, mp=30):
print("All motors initializing...")
for cp in list(range(0, mp)) + list(range(mp, -mp, -1)) + list(
range(-mp, 1)):
print("Power:", cp)
for motor in self.motors.values():
motor.run_bidirectional(cp)
sleep(0.01)
print("All motors initialized...")
def _initialize_imu(self, seconds=5):
print("IMU is being calibrated...")
self.imu.calibrate(seconds)
print("IMU is calibrated...")
def _get_z_balance_p(self, kp=0.0, kd=0.0, type_=int, tilt_degree=0):
# if not kp > kd:
# raise Exception("Kp must be bigger than Kd. Kp: %s - Kd: %s" % (kp, kd))
try:
x, y, _ = self.imu.get_degree().get()
# if abs(tilt_degree) == 90:
# if 45 < y < 90:
# tilt_degree = 90
# elif -90 < y < -45:
# tilt_degree = -90
y = y - tilt_degree
comp_sign = +1 if (x < 0 and y < 0) or (x > 0 and y > 0) else -1
if -1 < x < 1: x = 0
if -1 < y < 1: y = 0
lf = sign_n(-y - x) * math.sqrt(
abs(-y * -y + comp_sign * -x * -x)) # -y - x
rf = sign_n(-y + x) * math.sqrt(
abs(-y * -y - comp_sign * +x * +x)) # -y + x
lb = sign_n(+y - x) * math.sqrt(
abs(+y * +y - comp_sign * -x * -x)) # +y - x
rb = sign_n(+y + x) * math.sqrt(
abs(+y * +y + comp_sign * +x * +x)) # +y + x
err = np.array([[lf, rf], [lb, rb]])
ret = kp * err + kd * (err - self.old_err)
self.old_err = err
if type_ == int:
ret = np.rint(ret)
return ret.ravel()
except Exception as e:
print("Exception in _get_balance_p:", e)
return 0, 0, 0, 0
def _gradual_power_change(self,
current_powers,
ssc_control=True,
ssc_step=5,
ssc_sleep=0.01):
start_powers = self.motor_prev_powers
stop_powers = current_powers
motor_keys = stop_powers.keys()
diff = [
abs(stop_powers[key] - start_powers[key]) for key in motor_keys
]
steps = int(max(diff) / ssc_step)
power_list = {
key: np.linspace(start_powers[key],
stop_powers[key],
steps,
endpoint=False,
dtype=int)
for key in motor_keys
}
if steps and ssc_control:
for k in range(1, steps):
for key in motor_keys:
self.motors[key].run_bidirectional(int(power_list[key][k]))
print(str(key) + ":", power_list[key][k], end="\t")
print()
sleep(ssc_sleep)
for key in motor_keys:
self.motors[key].run_bidirectional(int(stop_powers[key]))
print(str(key) + ":", stop_powers[key], end="\t")
print()
def go_z_bidirectional(self, power, with_balance=True, tilt_degree=0):
power_per_motor = int(power / 4)
lf_p, rf_p, lb_p, rb_p = self._get_z_balance_p(
kp=0.35, kd=0.30, tilt_degree=tilt_degree) if with_balance else (0,
0,
0,
0)
current_powers = {
"z_lf": power_per_motor + lf_p,
"z_rf": power_per_motor + rf_p,
"z_lb": power_per_motor + lb_p,
"z_rb": power_per_motor + rb_p
}
self._gradual_power_change(current_powers,
ssc_control=self.ssc_control)
self.motor_prev_powers.update(current_powers)
def go_xy_and_turn(self, power, degree, turn_power):
"""
:param power: Power sent to the vehicle's movement
:param degree: degree of movement (0between 0-360 degree)
0 -> ileri
90 -> sağa
180 -> geri
270 -> sola
:param turn_power: Turn power
Positive value -> Turn right
Negative value -> Turn left
:return:
"""
# if turn_power:
# turn_power = turn_power / 4
# turn_power_per_motor = int(turn_power / 4)
# else:
# _, _, z = self.imu.get_instant_gyro().get()
# turn_power_per_motor = int(z)
turn_power = turn_power / 4
turn_power_per_motor = int(turn_power / 4)
go_power_per_motor = int(power / 2)
radian_rf = (45 - degree) / 180 * math.pi
radian_lf = (135 - degree) / 180 * math.pi
radian_lb = (225 - degree) / 180 * math.pi
radian_rb = (315 - degree) / 180 * math.pi
pow_rf = int(
math.sin(radian_rf) * go_power_per_motor - turn_power_per_motor)
pow_lf = int(
math.sin(radian_lf) * go_power_per_motor + turn_power_per_motor)
pow_lb = int(
math.sin(radian_lb) * go_power_per_motor - turn_power_per_motor)
pow_rb = int(
math.sin(radian_rb) * go_power_per_motor + turn_power_per_motor)
current_powers = {
"xy_lf": pow_lf,
"xy_rf": pow_rf,
"xy_lb": pow_lb,
"xy_rb": pow_rb
}
self._gradual_power_change(current_powers)
self.motor_prev_powers.update(current_powers)
def go_xyz_with_tilt(self,
xy_power,
z_power,
turn_power,
with_balance=True,
tilt_degree=0):
"""
:param xy_power: between from -70 to 70
:param z_power: between from -70 to 70
:param turn_power: Turn power
Positive value -> Turn right
Negative value -> Turn left
:param with_balance: Should the balance of the vehicle be achieved with pid control?
:param tilt_degree: the inclination the vehicle will make forward
negative value -> leaning forward of the vehicle
positive value -> raising the front of the vehicle
:return:
"""
tilt_radian = tilt_degree * math.pi / 180
xy_power_ = math.cos(-tilt_radian) * xy_power - math.sin(
-tilt_radian) * z_power
z_power_ = math.sin(-tilt_radian) * xy_power + math.cos(
-tilt_radian) * z_power
self.go_xy_and_turn(xy_power_, 0, turn_power)
self.go_z_bidirectional(z_power_,
with_balance=with_balance,
tilt_degree=tilt_degree)
def open_arm(self):
self.arm.change_angle(100)
self.arm_status = True
def close_arm(self):
self.arm.change_angle(30)
self.arm_status = False
def toggle_arm(self, arm_status=None):
if self.arm_status and (arm_status is None or arm_status == False):
self.close_arm()
elif not self.arm_status and (arm_status is None
or arm_status == True):
self.open_arm()
def run_all_motors_cw(self, power):
for motor in self.motors.values():
motor.run_clockwise(power)
def run_all_motors_ccw(self, power):
for motor in self.motors.values():
motor.run_counterclockwise(power)
def stop(self):
print("RovMovement is terminating...")
for motor in self.motors.values():
motor.stop()
self.arm.stop()
self.imu.stop()
print("RovMovement has been terminated.")
def close(self):
self.stop()
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");
class Rover():
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 keycontrol(self):
mapping = {
# KEY BINDINGS: key to function (with default value)
# ROVER KEYS
'w': 'fw',
's': 'rw',
'a': 'left',
'd': 'right',
' ': 'stop',
'r': 'init_log',
't': 'save_log',
# JIB KEYS
'i': 'jib.moveup',
'j': 'jib.moveleft',
'k': 'jib.movedw',
'l': 'jib.moveright',
# MEASURE DISTANCE
'm': 'print_distance'
}
print "ROVER KEYPAD: initialized"
keypad.keypad(self, mapping)
print "ROVER KEYPAD: terminated"
# DIRECTION
def stop(self):
self.m1.stop()
self.m2.stop()
self.state = 'stop'
self.power = 0
self.motorlog.append([time(), 'stop', 0, 0])
if self.verbose: print "stop | m1: %i | m2: %i" % (0, 0)
def fw(self, seconds=None):
if m1.use_pwm and m2.use_pwm:
if self.state == 'rw': self.power = 0
self.power = min(100, max(self.minpower, self.power + 10))
self.steerpower = 0
self._fw(seconds, power=self.power)
else:
self._fw(seconds)
def rw(self, seconds=None):
if m1.use_pwm and m2.use_pwm:
if self.state == 'fw': self.power = 0
self.power = min(100, max(self.minpower, self.power + 10))
self.steerpower = 0
self._rw(seconds, power=self.power)
else:
self._rw(seconds)
def left(self, seconds=None):
if m1.use_pwm and m2.use_pwm:
self.steerpower = min(
100, max(self.minsteerpower, self.steerpower + 10))
self._left(seconds, steerpower=self.steerpower)
else:
self._left(seconds)
def right(self, seconds=None):
if m1.use_pwm and m2.use_pwm:
self.steerpower = min(
100, max(self.minsteerpower, self.steerpower + 10))
self._right(seconds, steerpower=self.steerpower)
else:
self._right(seconds)
def _fw(self, seconds=None, power=100):
self.m1.fw(power=power)
self.m2.fw(power=power)
self.state = 'fw'
self.motorlog.append([time(), 'fw', power, power])
if self.verbose:
print "fw: %i%% | m1.fw: %i | m2.fw: %i" % (power, power, power)
if seconds is not None:
sleep(seconds)
self.stop()
def _rw(self, seconds=None, power=100):
self.m1.rw(power=power)
self.m2.rw(power=power)
self.state = 'rw'
self.motorlog.append([time(), 'rw', power, power])
if self.verbose:
print "rw: %i%% | m1.rw: %i | m2.rw %i" % (power, power, power)
if seconds is not None:
sleep(seconds)
self.stop()
def _left(self, seconds=None, steerpower=100):
if m1.use_pwm and m2.use_pwm:
# if the rover is not moving, rotating power is forced to 100
if self.state == 'stop':
steerpower = 100
self.power = 100
self.state = 'fw'
m2power = self.power
if steerpower >= 50:
# STRONG TURN (inverted direction on motors)
m1power = ((steerpower - 50) * 2 / 100.) * self.power
m1power = max(m1power,
self.minpower) # fixing low velocity turns
self.m1.rw(
power=m1power) if self.state == 'fw' else self.m1.fw(
power=m1power)
m1dir = 'rw' if self.state == 'fw' else 'fw'
else:
# LIGHT TURN (same direction, but slower motors)
m1power = ((50 - steerpower) * 2 / 100.) * self.power
m2power, m1power = max(m2power, 80), max(
m1power, 80) # fixing low velocity turns
self.m1.fw(
power=m1power) if self.state == 'fw' else self.m1.rw(
power=m1power)
m1dir = 'fw' if self.state == 'fw' else 'rw'
# adjust m2 power to be at least twice the minimum combined power
m2power += max(0, self.minpower * 2 - (m1power + m2power))
self.m2.rw(power=m2power) if self.state == 'rw' else self.m2.fw(
power=m2power)
m2dir = 'rw' if self.state == 'rw' else 'fw'
if self.verbose:
print "left: %i%% | m1.%s: %i | m2.%s %i" % (
steerpower, m1dir, m1power, m2dir, m2power)
else:
self.m1.rw(power=self.power)
self.m2.fw(power=self.power)
m1power, m2power = self.power, self.power
# log motor
self.motorlog.append([time(), 'left', m1power, m2power])
if seconds is not None:
sleep(seconds)
self.stop()
def _right(self, seconds=None, steerpower=100):
if m1.use_pwm and m2.use_pwm:
# if the rover is not moving, rotating power is forced to 100
if self.state == 'stop':
steerpower = 100
self.power = 100
self.state = 'fw'
m1power = self.power
if steerpower >= 50:
# STRONG TURN (inverted direction on motors)
m2power = ((steerpower - 50) * 2 / 100.) * self.power
m2power, m1power = max(m2power, 80), max(
m1power, 80) # fixing low velocity turns
self.m2.fw(
power=m2power) if self.state == 'rw' else self.m2.rw(
power=m2power)
m2dir = 'fw' if self.state == 'rw' else 'rw'
else:
# LIGHT TURN (same direction, but slower motors)
m2power = ((50 - steerpower) * 2 / 100.) * self.power
self.m2.fw(
power=m2power) if self.state == 'fw' else self.m2.rw(
power=m2power)
m2dir = 'fw' if self.state == 'fw' else 'rw'
# adjust m2 power to be at least twice the minimum combined power
m1power += max(0, self.minpower * 2 - (m2power + m1power))
self.m1.fw(power=m1power) if self.state == 'fw' else self.m1.rw(
power=m1power)
m1dir = 'fw' if self.state == 'fw' else 'rw'
if self.verbose:
print "right: %i%% | m1.%s: %i | m2.%s %i" % (
steerpower, m1dir, m1power, m2dir, m2power)
else:
self.m1.fw(power=self.power)
self.m2.rw(power=self.power)
m1power, m2power = self.power, self.power
# log motor
self.motorlog.append([time(), 'right', m1power, m2power])
if seconds is not None:
sleep(seconds)
self.stop()
def distance(self):
if self.dist is not None:
return self.dist.measure()
else:
return None
def print_distance(self):
dist = self.distance()[1]
if dist is None: print "Distance Measurement not initialized"
else: print "Distance: %.2fcm" % dist
# LOGGING
def init_log(self, seconds=3600, interval=0.010):
self.motorlog = []
self.imu.samples = [] # reset motor logger
self.stop() # stop car
self.imu.sampler(seconds, interval) # start reading accelerometer
def save_log(self, savepath=None):
self.stop()
acclog = self.imu.get_samples()
import pandas as pd
acclog = pd.DataFrame(acclog,
columns=[
'time', 'pitch', 'roll', 'head', 'ax', 'ay',
'az', 'mx', 'my', 'mz', 'gx', 'gy', 'gz'
]).set_index('time')
carlog = pd.DataFrame(self.motorlog,
columns=['time', 'action', 'm1',
'm2']).set_index('time')
carlog['seq'] = range(len(carlog))
data = pd.merge(acclog,
carlog,
how='outer',
left_index=True,
right_index=True)
# reset data with star date
data.index = data.index.values - data.index.values.min()
if savepath is not None:
if savepath == True: savepath = ''
data.to_csv(datetime.now().strftime(savepath +
'carlog_%Y%m%d_%H%M%S.csv'))
return data
class Rover():
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 keycontrol(self):
mapping = {
# KEY BINDINGS: key to function (with default value)
# ROVER KEYS
'w': 'fw', 's': 'rw', 'a': 'left', 'd': 'right',
' ': 'stop', 'r': 'init_log', 't': 'save_log',
# JIB KEYS
'i': 'jib.moveup', 'j': 'jib.moveleft',
'k': 'jib.movedw', 'l': 'jib.moveright',
# MEASURE DISTANCE
'm': 'print_distance'
}
print "ROVER KEYPAD: initialized"
keypad.keypad(self, mapping)
print "ROVER KEYPAD: terminated"
# DIRECTION
def stop(self):
self.m1.stop(); self.m2.stop(); self.state = 'stop'; self.power = 0
self.motorlog.append([time(),'stop',0,0])
if self.verbose: print "stop | m1: %i | m2: %i" % (0,0)
def fw(self, seconds=None):
if m1.use_pwm and m2.use_pwm:
if self.state == 'rw': self.power = 0
self.power = min(100, max(self.minpower, self.power + 10)); self.steerpower = 0
self._fw(seconds, power=self.power)
else: self._fw(seconds)
def rw(self, seconds=None):
if m1.use_pwm and m2.use_pwm:
if self.state == 'fw': self.power = 0
self.power = min(100, max(self.minpower, self.power + 10)); self.steerpower = 0
self._rw(seconds, power=self.power)
else: self._rw(seconds)
def left(self, seconds=None):
if m1.use_pwm and m2.use_pwm:
self.steerpower = min(100, max(self.minsteerpower, self.steerpower + 10))
self._left(seconds, steerpower=self.steerpower)
else: self._left(seconds)
def right(self, seconds=None):
if m1.use_pwm and m2.use_pwm:
self.steerpower = min(100, max(self.minsteerpower, self.steerpower + 10))
self._right(seconds, steerpower=self.steerpower)
else: self._right(seconds)
def _fw(self, seconds=None, power=100):
self.m1.fw(power=power); self.m2.fw(power=power); self.state = 'fw'
self.motorlog.append([time(),'fw',power,power])
if self.verbose: print "fw: %i%% | m1.fw: %i | m2.fw: %i" % (power,power,power)
if seconds is not None:
sleep(seconds); self.stop()
def _rw(self, seconds=None, power=100):
self.m1.rw(power=power); self.m2.rw(power=power); self.state = 'rw'
self.motorlog.append([time(),'rw',power,power])
if self.verbose: print "rw: %i%% | m1.rw: %i | m2.rw %i" % (power,power,power)
if seconds is not None:
sleep(seconds); self.stop()
def _left(self, seconds=None, steerpower=100):
if m1.use_pwm and m2.use_pwm:
# if the rover is not moving, rotating power is forced to 100
if self.state == 'stop': steerpower = 100; self.power = 100; self.state ='fw'
m2power = self.power
if steerpower >=50:
# STRONG TURN (inverted direction on motors)
m1power = ((steerpower-50)*2/100.)*self.power
m1power = max(m1power, self.minpower) # fixing low velocity turns
self.m1.rw(power=m1power) if self.state == 'fw' else self.m1.fw(power=m1power)
m1dir = 'rw' if self.state == 'fw' else 'fw'
else:
# LIGHT TURN (same direction, but slower motors)
m1power = ((50-steerpower)*2/100.)*self.power
m2power, m1power = max(m2power, 80), max(m1power, 80) # fixing low velocity turns
self.m1.fw(power=m1power) if self.state == 'fw' else self.m1.rw(power=m1power)
m1dir = 'fw' if self.state == 'fw' else 'rw'
# adjust m2 power to be at least twice the minimum combined power
m2power += max(0, self.minpower*2-(m1power+m2power))
self.m2.rw(power=m2power) if self.state == 'rw' else self.m2.fw(power=m2power)
m2dir = 'rw' if self.state == 'rw' else 'fw'
if self.verbose: print "left: %i%% | m1.%s: %i | m2.%s %i" % (steerpower,m1dir,m1power,m2dir,m2power)
else:
self.m1.rw(power=self.power); self.m2.fw(power=self.power)
m1power, m2power = self.power, self.power
# log motor
self.motorlog.append([time(),'left',m1power,m2power])
if seconds is not None:
sleep(seconds); self.stop()
def _right(self, seconds=None, steerpower=100):
if m1.use_pwm and m2.use_pwm:
# if the rover is not moving, rotating power is forced to 100
if self.state == 'stop': steerpower = 100; self.power = 100; self.state ='fw'
m1power = self.power
if steerpower >=50:
# STRONG TURN (inverted direction on motors)
m2power = ((steerpower-50)*2/100.)*self.power
m2power, m1power = max(m2power, 80), max(m1power, 80) # fixing low velocity turns
self.m2.fw(power=m2power) if self.state == 'rw' else self.m2.rw(power=m2power)
m2dir = 'fw' if self.state == 'rw' else 'rw'
else:
# LIGHT TURN (same direction, but slower motors)
m2power = ((50-steerpower)*2/100.)*self.power
self.m2.fw(power=m2power) if self.state == 'fw' else self.m2.rw(power=m2power)
m2dir = 'fw' if self.state == 'fw' else 'rw'
# adjust m2 power to be at least twice the minimum combined power
m1power += max(0, self.minpower*2-(m2power+m1power))
self.m1.fw(power=m1power) if self.state == 'fw' else self.m1.rw(power=m1power)
m1dir = 'fw' if self.state == 'fw' else 'rw'
if self.verbose: print "right: %i%% | m1.%s: %i | m2.%s %i" % (steerpower,m1dir,m1power,m2dir,m2power)
else:
self.m1.fw(power=self.power); self.m2.rw(power=self.power)
m1power, m2power = self.power, self.power
# log motor
self.motorlog.append([time(),'right',m1power,m2power])
if seconds is not None:
sleep(seconds); self.stop()
def distance(self):
if self.dist is not None:
return self.dist.measure()
else: return None
def print_distance(self):
dist = self.distance()[1]
if dist is None: print "Distance Measurement not initialized"
else: print "Distance: %.2fcm" % dist
# LOGGING
def init_log(self, seconds=3600, interval=0.010):
self.motorlog = []; self.imu.samples = [] # reset motor logger
self.stop() # stop car
self.imu.sampler(seconds, interval) # start reading accelerometer
def save_log(self, savepath=None):
self.stop()
acclog = self.imu.get_samples()
import pandas as pd
acclog = pd.DataFrame(acclog, columns=['time','pitch','roll','head','ax','ay','az','mx','my','mz','gx','gy','gz']).set_index('time')
carlog = pd.DataFrame(self.motorlog, columns=['time','action','m1','m2']).set_index('time')
carlog['seq'] = range(len(carlog))
data = pd.merge(acclog, carlog, how='outer', left_index=True, right_index=True)
# reset data with star date
data.index = data.index.values - data.index.values.min()
if savepath is not None:
if savepath == True: savepath = ''
data.to_csv(datetime.now().strftime(savepath+'carlog_%Y%m%d_%H%M%S.csv'))
return data
class RovMovement:
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 initialize_motors(self, mp=30):
print("All motors initializing...")
for cp in list(range(0, mp)) + list(range(mp, -mp, -1)) + list(
range(-mp, 1)):
print("Power:", cp)
for motor in self.all_motors_list:
motor.run_bidirectional(cp)
sleep(0.01)
print("All motors initialized...")
def _initialize_imu(self, seconds=5):
print("IMU is being calibrated...")
self.imu.calibrate(seconds)
print("IMU is calibrated...")
def _get_z_balance_p(self, kp=0.0, kd=0.0, type_=int):
# if not kp > kd:
# raise Exception("Kp must be bigger than Kd. Kp: %s - Kd: %s" % (kp, kd))
try:
x, y, _ = self.imu.get_degree().get()
comp_sign = +1 if (x < 0 and y < 0) or (x > 0 and y > 0) else -1
if -1 < x < 1: x = 0
if -1 < y < 1: y = 0
lf = sign_n(-y - x) * math.sqrt(
abs(-y * -y + comp_sign * -x * -x)) # -y - x
rf = sign_n(-y + x) * math.sqrt(
abs(-y * -y - comp_sign * +x * +x)) # -y + x
lb = sign_n(+y - x) * math.sqrt(
abs(+y * +y - comp_sign * -x * -x)) # +y - x
rb = sign_n(+y + x) * math.sqrt(
abs(+y * +y + comp_sign * +x * +x)) # +y + x
err = np.array([[lf, rf], [lb, rb]])
ret = kp * err + kd * (err - self.old_err)
self.old_err = err
if type_ == int:
ret = np.rint(ret)
return ret.ravel()
except Exception as e:
print("Exception in _get_balance_p:", e)
return 0, 0, 0, 0
def go_z_bidirectional(self, power, with_balance=True):
power_per_motor = int(power / 4)
lf_p, rf_p, lb_p, rb_p = self._get_z_balance_p(
kp=0.3, kd=0.15) if with_balance else (0, 0, 0, 0)
self.z_lf.run_bidirectional(power_per_motor + int(lf_p))
self.z_rf.run_bidirectional(power_per_motor + int(rf_p))
self.z_lb.run_bidirectional(power_per_motor + int(lb_p))
self.z_rb.run_bidirectional(power_per_motor + int(rb_p))
def go_up(self, power):
power_per_motor = int(power / 4)
for motor in self.z_motors_list:
motor.run_clockwise(power_per_motor)
def go_down(self, power):
power_per_motor = int(power / 4)
for motor in self.z_motors_list:
motor.run_counterclockwise(power_per_motor)
def turn_left(self, power):
power = power / 4
power_per_motor = int(power / 4)
self.xy_rf.run_clockwise(power_per_motor)
self.xy_lf.run_counterclockwise(power_per_motor)
self.xy_lb.run_clockwise(power_per_motor)
self.xy_rb.run_counterclockwise(power_per_motor)
def turn_right(self, power):
power = power / 4
power_per_motor = int(power / 4)
self.xy_rf.run_counterclockwise(power_per_motor)
self.xy_lf.run_clockwise(power_per_motor)
self.xy_lb.run_counterclockwise(power_per_motor)
self.xy_rb.run_clockwise(power_per_motor)
def go_xy(self, power, degree):
"""
:param power: Power sent to the vehicle's movement
:param degree: degree of movement (0between 0-360 degree)
0 -> ileri
90 -> sağa
180 -> geri
270 -> sola
:return:
"""
power_per_motor = int(power / 2)
radian_rf = (45 - degree) / 180 * math.pi
radian_lf = (135 - degree) / 180 * math.pi
radian_lb = (225 - degree) / 180 * math.pi
radian_rb = (315 - degree) / 180 * math.pi
pow_rf = int(math.sin(radian_rf) * power_per_motor)
pow_lf = int(math.sin(radian_lf) * power_per_motor)
pow_lb = int(math.sin(radian_lb) * power_per_motor)
pow_rb = int(math.sin(radian_rb) * power_per_motor)
self.xy_rf.run_bidirectional(pow_rf)
self.xy_lf.run_bidirectional(pow_lf)
self.xy_lb.run_bidirectional(pow_lb)
self.xy_rb.run_bidirectional(pow_rb)
def go_xy_and_turn(self, power, degree, turn_power):
"""
:param power: Power sent to the vehicle's movement
:param degree: degree of movement (0between 0-360 degree)
0 -> ileri
90 -> sağa
180 -> geri
270 -> sola
:param turn_power: Turn power
Positive value -> Turn right
Negative value -> Turn left
:return:
"""
turn_power = turn_power / 4
turn_power_per_motor = int(turn_power / 4)
go_power_per_motor = int(power / 2)
radian_rf = (45 - degree) / 180 * math.pi
radian_lf = (135 - degree) / 180 * math.pi
radian_lb = (225 - degree) / 180 * math.pi
radian_rb = (315 - degree) / 180 * math.pi
pow_rf = int(
math.sin(radian_rf) * go_power_per_motor - turn_power_per_motor)
pow_lf = int(
math.sin(radian_lf) * go_power_per_motor + turn_power_per_motor)
pow_lb = int(
math.sin(radian_lb) * go_power_per_motor - turn_power_per_motor)
pow_rb = int(
math.sin(radian_rb) * go_power_per_motor + turn_power_per_motor)
self.xy_rf.run_bidirectional(pow_rf)
self.xy_lf.run_bidirectional(pow_lf)
self.xy_lb.run_bidirectional(pow_lb)
self.xy_rb.run_bidirectional(pow_rb)
def open_arm(self):
self.arm.change_angle(100)
self.arm_status = True
def close_arm(self):
self.arm.change_angle(30)
self.arm_status = False
def toggle_arm(self, arm_status=None):
if self.arm_status and (arm_status is None or arm_status == False):
self.close_arm()
elif not self.arm_status and (arm_status is None
or arm_status == True):
self.open_arm()
def run_all_motors_cw(self, power):
for motor in self.all_motors_list:
motor.run_clockwise(power)
def run_all_motors_ccw(self, power):
for motor in self.all_motors_list:
motor.run_counterclockwise(power)
def stop(self):
print("RovMovement is terminating...")
for motor in self.all_motors_list:
motor.stop()
self.arm.stop()
self.imu.stop()
print("RovMovement has been terminated.")
def close(self):
self.stop()
# 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)
