mb_controller.c 3.59 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
    //mb_load_controller_config();
Demo User's avatar
Demo User committed
15

gxiang's avatar
gxiang committed
16
17
    left_wheel_velocity_pid = rc_filter_empty();
    right_wheel_velocity_pid = rc_filter_empty();
gxiang's avatar
gxiang committed
18
19
    left_wheel_low_pass = rc_filter_empty();
    right_wheel_low_pass = rc_filter_empty();
gxiang's avatar
gxiang committed
20
    //put variable for left pid and right pid
gxiang's avatar
gxiang committed
21
22
23
24
25
26
27
28
    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
29
30
    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
31
32
33
    
    rc_filter_first_order_lowpass(&left_wheel_low_pass,DT,0.1);
    rc_filter_first_order_lowpass(&right_wheel_low_pass,DT,0.1);
Demo User's avatar
Demo User committed
34

pgaskell's avatar
pgaskell committed
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
81
82
    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
83
int mb_controller_update(mb_state_t* mb_state, mb_setpoints_t* mb_setpoints){
gxiang's avatar
gxiang committed
84
85
86
87
88
89
    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
90
91
    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);
Demo User's avatar
Demo User committed
92
93
   // 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
94
95
96
97
98
99
100
101
102
103
104
105
106
107
    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
108
109
    rc_filter_free(&left_wheel_velocity_pid);
    rc_filter_free(&right_wheel_velocity_pid);
pgaskell's avatar
pgaskell committed
110
111
    return 0;
}