FRC - Hackathon / Mbed 2 deprecated TestMot

Programme de test des fonctions d'asservissement (vue en interne)

Dependencies:   Encoder_Nucleo_16_bits mbed

main.cpp@0:242dc469ac2b, 2018-06-06 (annotated)

Committer:
haarkon
Date:
Wed Jun 06 20:53:45 2018 +0000
Revision:
0:242dc469ac2b
Programme de test des fonctions d'asservissement (en interne)

Who changed what in which revision?

UserRevisionLine numberNew 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
haarkon 0:242dc469ac2b 27 *
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 }