Пример #1
0
 def __init__(self, max, servo_to_hinge, rate):
     self.max = max  # max min limit of the FINAL control surface deflection in degrees (e.g. 30 degrees of aileron deflection is +-15 deg)
     self.in_to_servo = 1000 / 175  # ratio of input channel to the corresponding servo deflection in degrees (in our case 1000 equals ~170-175 deg)
     self.servo_to_hinge = servo_to_hinge  # ratio of the servo deflection in degrees to the corresponding control surface deflection, depends on the mechanical arrangement
     self.rate = rate  # target deflection speed of the control surface in deg / s
     self.output = 0  # set to zero to initialise
     self.antiAliasing = LowPassFilter(
         1,
         0.05)  # anti-wonkifying smoothing of the input signal to the servo
Пример #2
0
def test_apply_noise_filter():
    """ Tests 4.3 Filter library
        Tests 4.3.2 Noise filtering
    """
    data = DataCapture()

    low_noise_filter = LowPassFilter("Chebyshev", 100)
    high_noise_filter = HighPassFilter("Chebyshev", 5000)

    low_noise = []  # random <100HZ noise
    high_noise = []  # random >5000HZ noise

    old_data = data.dataCapture()
    low_filtered_data = data.applyFilter(low_noise_filter, 2)
    high_filtered_data = data.applyFilter(high_noise_filter, 2)

    errors = []

    #TODO
    # replace == with a in comparison to check if the noise is in the data
    if low_noise == low_filtered_data:
        errors.append("Low noise detected in filtered data")
    if high_noise == high_filtered_data:
        errors.append("High noise detected in filtered data")

    # NOTE: currently returns PASSED as the two data types are both "NONE" and python considers them equal
    assert not errors, "Assert errors occured:\n{}".format("\n".join(errors))
Пример #3
0
class Actuator:
    # class for the control surface actuator
    def __init__(self, max, servo_to_hinge, rate):
        self.max = max  # max min limit of the FINAL control surface deflection in degrees (e.g. 30 degrees of aileron deflection is +-15 deg)
        self.in_to_servo = 1000 / 175  # ratio of input channel to the corresponding servo deflection in degrees (in our case 1000 equals ~170-175 deg)
        self.servo_to_hinge = servo_to_hinge  # ratio of the servo deflection in degrees to the corresponding control surface deflection, depends on the mechanical arrangement
        self.rate = rate  # target deflection speed of the control surface in deg / s
        self.output = 0  # set to zero to initialise
        self.antiAliasing = LowPassFilter(
            1,
            0.05)  # anti-wonkifying smoothing of the input signal to the servo

    def step(
        self, stick_input, sas_input, dt
    ):  #stick_input is in 0 to 1000 whereas sas_input is stability augmentation input in degrees corresponding to the surface deflection
        max_1 = self.max * self.servo_to_hinge * self.in_to_servo
        stick_input_1 = mapInput(stick_input, 0, 1000, 0, max_1)

        rate_1 = self.rate * self.servo_to_hinge * self.in_to_servo

        sas_to_in = sas_input * self.servo_to_hinge * self.in_to_servo

        input_1 = self.antiAliasing.step(
            limit(stick_input_1 + sas_to_in, max_1, 0), dt)

        self.output = limitByRate(input_1, self.output, rate_1, dt)
        return self.output
Пример #4
0
def test_apply_filter():
    """Tests 4.3.1 Filtering method
    """
    data = DataCapture()

    dfilter = LowPassFilter("Chebyshev", 1000)

    old_data = data.dataCapture()
    filtered_data = data.applyFilter(dfilter, 2)

    assert old_data != filtered_data, "No changes to filtered data"
Пример #5
0
    def __init__(self, kp=0, ki=0, kd=0, kff=None, kfa=None, derivative_corner=10):
        """
        kp = proportional gain term
        ki = integral gain term
        kd = derivative gain term
        kff= feed forward velocity term.  Applies signal relative to dervative of the target
        derivative_corner = corner frequency of derivative filter.
        Our real world signals are too noisy to use without significant filtering
        kfa= feed forward acceleration term.  Applies signal relative to the difference in
        rate of change of the target and desired.
        """
        self.updateGainConstants(kp, ki, kd, kff, kfa)
        
        self.max_movement_rate = 1e6

        # NOTE: it is important that these three variables are floating point to avoid truncation
        self.prev_error = 0.0
        self.prev_desired_pos = 0.0
        self.integral_error_accumulator = 0.0
        
        self.peak_detector = HystereticPeakDetector(0.0, -1.0, 1.0, math.pi / 20)

        self.d_lowpass = LowPassFilter(gain=1.0, corner_frequency=derivative_corner)
Пример #6
0
def test_check_avail_filters():
    """ Tests 4.3 Filter library
        Tests 4.3.3 Data filtering
    """
    data = DataCapture()

    lpf = LowPassFilter("Chebyshev", 250)
    hpf = HighPassFilter("Bessel", 500)
    maf = MovingAverageFilter(10)
    pkf = PeakFilter(10, 10)

    errors = []

    # replace == with a check if noise data is contained in filtered data
    if not lpf.getFilterInfo():
        errors.append("Low pass filter does not exist")
    if not hpf.getFilterInfo():
        errors.append("High pass filter does not exist")
    if not maf.getFilterInfo():
        errors.append("Moving average filter does not exist")
    if not pkf.getFilterInfo():
        errors.append("Peak filter does not exist")

    assert not errors, "Assert errors occured:\n{}".format("\n".join(errors))
Пример #7
0
from AltIMU_v3 import AltIMUv3
from filters import LowPassFilter, HighPassFilter
import time
import RPi.GPIO as GPIO
import math
GPIO.setmode(GPIO.BOARD)
GPIO.setup(7, GPIO.OUT)
GPIO.setup(10, GPIO.OUT)
# Setup Altimu
altimu = AltIMUv3()
altimu.enable()

# Initialize a low pass filter with a default value and a bias of 80%
low_pass_filter = LowPassFilter([0.0, 0.0, 1.0], 0.8)

while True:
    accel = altimu.get_accelerometer_cal()
    gyro = altimu.get_gyro_cal()
    time.sleep(0.1)
    if accel[2] < -1:
        if gyro[2] > 30 or gyro[2] < -30:
            GPIO.output(7, True)
    if accel[2] > 0.95:
        GPIO.output(7, True)
    if accel[1] > .7:
        GPIO.output(7, True)

    else:
        GPIO.output(7, False)

    if resultado > .98:
Пример #8
0
class PIDController:
    def __init__(self, kp=0, ki=0, kd=0, kff=None, kfa=None, derivative_corner=10):
        """
        kp = proportional gain term
        ki = integral gain term
        kd = derivative gain term
        kff= feed forward velocity term.  Applies signal relative to dervative of the target
        derivative_corner = corner frequency of derivative filter.
        Our real world signals are too noisy to use without significant filtering
        kfa= feed forward acceleration term.  Applies signal relative to the difference in
        rate of change of the target and desired.
        """
        self.updateGainConstants(kp, ki, kd, kff, kfa)
        
        self.max_movement_rate = 1e6

        # NOTE: it is important that these three variables are floating point to avoid truncation
        self.prev_error = 0.0
        self.prev_desired_pos = 0.0
        self.integral_error_accumulator = 0.0
        
        self.peak_detector = HystereticPeakDetector(0.0, -1.0, 1.0, math.pi / 20)

        self.d_lowpass = LowPassFilter(gain=1.0, corner_frequency=derivative_corner)

    def updateGainConstants(self, kp, ki, kd, kff=None, kfa=None):
        self.kp = kp
        self.ki = ki
        self.kd = kd
        if kff is not None:
            self.kff = kff
        else:
            self.kff = 0.0
        if kfa is not None:
            self.kfa = kfa
        else:
            self.kfa = 0.0

    def update(self, desired_pos, measured_pos):
        delta_time = time_sources.global_time.getDelta()
        
        # bound the desired position
        #desired_pos = self.boundDesiredPosition(desired_pos)

        desired_vel = (desired_pos - self.prev_desired_pos) / delta_time

        error = desired_pos - measured_pos

        self.peak_detector.update(error)
       
        if 0:
            if self.peak_detector.hasConverged():
                if self.peak_detector.isUnstable():
                    warningstring = ("LimbController: Maximum error for the" +
                                     " desired point has increased for %d seconds," +
                                     " but is within converged range.  Might be unstable." %
                                     self.peak_detector.getResolveTime())
                    #logger.warning(warningstring,
                    #        desired_pos=desired_pos,
                    #        measured_pos=measured_pos,
                    #        error=error, 
                    #        bad_value=error)
                elif self.peak_detector.isLimitCycle():
                    warningstring = ("LimbController: Maximum error for the" +
                                     " desired point has increased once or more for %d seconds," +
                                     " but is within converged range.  Might be unstable." %
                                     self.peak_detector.getResolveTime())
                    #logger.warning(warningstring,
                    #        desired_pos=desired_pos,
                    #        measured_pos=measured_pos,
                    #        error=error,
                    #        bad_value=error)
            else:
                if self.peak_detector.isUnstable():
                    errorstring = ("LimbController: Maximum error for the desired point" + 
                                   "has increased for %d seconds.  System potentially unstable." %
                                   self.peak_detector.getResolveTime())
                    #logger.error(errorstring,
                    #        desired_pos=desired_pos,
                    #        measured_pos=measured_pos,
                    #        error=error,
                    #        bad_value=error)
                    raise ValueError(errorstring)
                elif self.peak_detector.isLimitCycle():
                    errorstring = ("LimbController: Controller has not converged" + 
                                   "over %d seconds.  System potentially in a limit cycle." %
                                   self.peak_detector.getResolveTime())

                    #logger.error(errorstring,
                    #        desired_pos=desired_pos,
                    #        measured_pos=measured_pos,
                    #        error=error,
                    #        bad_value=error)
                    raise ValueError(errorstring)
        
        self.integral_error_accumulator += self.ki * error * delta_time
        derivative_error = self.d_lowpass.update((error - self.prev_error) / delta_time)

        velocity_error = desired_vel - derivative_error

        self.prev_error = error
        self.prev_desired_pos = desired_pos
        
        actuator_command = (self.kp * error + 
                            self.integral_error_accumulator + 
                            self.kd * derivative_error + 
                            self.kff * desired_vel + 
                            self.kfa * velocity_error)
        #actuator_command = self.boundActuatorCommand(actuator_command, measured_pos)
        
        return actuator_command

    def isErrorInBounds(self, error, measured_pos):
        """tests whether or not the error signal is within reasonable range
        not checking for NaN, since both desired and measured position
        are tested for that
        """
        
        #makes sure the error is bounded by a single leg rotation
        error = error % (2 * math.pi)
        error_min = -math.pi / 2
        error_max = math.pi / 2
        
        #is error within available soft range?
        if error > (measured_pos - error_min) or error > (error_max - measured_pos):
            #logger.error("LimbController.isErrorInBounds: error out of soft bounds.",
            #            error=error,
            #            measured_pos=measured_pos)
            raise ValueError("Error signal points to a position out of soft bounds.")
        return error
    
    def boundDesiredPosition(self, desired_pos):
        #caps desired position to soft movement range
        if math.isnan(desired_pos):
            #logger.error("LimbController.boundDesiredPosition: NaN where aN expected!",
            #            desired_pos=desired_pos,
            #            bad_value="desired_pos")
            raise ValueError("LimbController: desired_pos cannot be NaN.")
        
        command_min = -20
        command_max = 20
        
        if desired_pos < command_min or desired_pos > command_max:
            #logger.error("LimbController.boundDesiredPosition:"+
            #            " desired position out of bounds!",
            #            desired_pos=desired_pos,
            #            command_min=command_min,
            #            command_max=command_max,
            #            bad_value="desired_pos")
            raise ValueError("LimbController.boundDesiredPosition:" +
                             " desired position out of soft bounds")
        
        bounded_pos = saturate(desired_pos, command_min, command_max)
        return bounded_pos
    
    def boundActuatorCommand(self, actuator_command, measured_pos):
        #prevent the controller from commanding an unsafely fast actuator move
        if (abs(actuator_command - measured_pos) / time_sources.global_time.getDelta() > self.max_movement_rate):
            raise ValueError("LimbController: Actuator command would cause" +
                             "joint to move at an unsafe rate.")
        return actuator_command