mb_controller.c 3.4 KB
Newer Older
pgaskell's avatar
pgaskell committed
1
2
3
4
5
6
7
8
9
10
11
12
13
#include "../mobilebot/mobilebot.h"

/*******************************************************************************
* int mb_initialize()
*
* this initializes all the PID controllers from the configuration file
* you can use this as is or modify it if you want a different format
*
* return 0 on success
*
*******************************************************************************/

int mb_initialize_controller(){
gxiang's avatar
gxiang committed
14
15
16
    //mb_load_controller_config();
    left_wheel_velocity_pid = rc_filter_empty();
    right_wheel_velocity_pid = rc_filter_empty();
gxiang's avatar
gxiang committed
17
18
    left_wheel_low_pass = rc_filter_empty();
    right_wheel_low_pass = rc_filter_empty();
gxiang's avatar
gxiang committed
19
    //put variable for left pid and right pid
gxiang's avatar
gxiang committed
20
21
22
    float kp = 0.65;
    float ki = 0.02;
    float kd = 0.15;
gxiang's avatar
gxiang committed
23
24
25
26
    rc_filter_pid(&left_wheel_velocity_pid,kp,ki,kd,0.1,DT);
    rc_filter_pid(&right_wheel_velocity_pid,kp,ki,kd,0.1,DT);
    rc_filter_enable_saturation(&left_wheel_velocity_pid,-1.0,1.0);
    rc_filter_enable_saturation(&right_wheel_velocity_pid,-1.0,1.0);
gxiang's avatar
gxiang committed
27
28
29
    
    rc_filter_first_order_lowpass(&left_wheel_low_pass,DT,0.1);
    rc_filter_first_order_lowpass(&right_wheel_low_pass,DT,0.1);
pgaskell's avatar
pgaskell committed
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
    return 0;
}

/*******************************************************************************
* int mb_load_controller_config()
*
* this provides a basic configuration load routine
* you can use this as is or modify it if you want a different format
*
* return 0 on success
*
*******************************************************************************/


int mb_load_controller_config(){
    FILE* file = fopen(CFG_PATH, "r");
    if (file == NULL){
        printf("Error opening pid.cfg\n");
    }

/******
*
*   Example of loading a line from .cfg file:
*
*    fscanf(file, "%f %f %f %f", 
*        &pid_params.kp,
*        &pid_params.ki,
*        &pid_params.kd,
*        &pid_params.dFilterHz
*        );
*
******/

    fclose(file);
    return 0;
}

/*******************************************************************************
* int mb_controller_update()
* 
* TODO: Write your PID controller here
* take inputs from the global mb_state
* write outputs to the global mb_state
*
* return 0 on success
*
*******************************************************************************/

gxiang's avatar
gxiang committed
78
79
80
81
82
int mb_controller_update(mb_state_t* mb_state, mb_setpoints_t* mb_setpoints){
    //mb_state->left_velocity = rc_filter_march(&left_wheel_low_pass,mb_state->left_velocity);
    //mb_state->right_velocity = rc_filter_march(&right_wheel_low_pass, mb_state->right_velocity);
    float left_error = mb_setpoints->fwd_velocity - LEFT_MOTOR_POL*mb_state->left_velocity;
    float right_error = mb_setpoints->fwd_velocity -  RIGHT_MOTOR_POL*mb_state->right_velocity;
gxiang's avatar
gxiang committed
83
84
    mb_state->left_cmd = rc_filter_march(&left_wheel_velocity_pid, left_error);
    mb_state->right_cmd = rc_filter_march(&right_wheel_velocity_pid, right_error);
gxiang's avatar
gxiang committed
85
86
    mb_state->left_cmd = rc_filter_march(&left_wheel_low_pass,mb_state->left_cmd);
    mb_state->right_cmd = rc_filter_march(&right_wheel_low_pass, mb_state->right_cmd );
pgaskell's avatar
pgaskell committed
87
88
89
90
91
92
93
94
95
96
97
98
99
100
    return 0;
}


/*******************************************************************************
* int mb_destroy_controller()
* 
* TODO: Free all resources associated with your controller
*
* return 0 on success
*
*******************************************************************************/

int mb_destroy_controller(){
gxiang's avatar
gxiang committed
101
102
    rc_filter_free(&left_wheel_velocity_pid);
    rc_filter_free(&right_wheel_velocity_pid);
pgaskell's avatar
pgaskell committed
103
104
    return 0;
}