Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Menu
Open sidebar
diy
2018-gr2-project-selbstbalancierender-roboter
Commits
5f3833ec
Commit
5f3833ec
authored
Aug 01, 2018
by
Stefan
Browse files
filter korrigiert
parent
0cb04197
Changes
3
Hide whitespace changes
Inline
Side-by-side
HalloRobot/libProject/include/filter.h
View file @
5f3833ec
...
...
@@ -19,11 +19,6 @@
*/
float
complementary_filter
(
float
phi_acc
,
float
gyro_rate
,
uint16_t
f_sampling
);
/*! \brief Implements a iir Filter
* \param phi_acc angle phi calculated from accelero data
* \returns filtered phi_acc
*/
float
iir_filter
(
float
phi_acc
);
/*! \brief Resets filter angle
*/
...
...
HalloRobot/libProject/src/control.c
View file @
5f3833ec
#include
"control.h"
#include
<math.h>
#include
"ezs_io.h"
#include
"filter.h"
//defines
#define OUT_MAX 100 //maximum output in percent
...
...
@@ -92,6 +91,24 @@ float pid_control(float e)
return
new_out
;
}
static
float
iir_filter
(
float
phi_acc
)
{
static
float
in_1
=
0
;
static
float
in_2
=
0
;
static
float
out_1
=
0
;
static
float
out_2
=
0
;
const
float
a
[
2
]
=
{
-
1
.
9112
,
0
.
915
};
const
float
b
[
3
]
=
{
9.4469e-4
,
0
.
001
9
,
9.4469e-4
};
float
new_out
=
b
[
0
]
*
phi_acc
+
b
[
1
]
*
in_1
+
b
[
2
]
*
in_2
-
a
[
0
]
*
out_1
-
a
[
1
]
*
out_2
;
out_2
=
out_1
;
out_1
=
new_out
;
in_2
=
in_1
;
in_1
=
phi_acc
;
return
new_out
;
}
/*
* Implements Offset-controller.
* Feeds back correction control_output to correct offset.
...
...
@@ -104,7 +121,9 @@ float offset_control(float motor_signal)
// sum_offset += K*motor_signal/F_CONTROL;
float
new_offset
=
iir_filter
(
K
*
motor_signal
/
F_CONTROL
);
float
new_offset
=
iir_filter
(
K
*
motor_signal
);
// sum_offset = sum_offset + iir_filter(K*motor_signal);
// float new_offset = sum_offset
if
(
new_offset
>
max_offset
){
new_offset
=
max_offset
;
sum_offset
-=
motor_signal
/
F_CONTROL
;
...
...
HalloRobot/libProject/src/filter.c
View file @
5f3833ec
...
...
@@ -5,10 +5,7 @@ static float phi_cor = 0;
/* IIR filter with butterworth coefficients */
/*
* Implements a IIR filter with butterworth coefficients
*/
float
iir_filter
(
float
phi_acc
)
static
float
iir_filter
(
float
phi_acc
)
{
static
float
in_1
=
0
;
static
float
in_2
=
0
;
...
...
@@ -26,6 +23,7 @@ float iir_filter(float phi_acc)
return
new_out
;
}
/*
* Implements a basic complementary filter,
* representing a low-pass for accelero and high-pass for gyro.
...
...
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment