amiro-os / include / amiro / MotorControl.h @ 753ccd04
History | View | Annotate | Download (8.72 KB)
1 | 58fe0e0b | Thomas Schöpping | #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 | 753ccd04 | galberding | struct motorGains
|
12 | { |
||
13 | int pGain = 0; |
||
14 | float iGain = 0.0; |
||
15 | float dGain = 0.0; |
||
16 | }; |
||
17 | |||
18 | 58fe0e0b | Thomas Schöpping | 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 | ff7ad65b | Thomas Schöpping | /**
|
141 | * @brief Resets control gains.
|
||
142 | */
|
||
143 | void resetGains();
|
||
144 | |||
145 | 753ccd04 | galberding | void setGains(motorGains *motorConfig);
|
146 | |||
147 | void getGains(motorGains *motorConfig);
|
||
148 | |||
149 | void setMotorEnable(bool enable); |
||
150 | bool getMotorEnable();
|
||
151 | void toggleMotorEnable();
|
||
152 | 58fe0e0b | Thomas Schöpping | 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 | 753ccd04 | galberding | bool motorEnable = true; |
280 | 58fe0e0b | Thomas Schöpping | |
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_ */ |