Statistics
| Branch: | Tag: | Revision:

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