mb_controller.c 3.42 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
23
24
25
26
27
    float kpL = 2;
    float kiL = 7;
    float kdL = 0.2;
     float kpR = 2;
    float kiR = 7;
    float kdR = 0.2;
    rc_filter_pid(&left_wheel_velocity_pid,kpL,kiL,kdL,0.1,DT);
    rc_filter_pid(&right_wheel_velocity_pid,kpR,kiR,kdR,0.1,DT);
gxiang's avatar
gxiang committed
28
29
    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
30
31
32
    
    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
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
78
79
80
    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
81
int mb_controller_update(mb_state_t* mb_state, mb_setpoints_t* mb_setpoints){
gxiang's avatar
gxiang committed
82
83
84
85
86
87
    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_vel = (2*mb_setpoints->fwd_velocity  - mb_setpoints->turn_velocity*WHEEL_BASE)/2;
    float right_vel = (2*mb_setpoints->fwd_velocity + mb_setpoints->turn_velocity*WHEEL_BASE)/2;
    float left_error = left_vel - mb_state->left_velocity;
    float right_error = right_vel -  mb_state->right_velocity;
gxiang's avatar
gxiang committed
88
89
    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
90
   
pgaskell's avatar
pgaskell committed
91
92
93
94
95
96
97
98
99
100
101
102
103
104
    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
105
106
    rc_filter_free(&left_wheel_velocity_pid);
    rc_filter_free(&right_wheel_velocity_pid);
pgaskell's avatar
pgaskell committed
107
108
    return 0;
}