Main Page
Classes
Files
File List
File Members
src
diff_controller.h
Go to the documentation of this file.
1
/*
2
ArbotiX Firmware for ROS driver
3
Copyright (c) 2009-2011 Vanadium Labs LLC. All right reserved.
4
5
Redistribution and use in source and binary forms, with or without
6
modification, are permitted provided that the following conditions are met:
7
8
* Redistributions of source code must retain the above copyright
9
notice, this list of conditions and the following disclaimer.
10
* Redistributions in binary form must reproduce the above copyright
11
notice, this list of conditions and the following disclaimer in the
12
documentation and/or other materials provided with the distribution.
13
* Neither the name of Vanadium Labs LLC nor the names of its
14
contributors may be used to endorse or promote products derived
15
from this software without specific prior written permission.
16
17
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
18
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
19
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
20
DISCLAIMED. IN NO EVENT SHALL VANADIUM LABS BE LIABLE FOR ANY DIRECT, INDIRECT,
21
INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
22
LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
23
OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
24
LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
25
OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
26
ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
27
*/
28
29
#include <Arduino.h>
30
#include <math.h>
31
32
/* Register Storage */
33
int
left_pwm
;
34
int
right_pwm
;
35
int
left_speed
;
36
int
right_speed
;
37
38
/* PID Parameters */
39
int
Kp
;
40
int
Kd
;
41
int
Ki
;
42
int
Ko
;
// output scale
43
int
maxAccel
;
// maximum acceleration per frame (ticks)
44
45
/* PID modes */
46
unsigned
int
PIDmode
;
47
#define PID_NONE 0
48
#define PID_SPEED 1
49
50
#define FRAME_RATE 33 // frame rate in millis (30Hz)
51
#define FRAMES 30
52
unsigned
long
f_time
;
// last frame
53
54
unsigned
char
moving
= 0;
// base in motion
55
unsigned
char
paused
= 0;
// base was in motion, can resume
56
#define MAXOUTPUT 255 // motor PWM
57
58
/* Setpoint Info For a Motor */
59
typedef
struct
{
60
int
Velocity
;
// desired actual speed (count/frame)
61
long
Encoder
;
// actual reading
62
long
PrevEnc
;
// last reading
63
int
PrevErr
;
64
int
Ierror
;
65
int
output
;
// last motor setting
66
}
SetPointInfo
;
67
68
SetPointInfo
left
,
right
;
69
70
/* Initialize PID parameters to something known */
71
void
setupPID
(){
72
// Default values for the PR-MINI
73
Kp
= 25;
74
Kd
= 30;
75
Ki
= 0;
76
Ko
= 100;
77
maxAccel
= 50;
78
f_time
= 0;
79
}
80
81
/* PID control of motor speed */
82
void
DoPid
(
SetPointInfo
* p){
83
long
Perror;
84
long
output;
85
86
Perror = p->
Velocity
- (p->
Encoder
-p->
PrevEnc
);
87
88
// Derivative error is the delta Perror
89
output = (
Kp
*Perror +
Kd
*(Perror - p->
PrevErr
) +
Ki
*p->
Ierror
)/
Ko
;
90
p->
PrevErr
= Perror;
91
p->
PrevEnc
= p->
Encoder
;
92
93
output += p->
output
;
94
// Accumulate Integral error *or* Limit output.
95
// Stop accumulating when output saturates
96
if
(output >=
MAXOUTPUT
)
97
output =
MAXOUTPUT
;
98
else
if
(output <= -
MAXOUTPUT
)
99
output = -
MAXOUTPUT
;
100
else
101
p->
Ierror
+= Perror;
102
103
p->
output
= output;
104
}
105
106
/* Clear accumulators */
107
void
ClearPID
(){
108
PIDmode
= 0;
moving
= 0;
109
left.
PrevErr
= 0;
110
left.
Ierror
= 0;
111
left.
output
= 0;
112
right.
PrevErr
= 0;
113
right.
Ierror
= 0;
114
right.
output
= 0;
115
}
116
117
/* This is called by the main loop, does a X HZ PID loop. */
118
void
updatePID
(){
119
if
((
moving
> 0) && (
PIDmode
> 0)){
// otherwise, just return
120
unsigned
long
j = millis();
121
if
(j >
f_time
){
122
// update encoders
123
left.
Encoder
= Encoders.left;
124
right.
Encoder
= Encoders.right;
125
// do PID update on PWM
126
DoPid
(&left);
127
DoPid
(&right);
128
// set updated motor outputs
129
if
(
PIDmode
> 0){
130
drive.set(left.
output
, right.
output
);
131
}
132
// update timing
133
f_time
= j +
FRAME_RATE
;
134
}
135
}
136
}
137
138
void
clearAll
(){
139
PIDmode
= 0;
140
left.
Encoder
= 0;
141
right.
Encoder
= 0;
142
left.
PrevEnc
= 0;
143
right.
PrevEnc
= 0;
144
left.
output
= 0;
145
right.
output
= 0;
146
Encoders.Reset();
147
}
148
updatePID
void updatePID()
Definition:
diff_controller.h:118
maxAccel
int maxAccel
Definition:
diff_controller.h:43
SetPointInfo::PrevEnc
long PrevEnc
Definition:
diff_controller.h:62
clearAll
void clearAll()
Definition:
diff_controller.h:138
right_speed
int right_speed
Definition:
diff_controller.h:36
DoPid
void DoPid(SetPointInfo *p)
Definition:
diff_controller.h:82
FRAME_RATE
#define FRAME_RATE
Definition:
diff_controller.h:50
SetPointInfo::PrevErr
int PrevErr
Definition:
diff_controller.h:63
moving
unsigned char moving
Definition:
diff_controller.h:54
Ko
int Ko
Definition:
diff_controller.h:42
SetPointInfo
Definition:
diff_controller.h:59
Kp
int Kp
Definition:
diff_controller.h:39
Ki
int Ki
Definition:
diff_controller.h:41
f_time
unsigned long f_time
Definition:
diff_controller.h:52
setupPID
void setupPID()
Definition:
diff_controller.h:71
SetPointInfo::output
int output
Definition:
diff_controller.h:65
paused
unsigned char paused
Definition:
diff_controller.h:55
right_pwm
int right_pwm
Definition:
diff_controller.h:34
SetPointInfo::Velocity
int Velocity
Definition:
diff_controller.h:60
ClearPID
void ClearPID()
Definition:
diff_controller.h:107
Kd
int Kd
Definition:
diff_controller.h:40
MAXOUTPUT
#define MAXOUTPUT
Definition:
diff_controller.h:56
left_speed
int left_speed
Definition:
diff_controller.h:35
left_pwm
int left_pwm
Definition:
diff_controller.h:33
SetPointInfo::Ierror
int Ierror
Definition:
diff_controller.h:64
PIDmode
unsigned int PIDmode
Definition:
diff_controller.h:46
right
SetPointInfo right
Definition:
diff_controller.h:68
SetPointInfo::Encoder
long Encoder
Definition:
diff_controller.h:61
left
SetPointInfo left
Definition:
diff_controller.h:68
arbotix_firmware
Author(s): Michael Ferguson
autogenerated on Mon Jun 10 2019 12:43:32