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