borrowed codes
Dependencies: mbed Encoder_Nucleo_16_bits
main.cpp@1:e5dd0af74983, 2019-10-29 (annotated)
- Committer:
- tuk4
- Date:
- Tue Oct 29 12:02:28 2019 +0000
- Revision:
- 1:e5dd0af74983
- Parent:
- 0:242dc469ac2b
29102019 small changes
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
haarkon | 0:242dc469ac2b | 1 | #include "mbed.h" |
haarkon | 0:242dc469ac2b | 2 | #include "Nucleo_Encoder_16_bits.h" |
haarkon | 0:242dc469ac2b | 3 | |
haarkon | 0:242dc469ac2b | 4 | #define PI 3.1415926535898 |
haarkon | 0:242dc469ac2b | 5 | |
haarkon | 0:242dc469ac2b | 6 | Nucleo_Encoder_16_bits _Lencoder (TIM4); |
haarkon | 0:242dc469ac2b | 7 | Nucleo_Encoder_16_bits _Rencoder (TIM3); |
haarkon | 0:242dc469ac2b | 8 | PwmOut _Lpwm (PA_9); |
haarkon | 0:242dc469ac2b | 9 | PwmOut _Rpwm (PA_8); |
haarkon | 0:242dc469ac2b | 10 | DigitalOut _LdirA (PC_9); |
haarkon | 0:242dc469ac2b | 11 | DigitalOut _LdirB (PC_8); |
haarkon | 0:242dc469ac2b | 12 | DigitalOut _RdirA (PC_6); |
haarkon | 0:242dc469ac2b | 13 | DigitalOut _RdirB (PC_5); |
haarkon | 0:242dc469ac2b | 14 | |
haarkon | 0:242dc469ac2b | 15 | Serial pc (PA_2, PA_3, 921600); // Create a serial link to PC for communication |
haarkon | 0:242dc469ac2b | 16 | |
haarkon | 0:242dc469ac2b | 17 | DigitalOut led1 (PA_5); // Added Led1 for test purpose |
haarkon | 0:242dc469ac2b | 18 | DigitalOut led2 (PD_2); // Added Led2 for test purpose |
haarkon | 0:242dc469ac2b | 19 | DigitalOut disquette (PA_12); // Added baloon destructor command (without it, you might see baloon destructor motor be set to full speed) |
haarkon | 0:242dc469ac2b | 20 | |
haarkon | 0:242dc469ac2b | 21 | Ticker _tick; |
haarkon | 0:242dc469ac2b | 22 | |
haarkon | 0:242dc469ac2b | 23 | void controlLoop(void); |
haarkon | 0:242dc469ac2b | 24 | |
haarkon | 0:242dc469ac2b | 25 | /** |
haarkon | 0:242dc469ac2b | 26 | * Set the Kp value |
tuk4 | 1:e5dd0af74983 | 27 | *ccccccccccccccccccccccccccccccccccccccc |
haarkon | 0:242dc469ac2b | 28 | * @param Kp (float) : value of Kp |
haarkon | 0:242dc469ac2b | 29 | * @return The value of Kp (float) |
haarkon | 0:242dc469ac2b | 30 | * @note Can also be accessed using the global variable _Kp |
haarkon | 0:242dc469ac2b | 31 | */ |
haarkon | 0:242dc469ac2b | 32 | float setProportionnalValue (float KpValue); |
haarkon | 0:242dc469ac2b | 33 | |
haarkon | 0:242dc469ac2b | 34 | /** |
haarkon | 0:242dc469ac2b | 35 | * Set the Ki value |
haarkon | 0:242dc469ac2b | 36 | * |
haarkon | 0:242dc469ac2b | 37 | * @param Ki (float) : value of Ki |
haarkon | 0:242dc469ac2b | 38 | * @return The value of Ki (float) |
haarkon | 0:242dc469ac2b | 39 | * @note Can also be accessed using the global variable _Ki |
haarkon | 0:242dc469ac2b | 40 | */ |
haarkon | 0:242dc469ac2b | 41 | float setintegralValue (float KiValue); |
haarkon | 0:242dc469ac2b | 42 | |
haarkon | 0:242dc469ac2b | 43 | /** |
haarkon | 0:242dc469ac2b | 44 | * Set the Kd value |
haarkon | 0:242dc469ac2b | 45 | * |
haarkon | 0:242dc469ac2b | 46 | * @param Kd (float) : value of Kd |
haarkon | 0:242dc469ac2b | 47 | * @return The value of Kd (float) |
haarkon | 0:242dc469ac2b | 48 | * @note Can also be accessed using the global variable _Kd |
haarkon | 0:242dc469ac2b | 49 | */ |
haarkon | 0:242dc469ac2b | 50 | float setDerivativeValue (float KdValue); |
haarkon | 0:242dc469ac2b | 51 | |
haarkon | 0:242dc469ac2b | 52 | /** |
haarkon | 0:242dc469ac2b | 53 | * Set the Set point value of the speed for integrated full bridge |
haarkon | 0:242dc469ac2b | 54 | * |
haarkon | 0:242dc469ac2b | 55 | * @param left (double) : Set point value for left wheel speed (in mm/s) |
haarkon | 0:242dc469ac2b | 56 | * @param right (double) : Set point value for right wheel speed (in mm/s) |
haarkon | 0:242dc469ac2b | 57 | */ |
haarkon | 0:242dc469ac2b | 58 | void setSpeed (double left, double right); |
haarkon | 0:242dc469ac2b | 59 | |
haarkon | 0:242dc469ac2b | 60 | /** |
haarkon | 0:242dc469ac2b | 61 | * Get position of the robot (in mm for X and Y and radian for Theta) |
haarkon | 0:242dc469ac2b | 62 | * |
haarkon | 0:242dc469ac2b | 63 | * @param x (double - passed by reference) : actual position of the center of the robot, in mm, along X axis (front of the robot at startup) |
haarkon | 0:242dc469ac2b | 64 | * @param y (double - passed by reference) : actual position of the center of the robot, in mm, along Y axis (left of the robot at startup) |
haarkon | 0:242dc469ac2b | 65 | * @param theta (double - passed by reference) : radian angle between the normal to the line passing through the 2 wheels and the angle of the robot at startup |
haarkon | 0:242dc469ac2b | 66 | * @note the position is updated each time a motion computation take place (ie : each milliseconds) |
haarkon | 0:242dc469ac2b | 67 | */ |
haarkon | 0:242dc469ac2b | 68 | void getPosition (double *x, double *y, double *theta); |
haarkon | 0:242dc469ac2b | 69 | |
haarkon | 0:242dc469ac2b | 70 | /** |
haarkon | 0:242dc469ac2b | 71 | * Reset position of the robot (in mm for X and Y and radian for Theta) |
haarkon | 0:242dc469ac2b | 72 | * |
haarkon | 0:242dc469ac2b | 73 | * @note the positionis equal to 0 on all 3 axis (cannot be undone) |
haarkon | 0:242dc469ac2b | 74 | */ |
haarkon | 0:242dc469ac2b | 75 | void resetPosition (void); |
haarkon | 0:242dc469ac2b | 76 | |
haarkon | 0:242dc469ac2b | 77 | /** |
haarkon | 0:242dc469ac2b | 78 | * Get speed of the two wheels of the robot |
haarkon | 0:242dc469ac2b | 79 | * |
haarkon | 0:242dc469ac2b | 80 | * @param vG (double - passed by reference) : actual speed in mm/s of the left wheel |
haarkon | 0:242dc469ac2b | 81 | * @param vD (double - passed by reference) : actual speed in mm/s of the right wheel |
haarkon | 0:242dc469ac2b | 82 | */ |
haarkon | 0:242dc469ac2b | 83 | void getSpeed (double *vG, double *vD); |
haarkon | 0:242dc469ac2b | 84 | |
haarkon | 0:242dc469ac2b | 85 | /** |
haarkon | 0:242dc469ac2b | 86 | * Global Variable of corrective values |
haarkon | 0:242dc469ac2b | 87 | * @note works well with Kp = 2.0, Ki = 0.4 and Kd = 1 |
haarkon | 0:242dc469ac2b | 88 | */ |
haarkon | 0:242dc469ac2b | 89 | double _Kp, _Ki, _Kd; |
haarkon | 0:242dc469ac2b | 90 | /** |
haarkon | 0:242dc469ac2b | 91 | * Global Variable of speed |
haarkon | 0:242dc469ac2b | 92 | */ |
haarkon | 0:242dc469ac2b | 93 | double _SpeedG, _SpeedD; |
haarkon | 0:242dc469ac2b | 94 | /** |
haarkon | 0:242dc469ac2b | 95 | * Global Variable of measured speed |
haarkon | 0:242dc469ac2b | 96 | */ |
haarkon | 0:242dc469ac2b | 97 | double _measSpeedG, _measSpeedD; |
haarkon | 0:242dc469ac2b | 98 | /** |
haarkon | 0:242dc469ac2b | 99 | * Global Variable of measured displacement |
haarkon | 0:242dc469ac2b | 100 | */ |
haarkon | 0:242dc469ac2b | 101 | double _measDistG, _measDistD; |
haarkon | 0:242dc469ac2b | 102 | /** |
haarkon | 0:242dc469ac2b | 103 | * Global Variable to indicate that required wheel speed is unreachable (set if speed is unreachable) |
haarkon | 0:242dc469ac2b | 104 | * @note Must be cleared by user |
haarkon | 0:242dc469ac2b | 105 | */ |
haarkon | 0:242dc469ac2b | 106 | int _WheelStuckG, _WheelStuckD; |
haarkon | 0:242dc469ac2b | 107 | /** |
haarkon | 0:242dc469ac2b | 108 | * Global Variable of wheel PWM value |
haarkon | 0:242dc469ac2b | 109 | */ |
haarkon | 0:242dc469ac2b | 110 | double _PwmValueG, _PwmValueD; |
haarkon | 0:242dc469ac2b | 111 | /** |
haarkon | 0:242dc469ac2b | 112 | * Global Variable of gravity center of robot position (odometric, error increase with time) |
haarkon | 0:242dc469ac2b | 113 | */ |
haarkon | 0:242dc469ac2b | 114 | double _X, _Y, _THETA; |
haarkon | 0:242dc469ac2b | 115 | |
haarkon | 0:242dc469ac2b | 116 | /** |
haarkon | 0:242dc469ac2b | 117 | * Global Variable to indicate that required speed is unreachable (=1 if speed is unreachable) |
haarkon | 0:242dc469ac2b | 118 | * @note Must be cleared by user |
haarkon | 0:242dc469ac2b | 119 | */ |
haarkon | 0:242dc469ac2b | 120 | int RobotIsStuck; |
haarkon | 0:242dc469ac2b | 121 | |
haarkon | 0:242dc469ac2b | 122 | int main () |
haarkon | 0:242dc469ac2b | 123 | { |
haarkon | 0:242dc469ac2b | 124 | _tick.attach(&controlLoop, 0.001); |
haarkon | 0:242dc469ac2b | 125 | |
haarkon | 0:242dc469ac2b | 126 | _Lpwm.period_us(50); |
haarkon | 0:242dc469ac2b | 127 | _Lpwm = 0; |
haarkon | 0:242dc469ac2b | 128 | |
haarkon | 0:242dc469ac2b | 129 | _Rpwm.period_us(50); |
haarkon | 0:242dc469ac2b | 130 | _Rpwm = 0; |
haarkon | 0:242dc469ac2b | 131 | |
haarkon | 0:242dc469ac2b | 132 | _Kp = 2.0; |
haarkon | 0:242dc469ac2b | 133 | _Ki = 0.4; |
haarkon | 0:242dc469ac2b | 134 | _Kd = 1.0; |
haarkon | 0:242dc469ac2b | 135 | |
haarkon | 0:242dc469ac2b | 136 | pc.printf ("\n\rHelloWorld\n"); |
haarkon | 0:242dc469ac2b | 137 | led1 = 1; |
haarkon | 0:242dc469ac2b | 138 | led2 = 0; |
haarkon | 0:242dc469ac2b | 139 | disquette = 0; |
haarkon | 0:242dc469ac2b | 140 | resetPosition(); |
haarkon | 0:242dc469ac2b | 141 | wait(3); |
haarkon | 0:242dc469ac2b | 142 | |
haarkon | 0:242dc469ac2b | 143 | setSpeed (80,80); |
haarkon | 0:242dc469ac2b | 144 | while (1); |
haarkon | 0:242dc469ac2b | 145 | } |
haarkon | 0:242dc469ac2b | 146 | |
haarkon | 0:242dc469ac2b | 147 | float setProportionnalValue (float KpValue) |
haarkon | 0:242dc469ac2b | 148 | { |
haarkon | 0:242dc469ac2b | 149 | _Kp = KpValue; |
haarkon | 0:242dc469ac2b | 150 | return _Kp; |
haarkon | 0:242dc469ac2b | 151 | } |
haarkon | 0:242dc469ac2b | 152 | |
haarkon | 0:242dc469ac2b | 153 | float setintegralValue (float KiValue) |
haarkon | 0:242dc469ac2b | 154 | { |
haarkon | 0:242dc469ac2b | 155 | _Ki = KiValue; |
haarkon | 0:242dc469ac2b | 156 | return _Ki; |
haarkon | 0:242dc469ac2b | 157 | } |
haarkon | 0:242dc469ac2b | 158 | |
haarkon | 0:242dc469ac2b | 159 | float setDerivativeValue (float KdValue) |
haarkon | 0:242dc469ac2b | 160 | { |
haarkon | 0:242dc469ac2b | 161 | _Kd = KdValue; |
haarkon | 0:242dc469ac2b | 162 | return _Kd; |
haarkon | 0:242dc469ac2b | 163 | } |
haarkon | 0:242dc469ac2b | 164 | |
haarkon | 0:242dc469ac2b | 165 | void setSpeed (double left, double right) |
haarkon | 0:242dc469ac2b | 166 | { |
haarkon | 0:242dc469ac2b | 167 | _SpeedG = left; |
haarkon | 0:242dc469ac2b | 168 | _SpeedD = right; |
haarkon | 0:242dc469ac2b | 169 | } |
haarkon | 0:242dc469ac2b | 170 | |
haarkon | 0:242dc469ac2b | 171 | void getSpeed (double *vG, double *vD) |
haarkon | 0:242dc469ac2b | 172 | { |
haarkon | 0:242dc469ac2b | 173 | *vG = _measSpeedG; |
haarkon | 0:242dc469ac2b | 174 | *vD = _measSpeedD; |
haarkon | 0:242dc469ac2b | 175 | } |
haarkon | 0:242dc469ac2b | 176 | |
haarkon | 0:242dc469ac2b | 177 | void getPosition (double *x, double *y, double *theta) |
haarkon | 0:242dc469ac2b | 178 | { |
haarkon | 0:242dc469ac2b | 179 | *x = _X; |
haarkon | 0:242dc469ac2b | 180 | *y = _Y; |
haarkon | 0:242dc469ac2b | 181 | *theta = _THETA; |
haarkon | 0:242dc469ac2b | 182 | } |
haarkon | 0:242dc469ac2b | 183 | |
haarkon | 0:242dc469ac2b | 184 | void resetPosition (void) |
haarkon | 0:242dc469ac2b | 185 | { |
haarkon | 0:242dc469ac2b | 186 | _X = 0; |
haarkon | 0:242dc469ac2b | 187 | _Y = 0; |
haarkon | 0:242dc469ac2b | 188 | _THETA = 0; |
haarkon | 0:242dc469ac2b | 189 | } |
haarkon | 0:242dc469ac2b | 190 | |
haarkon | 0:242dc469ac2b | 191 | void controlLoop() |
haarkon | 0:242dc469ac2b | 192 | { |
haarkon | 0:242dc469ac2b | 193 | long PositionG, PositionD; |
haarkon | 0:242dc469ac2b | 194 | static float integralErrorG = 0, integralErrorD = 0; |
haarkon | 0:242dc469ac2b | 195 | static float oldErrorG, oldErrorD; |
haarkon | 0:242dc469ac2b | 196 | double errorG, commandeG; |
haarkon | 0:242dc469ac2b | 197 | double errorD, commandeD; |
haarkon | 0:242dc469ac2b | 198 | double meanDist, diffDist, deltaX, deltaY, deltaTheta; |
haarkon | 0:242dc469ac2b | 199 | static long oldPositionG=0, oldPositionD=0; |
haarkon | 0:242dc469ac2b | 200 | |
haarkon | 0:242dc469ac2b | 201 | PositionG = _Lencoder.GetCounter(); |
haarkon | 0:242dc469ac2b | 202 | PositionD = -_Rencoder.GetCounter(); |
haarkon | 0:242dc469ac2b | 203 | |
haarkon | 0:242dc469ac2b | 204 | // As we use mm/s for speed unit and we convert all mesure to this unit |
haarkon | 0:242dc469ac2b | 205 | |
haarkon | 0:242dc469ac2b | 206 | // Converting step to millimeters |
haarkon | 0:242dc469ac2b | 207 | _measDistG = ((double)PositionG - (double)oldPositionG) / 63.662; |
haarkon | 0:242dc469ac2b | 208 | _measDistD = ((double)PositionD - (double)oldPositionD) / 63.662; |
haarkon | 0:242dc469ac2b | 209 | |
haarkon | 0:242dc469ac2b | 210 | // Converting position to speed |
haarkon | 0:242dc469ac2b | 211 | _measSpeedG = 1000.0 * _measDistG; |
haarkon | 0:242dc469ac2b | 212 | _measSpeedD = 1000.0 * _measDistD; |
haarkon | 0:242dc469ac2b | 213 | |
haarkon | 0:242dc469ac2b | 214 | // Computing errors |
haarkon | 0:242dc469ac2b | 215 | errorG = _SpeedG - _measSpeedG; |
haarkon | 0:242dc469ac2b | 216 | errorD = _SpeedD - _measSpeedD; |
haarkon | 0:242dc469ac2b | 217 | integralErrorG += errorG; |
haarkon | 0:242dc469ac2b | 218 | integralErrorD += errorD; |
haarkon | 0:242dc469ac2b | 219 | |
haarkon | 0:242dc469ac2b | 220 | // Limiting integral error |
haarkon | 0:242dc469ac2b | 221 | if (integralErrorG > 10000) { |
haarkon | 0:242dc469ac2b | 222 | _WheelStuckG = 1; |
haarkon | 0:242dc469ac2b | 223 | integralErrorG = 10000; |
haarkon | 0:242dc469ac2b | 224 | } else { |
haarkon | 0:242dc469ac2b | 225 | if (integralErrorG < -10000) { |
haarkon | 0:242dc469ac2b | 226 | _WheelStuckG = 1; |
haarkon | 0:242dc469ac2b | 227 | integralErrorG = -10000; |
haarkon | 0:242dc469ac2b | 228 | } else { |
haarkon | 0:242dc469ac2b | 229 | _WheelStuckG = 0; |
haarkon | 0:242dc469ac2b | 230 | } |
haarkon | 0:242dc469ac2b | 231 | } |
haarkon | 0:242dc469ac2b | 232 | |
haarkon | 0:242dc469ac2b | 233 | if (integralErrorD > 10000) { |
haarkon | 0:242dc469ac2b | 234 | _WheelStuckD = 1; |
haarkon | 0:242dc469ac2b | 235 | integralErrorD = 10000; |
haarkon | 0:242dc469ac2b | 236 | } else { |
haarkon | 0:242dc469ac2b | 237 | if (integralErrorD < -10000) { |
haarkon | 0:242dc469ac2b | 238 | _WheelStuckD = 1; |
haarkon | 0:242dc469ac2b | 239 | integralErrorD = -10000; |
haarkon | 0:242dc469ac2b | 240 | } else { |
haarkon | 0:242dc469ac2b | 241 | _WheelStuckD = 0; |
haarkon | 0:242dc469ac2b | 242 | } |
haarkon | 0:242dc469ac2b | 243 | } |
haarkon | 0:242dc469ac2b | 244 | |
haarkon | 0:242dc469ac2b | 245 | // Correcting system (empiric test tells me that Kp = 4, Ki = 1 and Kd = 2, but this is probably not the good setting) |
haarkon | 0:242dc469ac2b | 246 | commandeG = _Kp * errorG + _Ki * integralErrorG + _Kd * (errorG - oldErrorG); |
haarkon | 0:242dc469ac2b | 247 | commandeD = _Kp * errorD + _Ki * integralErrorD + _Kd * (errorD - oldErrorD); |
haarkon | 0:242dc469ac2b | 248 | |
haarkon | 0:242dc469ac2b | 249 | // Convert to PWM |
haarkon | 0:242dc469ac2b | 250 | _PwmValueG = commandeG / 1300.0; |
haarkon | 0:242dc469ac2b | 251 | if (_PwmValueG > 1) _PwmValueG = 1; |
haarkon | 0:242dc469ac2b | 252 | if (_PwmValueG < -1) _PwmValueG = -1; |
haarkon | 0:242dc469ac2b | 253 | |
haarkon | 0:242dc469ac2b | 254 | if (_PwmValueG < 0) { |
haarkon | 0:242dc469ac2b | 255 | _Lpwm = -_PwmValueG; |
haarkon | 0:242dc469ac2b | 256 | _LdirA = 0; |
haarkon | 0:242dc469ac2b | 257 | _LdirB = 1; |
haarkon | 0:242dc469ac2b | 258 | } else { |
haarkon | 0:242dc469ac2b | 259 | _Lpwm = _PwmValueG; |
haarkon | 0:242dc469ac2b | 260 | _LdirA = 1; |
haarkon | 0:242dc469ac2b | 261 | _LdirB = 0; |
haarkon | 0:242dc469ac2b | 262 | } |
haarkon | 0:242dc469ac2b | 263 | |
haarkon | 0:242dc469ac2b | 264 | _PwmValueD = commandeD/1300.0; |
haarkon | 0:242dc469ac2b | 265 | if (_PwmValueD > 1) _PwmValueD = 1; |
haarkon | 0:242dc469ac2b | 266 | if (_PwmValueD < -1) _PwmValueD = -1; |
haarkon | 0:242dc469ac2b | 267 | |
haarkon | 0:242dc469ac2b | 268 | if (_PwmValueD < 0) { |
haarkon | 0:242dc469ac2b | 269 | _Rpwm = -_PwmValueD; |
haarkon | 0:242dc469ac2b | 270 | _RdirA = 1; |
haarkon | 0:242dc469ac2b | 271 | _RdirB = 0; |
haarkon | 0:242dc469ac2b | 272 | } else { |
haarkon | 0:242dc469ac2b | 273 | _Rpwm = _PwmValueD; |
haarkon | 0:242dc469ac2b | 274 | _RdirA = 0; |
haarkon | 0:242dc469ac2b | 275 | _RdirB = 1; |
haarkon | 0:242dc469ac2b | 276 | } |
haarkon | 0:242dc469ac2b | 277 | |
haarkon | 0:242dc469ac2b | 278 | //odometry (segments approx.) |
haarkon | 0:242dc469ac2b | 279 | meanDist = (_measDistG + _measDistD) / 2.0; |
haarkon | 0:242dc469ac2b | 280 | diffDist = _measDistD - _measDistG; |
haarkon | 0:242dc469ac2b | 281 | |
haarkon | 0:242dc469ac2b | 282 | deltaX = meanDist * cos (_THETA); |
haarkon | 0:242dc469ac2b | 283 | deltaY = meanDist * sin (_THETA); |
haarkon | 0:242dc469ac2b | 284 | deltaTheta = diffDist / 230; |
haarkon | 0:242dc469ac2b | 285 | |
haarkon | 0:242dc469ac2b | 286 | _X += deltaX; |
haarkon | 0:242dc469ac2b | 287 | _Y += deltaY; |
haarkon | 0:242dc469ac2b | 288 | _THETA += deltaTheta; |
haarkon | 0:242dc469ac2b | 289 | if (_THETA > 3.141592653589793) _THETA -= 6.283185307179586; |
haarkon | 0:242dc469ac2b | 290 | if (_THETA < -3.141592653589793) _THETA += 6.283185307179586; |
haarkon | 0:242dc469ac2b | 291 | |
haarkon | 0:242dc469ac2b | 292 | oldPositionG = PositionG; |
haarkon | 0:242dc469ac2b | 293 | oldPositionD = PositionD; |
haarkon | 0:242dc469ac2b | 294 | oldErrorG = errorG; |
haarkon | 0:242dc469ac2b | 295 | oldErrorD = errorD; |
haarkon | 0:242dc469ac2b | 296 | |
haarkon | 0:242dc469ac2b | 297 | pc.printf ("\rG = %5.1lf - %5.1lf = %5.1lf -> %5.1lf -> %4.2lf\n", _SpeedG, _measDistG, errorG, commandeG, _PwmValueG); |
haarkon | 0:242dc469ac2b | 298 | pc.printf ("\rD = %5.1lf - %5.1lf = %5.1lf -> %5.1lf -> %4.2lf\n", _SpeedD, _measDistD, errorD, commandeD, _PwmValueD); |
haarkon | 0:242dc469ac2b | 299 | } |