mb_controller.c 2.79 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
17
18
19
20
21
22
23
24
    //mb_load_controller_config();
    left_wheel_velocity_pid = rc_filter_empty();
    right_wheel_velocity_pid = rc_filter_empty();
    //put variable for left pid and right pid
    float kp = 1.0;
    float ki = 0.0;
    float kd = 0.0;
    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);
pgaskell's avatar
pgaskell committed
25
26
27
28
29
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
    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
*
*******************************************************************************/

int mb_controller_update(mb_state_t* mb_state, mb_setpoints_t* mb_setpoints){  
gxiang's avatar
gxiang committed
74
75
76
77
    float left_error = mb_setpoints->fwd_velocity - mb_state->left_velocity;
    float right_error = mb_setpoints->fwd_velocity - mb_state->right_velocity;
    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);
pgaskell's avatar
pgaskell committed
78
79
80
81
82
83
84
85
86
87
88
89
90
91
    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
92
93
    rc_filter_free(&left_wheel_velocity_pid);
    rc_filter_free(&right_wheel_velocity_pid);
pgaskell's avatar
pgaskell committed
94
95
    return 0;
}