コード例 #1
0
ファイル: joint_controller.py プロジェクト: braingram/Main
 def __init__(self, name=None, *args, **kwargs):
     super(LearningPistonController, self).__init__(*args, **kwargs)
     pit_init = super(LearningPistonController, self).valveCommandFromFlow
     self.clay_pit_pos = OneDClayPit(min_x=1e-6,
                                     max_x=4.0 * gpm2cmps,
                                     n_points=64,
                                     init_func=pit_init,
                                     max_slope=1.0 / (6.0 * gpm2cmps),
                                     k=1.0,
                                     name=name + '_pos')
     self.clay_pit_neg = OneDClayPit(min_x=-4.0 * gpm2cmps,
                                     max_x=-1e-6,
                                     n_points=64,
                                     init_func=pit_init,
                                     max_slope=1.0 / (6.0 * gpm2cmps),
                                     k=1.0,
                                     name=name + '_neg')
     self.valve_cmd = 0.0
コード例 #2
0
 def __init__(self, name=None, *args, **kwargs):
     super(LearningPistonController, self).__init__(*args, **kwargs)
     pit_init = super(LearningPistonController, self).valveCommandFromFlow
     self.clay_pit_pos = OneDClayPit( min_x=1e-6, max_x = 4.0*gpm2cmps,  n_points = 64, init_func=pit_init, max_slope=1.0/(6.0*gpm2cmps), k=1.0, name=name+'_pos' )
     self.clay_pit_neg = OneDClayPit( min_x=-4.0*gpm2cmps, max_x = -1e-6, n_points = 64, init_func=pit_init, max_slope=1.0/(6.0*gpm2cmps), k=1.0, name=name+'_neg' )
     self.valve_cmd = 0.0
コード例 #3
0
ファイル: joint_controller.py プロジェクト: braingram/Main
class LearningPistonController(PistonController):
    """
    Stores a history of measured rates vs. valve commands and corrects itself over time.
    """
    def __init__(self, name=None, *args, **kwargs):
        super(LearningPistonController, self).__init__(*args, **kwargs)
        pit_init = super(LearningPistonController, self).valveCommandFromFlow
        self.clay_pit_pos = OneDClayPit(min_x=1e-6,
                                        max_x=4.0 * gpm2cmps,
                                        n_points=64,
                                        init_func=pit_init,
                                        max_slope=1.0 / (6.0 * gpm2cmps),
                                        k=1.0,
                                        name=name + '_pos')
        self.clay_pit_neg = OneDClayPit(min_x=-4.0 * gpm2cmps,
                                        max_x=-1e-6,
                                        n_points=64,
                                        init_func=pit_init,
                                        max_slope=1.0 / (6.0 * gpm2cmps),
                                        k=1.0,
                                        name=name + '_neg')
        self.valve_cmd = 0.0

    def saveState(self):
        self.clay_pit_pos.saveState()
        self.clay_pit_neg.saveState()

    def update(self, target_rate, measured_rate=None):
        target_flow = self.flowFromLinearRate(target_rate)
        # Calc valve command
        # MIN FLOW RATE
        min_flow = 0.025 * gpm2cmps
        if target_flow > min_flow:
            self.valve_cmd = self.clay_pit_pos.lookup(target_flow)
        elif target_flow < -min_flow:
            self.valve_cmd = self.clay_pit_neg.lookup(target_flow)
        else:
            #Interpolate through the deadband.  Better than just setting 0.
            rem = (target_flow + min_flow) / (2 * min_flow)
            self.valve_cmd = (1-rem)*self.clay_pit_pos.lookup( min_flow)\
                             + (rem)*self.clay_pit_neg.lookup(-min_flow)
        # "Learn"
        if measured_rate != None:
            measured_flow = self.flowFromLinearRate(measured_rate)

            def sign(x):
                if x < 0:
                    return -1
                if x > 0:
                    return 1
                return 0

            def sat(x, lim):
                return max(min(x, lim), -lim)
            if sign(measured_flow) == sign(self.valve_cmd)\
                    and abs(target_flow) > min_flow:
                flow_error = target_flow - measured_flow
                #flow_error = measured_flow-target_flow
                error_mult = 10
                err = flow_error * error_mult
                #err = sat(err, 1e-5)
                if target_flow > 0:
                    self.clay_pit_pos.lookup(target_flow, self.valve_cmd + err)
                elif target_flow < 0:
                    self.clay_pit_neg.lookup(target_flow, self.valve_cmd + err)
        return self.valve_cmd
コード例 #4
0
class LearningPistonController(PistonController):
    """
    Stores a history of measured rates vs. valve commands and corrects itself over time.
    """
    def __init__(self, name=None, *args, **kwargs):
        super(LearningPistonController, self).__init__(*args, **kwargs)
        pit_init = super(LearningPistonController, self).valveCommandFromFlow
        self.clay_pit_pos = OneDClayPit( min_x=1e-6, max_x = 4.0*gpm2cmps,  n_points = 64, init_func=pit_init, max_slope=1.0/(6.0*gpm2cmps), k=1.0, name=name+'_pos' )
        self.clay_pit_neg = OneDClayPit( min_x=-4.0*gpm2cmps, max_x = -1e-6, n_points = 64, init_func=pit_init, max_slope=1.0/(6.0*gpm2cmps), k=1.0, name=name+'_neg' )
        self.valve_cmd = 0.0
    def saveState(self):
        self.clay_pit_pos.saveState()
        self.clay_pit_neg.saveState()
    def update(self, target_rate, measured_rate=None):
        target_flow = self.flowFromLinearRate(target_rate)
        # Calc valve command
        # MIN FLOW RATE
        min_flow = 0.025*gpm2cmps
        if target_flow > min_flow:
            self.valve_cmd = self.clay_pit_pos.lookup(target_flow)
        elif target_flow < -min_flow:
            self.valve_cmd = self.clay_pit_neg.lookup(target_flow)
        else:
            #Interpolate through the deadband.  Better than just setting 0.
            rem = (target_flow+min_flow)/(2*min_flow)
            self.valve_cmd = (1-rem)*self.clay_pit_pos.lookup( min_flow)\
                             + (rem)*self.clay_pit_neg.lookup(-min_flow)
        # "Learn"
        if measured_rate != None:
            measured_flow = self.flowFromLinearRate(measured_rate)
            def sign(x):
                if x<0:
                    return -1
                if x>0:
                    return 1
                return 0
            def sat(x,lim):
                return max(min(x,lim),-lim)
            if sign(measured_flow) == sign(self.valve_cmd)\
                    and abs(target_flow) > min_flow:
                flow_error = target_flow-measured_flow
                #flow_error = measured_flow-target_flow
                error_mult=10
                err = flow_error*error_mult
                #err = sat(err, 1e-5)
                if   target_flow > 0:
                    self.clay_pit_pos.lookup( target_flow, self.valve_cmd+err )
                elif target_flow < 0:
                    self.clay_pit_neg.lookup( target_flow, self.valve_cmd+err )
        return self.valve_cmd