amiro-os / include / amiro / MotorControl.h @ 753ccd04
History | View | Annotate | Download (8.72 KB)
1 |
#ifndef AMIRO_MOTOR_CONTROL_H_
|
---|---|
2 |
#define AMIRO_MOTOR_CONTROL_H_
|
3 |
|
4 |
#include <amiro/MotorIncrements.h> |
5 |
#include <amiro/Constants.h> |
6 |
#include <Types.h> |
7 |
#include <amiro/FileSystemInputOutput/FSIODiWheelDrive.hpp> |
8 |
|
9 |
namespace amiro { |
10 |
|
11 |
struct motorGains
|
12 |
{ |
13 |
int pGain = 0; |
14 |
float iGain = 0.0; |
15 |
float dGain = 0.0; |
16 |
}; |
17 |
|
18 |
class MotorControl : public chibios_rt::BaseStaticThread<512> {
|
19 |
public:
|
20 |
/**
|
21 |
* Constructor
|
22 |
*
|
23 |
* @param pwm pulse width modulation driver (pwmd)
|
24 |
* Can be any free PWMDx in 'ChibiOS-RT/os/hal/platforms/STM32/TIMv1/pwm_lld.h'
|
25 |
* @param mi object for retrieving the motor increments of the qei
|
26 |
* @param port GPIO port for motor control (should be the macro 'GPIOB')
|
27 |
* @param pad GPIO command for motor control (should be the macro 'GPIOB_POWER_EN' for enable)
|
28 |
* @param memory Memory interface to load/store parameters
|
29 |
*/
|
30 |
MotorControl(PWMDriver* pwm, MotorIncrements* mi, GPIO_TypeDef* port, int pad, fileSystemIo::FSIODiWheelDrive *memory);
|
31 |
|
32 |
/**
|
33 |
* Get the current speed of the left wheel in rounds/min
|
34 |
*
|
35 |
* @return speed of left wheel in rounds/min
|
36 |
*/
|
37 |
int getCurrentRPMLeft();
|
38 |
|
39 |
/**
|
40 |
* Get the current speed of the right wheel in rounds/min
|
41 |
*
|
42 |
* @return speed of right wheel in rounds/min
|
43 |
*/
|
44 |
int getCurrentRPMRight();
|
45 |
|
46 |
chibios_rt::EvtSource* getEventSource(); |
47 |
|
48 |
/**
|
49 |
* Sets the target velocity
|
50 |
*
|
51 |
* @param targetVelocity Kartesian kinematic vector
|
52 |
*/
|
53 |
void setTargetSpeed(const types::kinematic &targetVelocity); |
54 |
|
55 |
/**
|
56 |
* Sets the target velocity in µ rounds per minute for every wheel
|
57 |
*
|
58 |
* @param targetURpmLeft Rounds per minute in µ1/min of the left wheel
|
59 |
* @param targetURpmLeft Rounds per minute in µ1/min of the right wheel
|
60 |
*/
|
61 |
void setTargetRPM(int32_t targetURpmLeft, int32_t targetURpmRight);
|
62 |
|
63 |
/**
|
64 |
* Get the current velocitiy as a kinematic struct
|
65 |
*
|
66 |
* @return Current velocity
|
67 |
*/
|
68 |
types::kinematic getCurrentVelocity(); |
69 |
|
70 |
/**
|
71 |
* Sets the correction factor for the wheel diameter.
|
72 |
* The factor Ed is the ratio of the wheel diameters
|
73 |
* <Ed = wheelDiameterRight / wheelDiameterLeft> as
|
74 |
* introduced in eq. 3 by J. Borenstein (Correction of
|
75 |
* Systematic Odometry Errors in Mobile Robots). This
|
76 |
* function calculates then the correction factors for every
|
77 |
* wheel by eq. 17a and 17b.
|
78 |
*
|
79 |
* @param Ed Wheel diameter ratio
|
80 |
* @param storeEbToMemory Do override Ed in the memory with the given value
|
81 |
* @return Return value of the memory interface
|
82 |
*/
|
83 |
msg_t setWheelDiameterCorrectionFactor(float Ed = 1.0f, bool_t storeEdToMemory = false); |
84 |
|
85 |
/**
|
86 |
* Sets the correction factor for the wheel base width.
|
87 |
* The factor Eb is the ratio of the actual and nominal
|
88 |
* base width <Eb = bActual / bNominal> as introduced
|
89 |
* in eq. 3 by J. Borenstein (Correction of
|
90 |
* Systematic Odometry Errors in Mobile Robots). This
|
91 |
* function calculates then the actual base width of the
|
92 |
* robot using eq. 4.
|
93 |
*
|
94 |
* @param Eb Base width ratio
|
95 |
* @param storeEbToMemory Do override Eb in the memory with the given value
|
96 |
* @return Return value of the memory interface
|
97 |
*/
|
98 |
msg_t setActualWheelBaseDistance(float Eb = 1.0f, bool_t storeEbToMemory = false); |
99 |
|
100 |
/**
|
101 |
* Calculate the current increments of both wheels and
|
102 |
* update oldIncrement and incrementsDifference,
|
103 |
* which is the corrected difference between the current increments
|
104 |
* and the old ones.
|
105 |
* The corrected difference is the original difference between the old
|
106 |
* increments and new increments, multiplied with the wheelDiameterCorrectionFactor.
|
107 |
* The incremennt source is given by motorIncrements.
|
108 |
*
|
109 |
* @param motorIncrements Increment source
|
110 |
* @param oldIncrement Old increments, which are updated
|
111 |
* @param incrementDifference Corrected difference between old and current increments
|
112 |
* @return Return value of the memory interface
|
113 |
*/
|
114 |
static void updateIncrements(MotorIncrements* motorIncrements, int32_t (&oldIncrement)[2], float (&incrementDifference)[2]); |
115 |
|
116 |
/**
|
117 |
* Calculate the current speed of both wheels and
|
118 |
* updates actualSpeed.
|
119 |
*
|
120 |
* @param incrementDifference Difference between old and current increments
|
121 |
* @param actualSpeed Actual speed of both wheels
|
122 |
* @param Update period
|
123 |
*/
|
124 |
static void updateSpeed(const float (&incrementDifference)[2], int32_t (&actualSpeed)[2], const uint32_t period); |
125 |
|
126 |
/**
|
127 |
* Calculate the current driven distance of both wheels and
|
128 |
* updates actualDistance.
|
129 |
*
|
130 |
* @param incrementDifference Difference between old and current increments
|
131 |
* @param actualDistance Actual distance driven
|
132 |
*/
|
133 |
static void updateDistance(const float (&incrementDifference)[2], float (&actualDistance)[2]); |
134 |
|
135 |
/**
|
136 |
* Prints Control Gains
|
137 |
*/
|
138 |
void printGains();
|
139 |
|
140 |
/**
|
141 |
* @brief Resets control gains.
|
142 |
*/
|
143 |
void resetGains();
|
144 |
|
145 |
void setGains(motorGains *motorConfig);
|
146 |
|
147 |
void getGains(motorGains *motorConfig);
|
148 |
|
149 |
void setMotorEnable(bool enable); |
150 |
bool getMotorEnable();
|
151 |
void toggleMotorEnable();
|
152 |
protected:
|
153 |
virtual msg_t main(void);
|
154 |
|
155 |
private:
|
156 |
|
157 |
/**
|
158 |
* Calculate the velocitiy in the robot frame
|
159 |
* and saves it to this->currentVelocity
|
160 |
*/
|
161 |
void calcVelocity();
|
162 |
|
163 |
/**
|
164 |
* PID Controller that works directly on the forward velocity v of the robots center and its
|
165 |
* angular velocity w around its z axis. The methods setLeftWheelSpeed() and setRightWheelSpeed()
|
166 |
* are used to set the final pwm values.
|
167 |
*/
|
168 |
void PIDController();
|
169 |
|
170 |
/**
|
171 |
* Deletes the oldest entry in lastVelocitiesV[] and lastVelocitiesW[], pushes the other values up and saves the currentVelocity.x(w_z) in the last entry.
|
172 |
*/
|
173 |
void updateDerivativeLastVelocities();
|
174 |
|
175 |
|
176 |
////////////////////////////////////////////////
|
177 |
/////////////////Calibration////////////////////
|
178 |
////////////////////////////////////////////////
|
179 |
/**
|
180 |
* Finds a prefactor for the stronger motor to match its power level to the weaker motor.
|
181 |
*/
|
182 |
void calibrate();
|
183 |
|
184 |
/**
|
185 |
* Finds the P, I and D gains for the PID Controller using the Nichols-Ziegler Method.
|
186 |
*/
|
187 |
void calibrateZiegler();
|
188 |
|
189 |
/**
|
190 |
* Counts the number of sign changes from the last 30 velocities.
|
191 |
*/
|
192 |
int getNumberofSignChanges();
|
193 |
|
194 |
/**
|
195 |
* Update the past lastVelocitiesV array.
|
196 |
*/
|
197 |
void updatePastVelocities();
|
198 |
|
199 |
|
200 |
/**
|
201 |
* Sets wheel speed according to the output of the PID Controller
|
202 |
*/
|
203 |
void setLeftWheelSpeed(int diffv, int diffw); |
204 |
/**
|
205 |
* Sets wheel speed according to the output of the PID Controller
|
206 |
*/
|
207 |
void setRightWheelSpeed(int diffv, int diffw); |
208 |
|
209 |
int getLeftWheelSpeed();
|
210 |
int getRightWheelSpeed();
|
211 |
|
212 |
|
213 |
/**
|
214 |
* Write the duty cicle from this->pwmPercentage
|
215 |
* to the PWM driver
|
216 |
*/
|
217 |
void writePulseWidthModulation();
|
218 |
|
219 |
/**
|
220 |
* Control logic to save space in main loop
|
221 |
*/
|
222 |
void controllerAndCalibrationLogic();
|
223 |
|
224 |
|
225 |
|
226 |
PWMDriver* pwmDriver; |
227 |
PWMConfig pwmConfig; |
228 |
MotorIncrements* motorIncrements; |
229 |
GPIO_TypeDef *powerEnablePort; |
230 |
const int powerEnablePad; |
231 |
chibios_rt::EvtSource eventSource; |
232 |
//const uint32_t period;
|
233 |
uint32_t period; |
234 |
fileSystemIo::FSIODiWheelDrive *memory; |
235 |
int32_t actualSpeed[2];
|
236 |
float actualDistance[2]; |
237 |
float errorSum[2]; |
238 |
int32_t increment[2];
|
239 |
float incrementDifference[2]; |
240 |
int pwmPercentage[2]; |
241 |
types::kinematic targetVelocity; |
242 |
types::kinematic currentVelocity; |
243 |
types::kinematic lastVelocity; |
244 |
float errorSumDiff;
|
245 |
bool newTargetVelocities;
|
246 |
int delay = 0; |
247 |
float Ed;
|
248 |
float Eb;
|
249 |
|
250 |
int pGain = 1000; |
251 |
|
252 |
float iGain = 0.08; |
253 |
int antiWindupV = 7100000; |
254 |
int antiWindupW = 200000000; |
255 |
int accumulatedErrorV =0; // accumulated velocity error |
256 |
int accumulatedErrorW =0; // accumulated omega error |
257 |
|
258 |
float dGain =0.01; |
259 |
int lastVelocitiesV[6]; |
260 |
int lastVelocitiesW[6]; |
261 |
|
262 |
//motor calibration variables
|
263 |
int numberOfLastVelocitiesV = 30; |
264 |
int lastVelocitiesVBig[30]; |
265 |
int rightWValues[3]; |
266 |
int leftWValues[3]; |
267 |
float motorCalibrationFactor =1.0; |
268 |
//ziegler calibration variables
|
269 |
int zieglerHelp= 1; |
270 |
int zieglerHelp2 = 0; |
271 |
bool ziegler= true; |
272 |
bool ziegler2 = true; |
273 |
float zieglerPeriod;
|
274 |
int zieglerCalibrationTime = 0; |
275 |
int wheelCalibrationTime = 0; |
276 |
bool startedZieglerCalibration = false; |
277 |
bool startedWheelCalibration = false; |
278 |
bool motorCalibration = true; |
279 |
bool motorEnable = true; |
280 |
|
281 |
|
282 |
|
283 |
|
284 |
public:
|
285 |
bool isCalibrating = false; |
286 |
static float wheelDiameterCorrectionFactor[2]; |
287 |
static float actualWheelBaseDistanceSI; |
288 |
|
289 |
|
290 |
}; |
291 |
} |
292 |
|
293 |
#endif /* AMIRO_MOTOR_CONTROL_H_ */ |