Team_temp / VT1_domc

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed-dev

Fork of Adafruit9-DOf_AHRS_Regler_Discrete by RT-Labor IMS

Source/main.cpp@1:8c4f93e10af3, 2017-10-19 (annotated)

Committer:
domanpie
Date:
Thu Oct 19 11:50:04 2017 +0000
Revision:
1:8c4f93e10af3
Parent:
0:772bf4786416
Child:
3:bd353b8184cc
V1.0; domc

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bmanga95 0:772bf4786416 1 #include "mbed.h"
bmanga95 0:772bf4786416 2 #define _MBED_
bmanga95 0:772bf4786416 3 #include "Adafruit_9DOF.h"
bmanga95 0:772bf4786416 4 #include "Serial_base.h"
domanpie 1:8c4f93e10af3 5 #include "MadgwickAHRS.h"
bmanga95 0:772bf4786416 6
domanpie 1:8c4f93e10af3 7 /**
domanpie 1:8c4f93e10af3 8 * Defines
domanpie 1:8c4f93e10af3 9 */
domanpie 1:8c4f93e10af3 10 #define PI 3.1415926536
domanpie 1:8c4f93e10af3 11
domanpie 1:8c4f93e10af3 12 /* Choose PID-T1 parameters VELOCITY Controller */
domanpie 1:8c4f93e10af3 13 //P-Part
domanpie 1:8c4f93e10af3 14 #define P_gain 1.666666666666668//2.5
domanpie 1:8c4f93e10af3 15 #define P_b01 1.0
domanpie 1:8c4f93e10af3 16 #define P_b11 -0.843146028951280
domanpie 1:8c4f93e10af3 17 #define P_a11 -0.738576714918798
domanpie 1:8c4f93e10af3 18 //I-Part
domanpie 1:8c4f93e10af3 19 #define I_gain 0.050505050505053
domanpie 1:8c4f93e10af3 20 #define I_b11 1.0
domanpie 1:8c4f93e10af3 21 #define I_a11 -1.0
domanpie 1:8c4f93e10af3 22 //Pre-Gain
domanpie 1:8c4f93e10af3 23 #define k 0.053348382301167*2
domanpie 1:8c4f93e10af3 24
domanpie 1:8c4f93e10af3 25 /* Choose P parameter POSITION Controller */
domanpie 1:8c4f93e10af3 26 #define KpPosition 25*1.5
domanpie 1:8c4f93e10af3 27
domanpie 1:8c4f93e10af3 28 /* Choose setposition for the gimbal */
domanpie 1:8c4f93e10af3 29 #define SOLLPOSITION 0 // 30° = -0.523598775598299 //[rad]
bmanga95 0:772bf4786416 30
bmanga95 0:772bf4786416 31
bmanga95 0:772bf4786416 32 /* Assign a unique ID to the sensors */
domanpie 1:8c4f93e10af3 33 //This board/chip uses I2C 7-bit addresses 0x19 & 0x1E & 0x6B
bmanga95 0:772bf4786416 34
bmanga95 0:772bf4786416 35 Adafruit_9DOF dof = Adafruit_9DOF();
bmanga95 0:772bf4786416 36
bmanga95 0:772bf4786416 37 Adafruit_LSM303_Accel_Unified accel = Adafruit_LSM303_Accel_Unified(30301);
bmanga95 0:772bf4786416 38
bmanga95 0:772bf4786416 39 Adafruit_LSM303_Mag_Unified mag = Adafruit_LSM303_Mag_Unified(30302);
bmanga95 0:772bf4786416 40
domanpie 1:8c4f93e10af3 41 Adafruit_L3GD20_Unified gyro = Adafruit_L3GD20_Unified(30303);
bmanga95 0:772bf4786416 42
domanpie 1:8c4f93e10af3 43 //Debug/Communication with PC through
domanpie 1:8c4f93e10af3 44 Serial pc(PA_0, PA_1, 115200); //921600; 115200
domanpie 1:8c4f93e10af3 45 Serial serial1(USBTX, USBRX, 9600);
domanpie 1:8c4f93e10af3 46
domanpie 1:8c4f93e10af3 47 // Magnetometer correction
domanpie 1:8c4f93e10af3 48 // mounted in gimbal
domanpie 1:8c4f93e10af3 49
domanpie 1:8c4f93e10af3 50 //Cal for Nickachse
domanpie 1:8c4f93e10af3 51 //double RotMag [3][3] = {{0.947568, -0.0262851, 0.0355492},
domanpie 1:8c4f93e10af3 52 // {-0.0262851, 0.98413, 0.014105},
domanpie 1:8c4f93e10af3 53 // {0.0355492, 0.014105, 0.970769}};
domanpie 1:8c4f93e10af3 54
domanpie 1:8c4f93e10af3 55 //double offsetsMag [3] = { -22.1205, -13.9738, -2.55159 };
domanpie 1:8c4f93e10af3 56
domanpie 1:8c4f93e10af3 57 double offsetsMag[3] = {23.5297, -7.51693, 17.5943};
domanpie 1:8c4f93e10af3 58 double RotMag[3][3] = {{0.678312, -0.0371453, 0.0139236}, {-0.0371453, 0.993299, 0.0229827}, {0.0139236, 0.0229827, 0.809984}};
domanpie 1:8c4f93e10af3 59
domanpie 1:8c4f93e10af3 60 //double gainsMag [3] = { 39.1659, 38.1534, 35.7245 };
domanpie 1:8c4f93e10af3 61 //double refMag = 47.9824; //nT
domanpie 1:8c4f93e10af3 62
domanpie 1:8c4f93e10af3 63 //Accelerometer correction
domanpie 1:8c4f93e10af3 64
domanpie 1:8c4f93e10af3 65 double RotAccel [3][3] = { {-0.1251, -0.7810, -0.6118},
domanpie 1:8c4f93e10af3 66 { 0.0409, 0.6121, -0.7879},
domanpie 1:8c4f93e10af3 67 { 0.9913, -0.1238, -0.0466} };
domanpie 1:8c4f93e10af3 68
domanpie 1:8c4f93e10af3 69
domanpie 1:8c4f93e10af3 70 double offsetsAccel [3] = { -0.215747, 0.509946, -0.235360 };
domanpie 1:8c4f93e10af3 71 double gainsAccel [3] = { 0.978548, 0.976637, 0.939921 };
domanpie 1:8c4f93e10af3 72
domanpie 1:8c4f93e10af3 73
domanpie 1:8c4f93e10af3 74 //Gyrometer correction
domanpie 1:8c4f93e10af3 75 double offsetsGyro [3] = { -0.0107709, 0.00729576, 0.0225833 };
bmanga95 0:772bf4786416 76
domanpie 1:8c4f93e10af3 77 //Filter for orientation estimation
domanpie 1:8c4f93e10af3 78 Madgwick filter;
domanpie 1:8c4f93e10af3 79
domanpie 1:8c4f93e10af3 80 DigitalIn CPUEnable(D11);
domanpie 1:8c4f93e10af3 81 DigitalOut motorEnable(D12, 0);
domanpie 1:8c4f93e10af3 82 PwmOut SollStrom(PB_3); //in [A]
domanpie 1:8c4f93e10af3 83
domanpie 1:8c4f93e10af3 84 //Timer for controller interrupt
domanpie 1:8c4f93e10af3 85 // the controll loop will be atached to the timer interrupt
domanpie 1:8c4f93e10af3 86 Ticker ControllerLoopTimer;
domanpie 1:8c4f93e10af3 87 void updateControllers(void);
domanpie 1:8c4f93e10af3 88
domanpie 1:8c4f93e10af3 89 //Sensor events, contains the last read sensor values
domanpie 1:8c4f93e10af3 90 sensors_event_t mag_event;
domanpie 1:8c4f93e10af3 91 sensors_event_t accel_event;
domanpie 1:8c4f93e10af3 92 sensors_event_t gyro_event;
bmanga95 0:772bf4786416 93
domanpie 1:8c4f93e10af3 94 //Sensor values after the calibration/correction is applied, (x,y,z)
domanpie 1:8c4f93e10af3 95 double CorrectedMagnetometer[3];
domanpie 1:8c4f93e10af3 96 double CorrectedAccelerometer[3];
domanpie 1:8c4f93e10af3 97 double CorrectedGyrometer[3];
domanpie 1:8c4f93e10af3 98
domanpie 1:8c4f93e10af3 99 //"User Interface", Start/Stop controller
domanpie 1:8c4f93e10af3 100 DigitalIn SystemEnable(USER_BUTTON);
domanpie 1:8c4f93e10af3 101 //Status LED, on when controller on
domanpie 1:8c4f93e10af3 102 DigitalOut SystemEnableLED(LED3, 0);
domanpie 1:8c4f93e10af3 103
domanpie 1:8c4f93e10af3 104 //variables for loop time estimation
domanpie 1:8c4f93e10af3 105 uint32_t LoopCounter = 0;
domanpie 1:8c4f93e10af3 106 Timer t;
domanpie 1:8c4f93e10af3 107 float TotalTime;
domanpie 1:8c4f93e10af3 108
domanpie 1:8c4f93e10af3 109 //maximaler Fehler
domanpie 1:8c4f93e10af3 110 float eMaxPos = 0;
domanpie 1:8c4f93e10af3 111 float eMaxNeg = 0;
bmanga95 0:772bf4786416 112
bmanga95 0:772bf4786416 113 /**************************************************************************
bmanga95 0:772bf4786416 114 /
bmanga95 0:772bf4786416 115 /
bmanga95 0:772bf4786416 116 @brief Initialises all the sensors used by this example
bmanga95 0:772bf4786416 117
bmanga95 0:772bf4786416 118 /
bmanga95 0:772bf4786416 119 **************************************************************************/
bmanga95 0:772bf4786416 120
bmanga95 0:772bf4786416 121 void initSensors()
bmanga95 0:772bf4786416 122 {
bmanga95 0:772bf4786416 123
bmanga95 0:772bf4786416 124 if(!accel.begin())
bmanga95 0:772bf4786416 125 {
bmanga95 0:772bf4786416 126
bmanga95 0:772bf4786416 127 /* There was a problem detecting the LSM303 ... check your connections */
bmanga95 0:772bf4786416 128
domanpie 1:8c4f93e10af3 129 pc.printf(("Ooops, no LSM303 detected ... Check your wiring!"));
bmanga95 0:772bf4786416 130
bmanga95 0:772bf4786416 131 while(1);
bmanga95 0:772bf4786416 132
bmanga95 0:772bf4786416 133 }
bmanga95 0:772bf4786416 134 if(!mag.begin())
bmanga95 0:772bf4786416 135 {
bmanga95 0:772bf4786416 136
bmanga95 0:772bf4786416 137 /* There was a problem detecting the LSM303 ... check your connections */
bmanga95 0:772bf4786416 138
domanpie 1:8c4f93e10af3 139 pc.printf("Ooops, no LSM303 detected ... Check your wiring!");
domanpie 1:8c4f93e10af3 140
domanpie 1:8c4f93e10af3 141 while(1);
domanpie 1:8c4f93e10af3 142
domanpie 1:8c4f93e10af3 143 }
domanpie 1:8c4f93e10af3 144
domanpie 1:8c4f93e10af3 145 gyroRange_t rng = GYRO_RANGE_2000DPS; //set gyro range to +/-2000dps
domanpie 1:8c4f93e10af3 146
domanpie 1:8c4f93e10af3 147 if(!gyro.begin(rng))
domanpie 1:8c4f93e10af3 148 {
domanpie 1:8c4f93e10af3 149
domanpie 1:8c4f93e10af3 150 /* There was a problem detecting the LSM303 ... check your connections */
domanpie 1:8c4f93e10af3 151
domanpie 1:8c4f93e10af3 152 pc.printf("Ooops, no L3GD20 detected ... Check your wiring!");
bmanga95 0:772bf4786416 153
bmanga95 0:772bf4786416 154 while(1);
bmanga95 0:772bf4786416 155
bmanga95 0:772bf4786416 156 }
bmanga95 0:772bf4786416 157
bmanga95 0:772bf4786416 158 }
domanpie 1:8c4f93e10af3 159 /**************************************************************************
bmanga95 0:772bf4786416 160
domanpie 1:8c4f93e10af3 161 @brief system initialisation
bmanga95 0:772bf4786416 162
bmanga95 0:772bf4786416 163
domanpie 1:8c4f93e10af3 164 **************************************************************************/
bmanga95 0:772bf4786416 165
domanpie 1:8c4f93e10af3 166 void setup(void)
domanpie 1:8c4f93e10af3 167 {
domanpie 1:8c4f93e10af3 168 pc.printf(("Adafruit 9 DOF Pitch/Roll/Heading Example \n"));
domanpie 1:8c4f93e10af3 169 pc.printf("\n");
bmanga95 0:772bf4786416 170
bmanga95 0:772bf4786416 171 /* Initialise the sensors */
bmanga95 0:772bf4786416 172 initSensors();
bmanga95 0:772bf4786416 173
bmanga95 0:772bf4786416 174 }
bmanga95 0:772bf4786416 175
bmanga95 0:772bf4786416 176
domanpie 1:8c4f93e10af3 177 int main()
domanpie 1:8c4f93e10af3 178 {
domanpie 1:8c4f93e10af3 179 //init system
domanpie 1:8c4f93e10af3 180 setup();
domanpie 1:8c4f93e10af3 181 //set PWM frequncy to 1 kHZ
domanpie 1:8c4f93e10af3 182 SollStrom.period(0.001);
domanpie 1:8c4f93e10af3 183 //set Current to 0 Ampère
domanpie 1:8c4f93e10af3 184 SollStrom.write(0.5);
domanpie 1:8c4f93e10af3 185 //enable motor
domanpie 1:8c4f93e10af3 186 motorEnable.write(1);
domanpie 1:8c4f93e10af3 187
domanpie 1:8c4f93e10af3 188 //wait for start (user button press)
domanpie 1:8c4f93e10af3 189 while( SystemEnable )
domanpie 1:8c4f93e10af3 190 {
domanpie 1:8c4f93e10af3 191 serial1.printf("Wait for system enable! \r\n");
domanpie 1:8c4f93e10af3 192 }
domanpie 1:8c4f93e10af3 193 while( !SystemEnable ){}
domanpie 1:8c4f93e10af3 194 //attach controller loop to timer interrupt
domanpie 1:8c4f93e10af3 195 ControllerLoopTimer.attach(&updateControllers, 0.003030303030303f); //Assume Ts = 1/330;
domanpie 1:8c4f93e10af3 196 //controller on -> LED on
domanpie 1:8c4f93e10af3 197 SystemEnableLED = 1;
domanpie 1:8c4f93e10af3 198 //start timer for sample time estimation
domanpie 1:8c4f93e10af3 199 t.start();
domanpie 1:8c4f93e10af3 200 //do as long user button d'ont get pressed
domanpie 1:8c4f93e10af3 201 while ( SystemEnable )
domanpie 1:8c4f93e10af3 202 {
domanpie 1:8c4f93e10af3 203 //read raw sensor data
domanpie 1:8c4f93e10af3 204 mag.getEvent(&mag_event);
domanpie 1:8c4f93e10af3 205 accel.getEvent(&accel_event);
domanpie 1:8c4f93e10af3 206 gyro.getEvent(&gyro_event);
domanpie 1:8c4f93e10af3 207
domanpie 1:8c4f93e10af3 208 //Magnetometer correction
domanpie 1:8c4f93e10af3 209 mag_event.magnetic.x = ((double)mag_event.magnetic.x) - offsetsMag[0];
domanpie 1:8c4f93e10af3 210 mag_event.magnetic.y = ((double)mag_event.magnetic.y) - offsetsMag[1];
domanpie 1:8c4f93e10af3 211 mag_event.magnetic.z = ((double)mag_event.magnetic.z) - offsetsMag[2];
domanpie 1:8c4f93e10af3 212
domanpie 1:8c4f93e10af3 213 CorrectedMagnetometer[0] = ((double)mag_event.magnetic.x)*RotMag[0][0] + ((double)mag_event.magnetic.y)*RotMag[0][1] + ((double)mag_event.magnetic.z)*RotMag[0][2];
domanpie 1:8c4f93e10af3 214 CorrectedMagnetometer[1] = ((double)mag_event.magnetic.x)*RotMag[1][0] + ((double)mag_event.magnetic.y)*RotMag[1][1] + ((double)mag_event.magnetic.z)*RotMag[1][2];
domanpie 1:8c4f93e10af3 215 CorrectedMagnetometer[2] = ((double)mag_event.magnetic.x)*RotMag[2][0] + ((double)mag_event.magnetic.y)*RotMag[2][1] + ((double)mag_event.magnetic.z)*RotMag[2][2];
domanpie 1:8c4f93e10af3 216
domanpie 1:8c4f93e10af3 217
domanpie 1:8c4f93e10af3 218 //Accelerometer correction
domanpie 1:8c4f93e10af3 219 CorrectedAccelerometer[0] = ((double)accel_event.acceleration.x) - offsetsAccel[0];
domanpie 1:8c4f93e10af3 220 CorrectedAccelerometer[1] = ((double)accel_event.acceleration.y) - offsetsAccel[1];
domanpie 1:8c4f93e10af3 221 CorrectedAccelerometer[2] = ((double)accel_event.acceleration.z) - offsetsAccel[2];
domanpie 1:8c4f93e10af3 222
domanpie 1:8c4f93e10af3 223 CorrectedAccelerometer[0] = CorrectedAccelerometer[0] * gainsAccel[0];
domanpie 1:8c4f93e10af3 224 CorrectedAccelerometer[1] = CorrectedAccelerometer[1] * gainsAccel[1];
domanpie 1:8c4f93e10af3 225 CorrectedAccelerometer[2] = CorrectedAccelerometer[2] * gainsAccel[2];
domanpie 1:8c4f93e10af3 226
domanpie 1:8c4f93e10af3 227
domanpie 1:8c4f93e10af3 228 //Gyrometer correction
domanpie 1:8c4f93e10af3 229 CorrectedGyrometer[0] = ((double)gyro_event.gyro.x) - offsetsGyro[0];
domanpie 1:8c4f93e10af3 230 CorrectedGyrometer[1] = ((double)gyro_event.gyro.y) - offsetsGyro[1];
domanpie 1:8c4f93e10af3 231 CorrectedGyrometer[2] = ((double)gyro_event.gyro.z) - offsetsGyro[2];
domanpie 1:8c4f93e10af3 232
domanpie 1:8c4f93e10af3 233 //Perform Madgwick-filter update
domanpie 1:8c4f93e10af3 234 filter.update( CorrectedGyrometer[0]/ PI * 180, -CorrectedGyrometer[1]/ PI * 180, -CorrectedGyrometer[2]/ PI * 180 ,
domanpie 1:8c4f93e10af3 235 -CorrectedAccelerometer[0] , CorrectedAccelerometer[1] , CorrectedAccelerometer[2] ,
domanpie 1:8c4f93e10af3 236 CorrectedMagnetometer[0] , -CorrectedMagnetometer[1], -CorrectedMagnetometer[2]);
domanpie 1:8c4f93e10af3 237
domanpie 1:8c4f93e10af3 238 LoopCounter++;
domanpie 1:8c4f93e10af3 239 if(LoopCounter == 12000)
domanpie 1:8c4f93e10af3 240 {
domanpie 1:8c4f93e10af3 241 eMaxPos = 0;
domanpie 1:8c4f93e10af3 242 eMaxNeg = 0;
domanpie 1:8c4f93e10af3 243 serial1.printf("Error reseted");
domanpie 1:8c4f93e10af3 244 }
domanpie 1:8c4f93e10af3 245 //serial1.printf("Roll: %f Grad \r\n",filter.getRoll());
domanpie 1:8c4f93e10af3 246
domanpie 1:8c4f93e10af3 247 }
domanpie 1:8c4f93e10af3 248 //Stop timer
domanpie 1:8c4f93e10af3 249 TotalTime = t.read();
domanpie 1:8c4f93e10af3 250 //disable motor
domanpie 1:8c4f93e10af3 251 motorEnable.write(0);
domanpie 1:8c4f93e10af3 252 //set Current to 0 Ampère
domanpie 1:8c4f93e10af3 253 SollStrom.write(0.5);
domanpie 1:8c4f93e10af3 254 //Switch controller off
domanpie 1:8c4f93e10af3 255 ControllerLoopTimer.detach();
domanpie 1:8c4f93e10af3 256 //controller off -> LED off
domanpie 1:8c4f93e10af3 257 SystemEnableLED = 0;
domanpie 1:8c4f93e10af3 258 //Print mean loop time ->sample frequency of the madgwick-filter
domanpie 1:8c4f93e10af3 259 serial1.printf("Mean loop time: %f [s] \r\n", TotalTime/LoopCounter);
domanpie 1:8c4f93e10af3 260 serial1.printf("Max pos Fehler: %f [°] \r\n", eMaxPos/PI*180);
domanpie 1:8c4f93e10af3 261 serial1.printf("Max neg Fehler: %f [°] \r\n", eMaxNeg/PI*180);
domanpie 1:8c4f93e10af3 262 serial1.printf("Finished!");
domanpie 1:8c4f93e10af3 263 }
domanpie 1:8c4f93e10af3 264
domanpie 1:8c4f93e10af3 265
domanpie 1:8c4f93e10af3 266
bmanga95 0:772bf4786416 267 /**************************************************************************
bmanga95 0:772bf4786416 268 /
bmanga95 0:772bf4786416 269 /
domanpie 1:8c4f93e10af3 270 @brief Regler (Geschwindigkeits- und Positionsregler)
bmanga95 0:772bf4786416 271
domanpie 1:8c4f93e10af3 272 /
domanpie 1:8c4f93e10af3 273 **************************************************************************/
domanpie 1:8c4f93e10af3 274 //-----------------------------------------------------------------------//
domanpie 1:8c4f93e10af3 275 // //
domanpie 1:8c4f93e10af3 276 // //
domanpie 1:8c4f93e10af3 277 // controller (PID-T1) //
domanpie 1:8c4f93e10af3 278 //ThetaDes[rad] ____ ____/ //
domanpie 1:8c4f93e10af3 279 // ------>O--->| Kx |----->O---->| Cv |---> I_des [A] //
domanpie 1:8c4f93e10af3 280 // ^- ---- ^- ---- //
domanpie 1:8c4f93e10af3 281 // | Pos.Cntrl. | Speed Cntrl. //
domanpie 1:8c4f93e10af3 282 // | | //
domanpie 1:8c4f93e10af3 283 // | | //
domanpie 1:8c4f93e10af3 284 // | +---- Speed Omega [rad/s] //
domanpie 1:8c4f93e10af3 285 // | //
domanpie 1:8c4f93e10af3 286 // +--------------------- Position Theta [rad] //
domanpie 1:8c4f93e10af3 287 // //
domanpie 1:8c4f93e10af3 288 //-----------------------------------------------------------------------//
bmanga95 0:772bf4786416 289
domanpie 1:8c4f93e10af3 290 // Cv is split into a prop. and an integrating part (limiting, windup)
domanpie 1:8c4f93e10af3 291 // e ___ ____
domanpie 1:8c4f93e10af3 292 // ---->| k |---+------>| Cp |--------->O---->
domanpie 1:8c4f93e10af3 293 // --- | ---- ^
domanpie 1:8c4f93e10af3 294 // | ____ ________ |
domanpie 1:8c4f93e10af3 295 // +->| k_I|->| z/(z-1)|---+
domanpie 1:8c4f93e10af3 296 // ---- -------- \
domanpie 1:8c4f93e10af3 297 // Integrator
domanpie 1:8c4f93e10af3 298 //
domanpie 1:8c4f93e10af3 299 // the anti-windup loop is not shon in the diagram above! (see documentation
domanpie 1:8c4f93e10af3 300 // for detail)
domanpie 1:8c4f93e10af3 301 void updateControllers(void)
domanpie 1:8c4f93e10af3 302 {
domanpie 1:8c4f93e10af3 303 static float ek_1 = 0;
domanpie 1:8c4f93e10af3 304 static float ukP_1 = 0;
domanpie 1:8c4f93e10af3 305 static float ukI_1 = 0;
domanpie 1:8c4f93e10af3 306
domanpie 1:8c4f93e10af3 307 float ePosition = SOLLPOSITION - filter.getRollRadians();
domanpie 1:8c4f93e10af3 308
domanpie 1:8c4f93e10af3 309 //Positionsregler
domanpie 1:8c4f93e10af3 310 float stellgroessePosition = KpPosition*(ePosition);
domanpie 1:8c4f93e10af3 311 if( ePosition > eMaxPos)
domanpie 1:8c4f93e10af3 312 {
domanpie 1:8c4f93e10af3 313 eMaxPos = ePosition;
domanpie 1:8c4f93e10af3 314 }
domanpie 1:8c4f93e10af3 315 if( ePosition < eMaxNeg )
domanpie 1:8c4f93e10af3 316 {
domanpie 1:8c4f93e10af3 317 eMaxNeg = ePosition;
domanpie 1:8c4f93e10af3 318 }
bmanga95 0:772bf4786416 319
domanpie 1:8c4f93e10af3 320 //Geschwindigkeitsregler
domanpie 1:8c4f93e10af3 321 float ek = k*(stellgroessePosition - CorrectedGyrometer[0]);
domanpie 1:8c4f93e10af3 322 //P-part
domanpie 1:8c4f93e10af3 323 float ukP = P_gain*(P_b01*ek + P_b11*ek_1) - P_a11*ukP_1;
domanpie 1:8c4f93e10af3 324 //I-Part
domanpie 1:8c4f93e10af3 325 float stellgroesseStrom = ukP + ukI_1;
domanpie 1:8c4f93e10af3 326 float ukI = I_gain*I_b11*ek_1 - I_a11*ukI_1;
bmanga95 0:772bf4786416 327
domanpie 1:8c4f93e10af3 328 //Limit +/- 0.5 Ampère
domanpie 1:8c4f93e10af3 329 float uSaturated;
domanpie 1:8c4f93e10af3 330 if(stellgroesseStrom > 0.5)
domanpie 1:8c4f93e10af3 331 {
domanpie 1:8c4f93e10af3 332 SollStrom.write( 0.8999999 );
domanpie 1:8c4f93e10af3 333 uSaturated = 0.5;
domanpie 1:8c4f93e10af3 334 }
domanpie 1:8c4f93e10af3 335 else if( stellgroesseStrom < -0.5 )
domanpie 1:8c4f93e10af3 336 {
domanpie 1:8c4f93e10af3 337 SollStrom.write( 0.1000001 );
domanpie 1:8c4f93e10af3 338 uSaturated = -0.5;
domanpie 1:8c4f93e10af3 339 }
domanpie 1:8c4f93e10af3 340 else
domanpie 1:8c4f93e10af3 341 {
domanpie 1:8c4f93e10af3 342 SollStrom.write(0.5f+stellgroesseStrom*0.4f/0.5f);
domanpie 1:8c4f93e10af3 343 uSaturated = stellgroesseStrom;
domanpie 1:8c4f93e10af3 344 }
domanpie 1:8c4f93e10af3 345
domanpie 1:8c4f93e10af3 346 //Anti-Windup (back calculation)
domanpie 1:8c4f93e10af3 347 float deltaI = ( -stellgroesseStrom + uSaturated );
domanpie 1:8c4f93e10af3 348 ukI_1 = ukI + deltaI*I_gain;
domanpie 1:8c4f93e10af3 349 ek_1 = ek;
domanpie 1:8c4f93e10af3 350 ukP_1 = ukP;
bmanga95 0:772bf4786416 351
bmanga95 0:772bf4786416 352
bmanga95 0:772bf4786416 353 }