Blocks.PID 

 1import numpy as np
 2import math
 3from time import sleep
 4
 5def main(inputs, outputs, parameters, synchronise):
 6    '''
 7    ## Applies PID for a Given Error Value
 8    The error is read as an input from the `inputs wire`.\n
 9    The Kp, Ki, and Kd parameters are read from the parameters of the same name.
10    Once there it applies the PID technique to the error variable in order to minimize it.
11
12    The resulting values are shared through the `share_array()` function. 
13    
14    **Inputs**: Error
15
16    **Outputs**: `cmd_vel` (Linear Velocity, Angular Velocity)
17
18    **Parameters**: Kp, Ki, Kd
19    '''
20    auto_enable = True
21    try:
22        enable = inputs.read_number("Enable")
23    except Exception:
24        auto_enable = True
25
26    kp = parameters.read_number("Kp")
27    ki = parameters.read_number("Ki")
28    kd = parameters.read_number("Kd")
29
30    previousError, I = 0, 0
31    # Problem this should be inside the while loop
32    msg = inputs.read_number("Inp")
33
34    while(auto_enable or inputs.read_number('Enable')):
35        if msg is None:
36            continue
37
38        error = float(msg)
39        sleep(0.01)
40
41        P = error
42        I = I + error
43        D = error - previousError
44        PIDvalue = (kp*P) + (ki*I) + (kd*D)
45        previousError = error
46
47        linear_velocity = 5.0
48        angular_velocity = -PIDvalue
49
50        data = [linear_velocity, angular_velocity]
51        outputs.share_array("Out", data)
52        synchronise()
def main(inputs, outputs, parameters, synchronise) 
 6def main(inputs, outputs, parameters, synchronise):
 7    '''
 8    ## Applies PID for a Given Error Value
 9    The error is read as an input from the `inputs wire`.\n
10    The Kp, Ki, and Kd parameters are read from the parameters of the same name.
11    Once there it applies the PID technique to the error variable in order to minimize it.
12
13    The resulting values are shared through the `share_array()` function. 
14    
15    **Inputs**: Error
16
17    **Outputs**: `cmd_vel` (Linear Velocity, Angular Velocity)
18
19    **Parameters**: Kp, Ki, Kd
20    '''
21    auto_enable = True
22    try:
23        enable = inputs.read_number("Enable")
24    except Exception:
25        auto_enable = True
26
27    kp = parameters.read_number("Kp")
28    ki = parameters.read_number("Ki")
29    kd = parameters.read_number("Kd")
30
31    previousError, I = 0, 0
32    # Problem this should be inside the while loop
33    msg = inputs.read_number("Inp")
34
35    while(auto_enable or inputs.read_number('Enable')):
36        if msg is None:
37            continue
38
39        error = float(msg)
40        sleep(0.01)
41
42        P = error
43        I = I + error
44        D = error - previousError
45        PIDvalue = (kp*P) + (ki*I) + (kd*D)
46        previousError = error
47
48        linear_velocity = 5.0
49        angular_velocity = -PIDvalue
50
51        data = [linear_velocity, angular_velocity]
52        outputs.share_array("Out", data)
53        synchronise()

Block Description

Applies PID for a Given Error Value

The error is read as an input from the inputs wire.

The Kp, Ki, and Kd parameters are read from the parameters of the same name. Once there it applies the PID technique to the error variable in order to minimize it.

The resulting values are shared through the share_array() function.

Inputs: Error

Outputs: cmd_vel (Linear Velocity, Angular Velocity)

Parameters: Kp, Ki, Kd