Пример #1
0
    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)
Пример #2
0
    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)
Пример #3
0
 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!"
Пример #4
0
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)
Пример #5
0
    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)
Пример #6
0
 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!"
Пример #7
0
 def __init__(self):
     self.imu = Imu()
     self.navigation = Navigation()
Пример #8
0
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()
Пример #9
0
    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()
Пример #10
0
	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");
Пример #11
0
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
Пример #12
0
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
Пример #13
0
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()
Пример #14
0
			# 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()
Пример #16
0
    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)