Statistics
| Branch: | Tag: | Revision:

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_ */