ELEC3850ProjectSynchronousMachine01 / Mbed 2 deprecated ProjectCode01

Main code of our project.

Dependencies:   TextLCD mbed PID

main.cpp@13:83cc9d66749d, 2018-10-17 (annotated)

Committer:
nicovv44
Date:
Wed Oct 17 08:58:59 2018 +0000
Revision:
13:83cc9d66749d
Parent:
12:ed2a94c17109
Child:
14:3f7e54ee1211
Both PID seem to be tunned

Who changed what in which revision?

UserRevisionLine numberNew contents of line
nicovv44 0:1f66eaf1013d 1 #include "mbed.h"
nicovv44 0:1f66eaf1013d 2 #include "TextLCD.h"
nicovv44 9:70bfbd406554 3 #include "PID.h"
nicovv44 9:70bfbd406554 4
nicovv44 13:83cc9d66749d 5 #define PIDRATE 0.2
nicovv44 9:70bfbd406554 6
nicovv44 9:70bfbd406554 7 //Kc, Ti, Td, interval
nicovv44 13:83cc9d66749d 8 PID PIDcontroller1(2.9, 1.0, 0.0, PIDRATE);//frequency PID
nicovv44 13:83cc9d66749d 9 PID PIDcontroller2(0.2, 1.0, 0.0, PIDRATE);//amplitude PID
nicovv44 0:1f66eaf1013d 10
nicovv44 0:1f66eaf1013d 11 TextLCD lcd(D2, D3, D4, D5, D6, D7); // rs, e, d4-d7
nicovv44 0:1f66eaf1013d 12 Serial pc(USBTX, USBRX); // tx, rx
nicovv44 5:e9766e0573d0 13 PwmOut pwmDC(D9);
nicovv44 5:e9766e0573d0 14 PwmOut pwmSY(D13);
nicovv44 5:e9766e0573d0 15 DigitalOut relay1(D8);
nicovv44 5:e9766e0573d0 16 DigitalOut relay2(D14);
nicovv44 5:e9766e0573d0 17 DigitalOut relay3(D11);
nicovv44 5:e9766e0573d0 18 DigitalOut relay4(D12);
nicovv44 0:1f66eaf1013d 19
nicovv44 3:a1b11dfd26f3 20 AnalogIn syncPin(A0);
nicovv44 3:a1b11dfd26f3 21 AnalogIn gridPin(A1);
nicovv44 3:a1b11dfd26f3 22 AnalogIn differentialPin(A2);
nicovv44 5:e9766e0573d0 23 AnalogIn potarDC(A3);
nicovv44 5:e9766e0573d0 24 AnalogIn potarSY(A4);
nicovv44 0:1f66eaf1013d 25
nicovv44 0:1f66eaf1013d 26 const float sqrt2 = 1.414213562;
nicovv44 0:1f66eaf1013d 27
nicovv44 0:1f66eaf1013d 28 Timeout timeout;
nicovv44 5:e9766e0573d0 29 Ticker tickerPWMDC;
nicovv44 11:b6bee52941ea 30 Ticker tickerPID;
nicovv44 0:1f66eaf1013d 31 volatile bool looping = false;
nicovv44 4:886ce7eefa6e 32 volatile bool synchronized = false;
nicovv44 4:886ce7eefa6e 33 volatile bool mainLoop = true;
nicovv44 9:70bfbd406554 34 volatile float PID1Output;
nicovv44 13:83cc9d66749d 35 volatile float PID2Output;
nicovv44 0:1f66eaf1013d 36
nicovv44 0:1f66eaf1013d 37
nicovv44 0:1f66eaf1013d 38 // ##############################################
nicovv44 0:1f66eaf1013d 39 // ########## PROTOTYPES ########################
nicovv44 0:1f66eaf1013d 40 // #############################################################################
nicovv44 0:1f66eaf1013d 41 void stopLooping(void);
nicovv44 0:1f66eaf1013d 42 float getVolageRMS(AnalogIn ana_pin);
nicovv44 0:1f66eaf1013d 43 float getVolageReadedMax(AnalogIn ana_pin);
nicovv44 0:1f66eaf1013d 44 float getFrequency(AnalogIn ana_pin);
nicovv44 4:886ce7eefa6e 45 void displayLCD(float syncRMS, float gridRMS, float syncFreq, float gridFreq);
nicovv44 5:e9766e0573d0 46 void tickerPWMDCfunction();
nicovv44 9:70bfbd406554 47 void initPID1();
nicovv44 13:83cc9d66749d 48 void initPID2();
nicovv44 9:70bfbd406554 49 void tickerPIDfunction();
nicovv44 0:1f66eaf1013d 50
nicovv44 0:1f66eaf1013d 51
nicovv44 0:1f66eaf1013d 52 // ##############################################
nicovv44 0:1f66eaf1013d 53 // ########## MAIN ##############################
nicovv44 0:1f66eaf1013d 54 // #############################################################################
nicovv44 0:1f66eaf1013d 55 int main() {
nicovv44 3:a1b11dfd26f3 56 float syncRMS, gridRMS, syncFreq, gridFreq;
nicovv44 9:70bfbd406554 57
nicovv44 13:83cc9d66749d 58 //initialise PIDcontrollers
nicovv44 9:70bfbd406554 59 initPID1();
nicovv44 13:83cc9d66749d 60 initPID2();
nicovv44 0:1f66eaf1013d 61
nicovv44 5:e9766e0573d0 62 relay1 = 1;//Relay off=1, on=0
nicovv44 5:e9766e0573d0 63 relay2 = 1;//Relay off=1, on=0
nicovv44 5:e9766e0573d0 64 relay3 = 1;//Relay off=1, on=0
nicovv44 8:a161b056971c 65 relay4 = 0;//Relay off=1, on=0
nicovv44 5:e9766e0573d0 66
nicovv44 4:886ce7eefa6e 67 while(mainLoop){
nicovv44 5:e9766e0573d0 68 pwmDC.period(0.001f);
nicovv44 13:83cc9d66749d 69 pwmDC.write(1-0.50f); //(1-duty)
nicovv44 8:a161b056971c 70 pwmSY.period(0.001f); //(1-duty)
nicovv44 13:83cc9d66749d 71 pwmSY.write(1-0.50f);
nicovv44 8:a161b056971c 72 //tickerPWMDC.attach(&tickerPWMDCfunction, 0.1);
nicovv44 3:a1b11dfd26f3 73
nicovv44 12:ed2a94c17109 74 //pc.printf("\n\nAccelerating\r\n");
nicovv44 12:ed2a94c17109 75 lcd.printf("ACCELERATING");
nicovv44 9:70bfbd406554 76 wait(5);//wait so the motor get steady state
nicovv44 11:b6bee52941ea 77 //pwmDC.write(1-0.00f); //(1-duty)
nicovv44 9:70bfbd406554 78
nicovv44 4:886ce7eefa6e 79 //manual synchronisation
nicovv44 4:886ce7eefa6e 80 while(!synchronized){
nicovv44 4:886ce7eefa6e 81 //measure and calculate desired value
nicovv44 4:886ce7eefa6e 82 syncRMS = getVolageRMS(syncPin);
nicovv44 4:886ce7eefa6e 83 gridRMS = getVolageRMS(gridPin);
nicovv44 4:886ce7eefa6e 84 syncFreq = getFrequency(syncPin);
nicovv44 4:886ce7eefa6e 85 gridFreq = getFrequency(gridPin);
nicovv44 9:70bfbd406554 86 //Update the PID process variable.
nicovv44 11:b6bee52941ea 87 PIDcontroller1.setProcessValue(syncFreq);
nicovv44 13:83cc9d66749d 88 PIDcontroller2.setProcessValue(syncRMS);
nicovv44 9:70bfbd406554 89 //Interrupt for a correct PID rate
nicovv44 13:83cc9d66749d 90 //tickerPID.attach(&tickerPIDfunction, PIDRATE);
nicovv44 4:886ce7eefa6e 91 //display values on LCD
nicovv44 4:886ce7eefa6e 92 displayLCD(syncRMS, gridRMS, syncFreq, gridFreq);
nicovv44 13:83cc9d66749d 93 //update PID values
nicovv44 11:b6bee52941ea 94 PID1Output = PIDcontroller1.compute();
nicovv44 13:83cc9d66749d 95 PID2Output = PIDcontroller2.compute();
nicovv44 13:83cc9d66749d 96 //drive PWMs with PID values
nicovv44 11:b6bee52941ea 97 pwmDC.write(1-PID1Output); //(1-duty)
nicovv44 13:83cc9d66749d 98 pwmSY.write(1-PID2Output); //(1-duty)
nicovv44 13:83cc9d66749d 99 pc.printf("PID1:%f \t syncFreq:%f \r\n", PID1Output, syncFreq);
nicovv44 13:83cc9d66749d 100 pc.printf("PID2:%f \t syncRMS:%f \r\n\n", PID2Output, syncRMS);
nicovv44 11:b6bee52941ea 101 wait(PIDRATE);
nicovv44 4:886ce7eefa6e 102 //voltage and frequency matching
nicovv44 4:886ce7eefa6e 103 if(abs(syncRMS-gridRMS)<0.5 && abs(syncFreq-gridFreq)<0.1){
nicovv44 12:ed2a94c17109 104 //pc.printf("voltage and freqency OK\r\n");
nicovv44 12:ed2a94c17109 105 lcd.locate(11,0);//(col,row)
nicovv44 12:ed2a94c17109 106 lcd.printf("V&fOK");
nicovv44 4:886ce7eefa6e 107 while(!synchronized){//phase matching loop
nicovv44 12:ed2a94c17109 108 //measure and calculate desired value
nicovv44 12:ed2a94c17109 109 syncRMS = getVolageRMS(syncPin);
nicovv44 12:ed2a94c17109 110 gridRMS = getVolageRMS(gridPin);
nicovv44 12:ed2a94c17109 111 syncFreq = getFrequency(syncPin);
nicovv44 12:ed2a94c17109 112 gridFreq = getFrequency(gridPin);
nicovv44 12:ed2a94c17109 113 //display values on LCD
nicovv44 12:ed2a94c17109 114 displayLCD(syncRMS, gridRMS, syncFreq, gridFreq);
nicovv44 4:886ce7eefa6e 115 //phase matching
nicovv44 7:1f7d87007512 116 if(getVolageReadedMax(differentialPin) < 0.050){
nicovv44 12:ed2a94c17109 117 //pc.printf("SYNCHONIZATION OK\r\n\n");
nicovv44 12:ed2a94c17109 118 lcd.locate(12,1);//(col,row)
nicovv44 12:ed2a94c17109 119 lcd.printf("SYNC");
nicovv44 5:e9766e0573d0 120 relay1 = 0;//Relay off=1, on=0 // to hear the noise
nicovv44 5:e9766e0573d0 121 relay2 = 0;//Relay off=1, on=0 // to hear the noise
nicovv44 5:e9766e0573d0 122 relay3 = 0;//Relay off=1, on=0 // to hear the noise
nicovv44 8:a161b056971c 123 relay4 = 1;//Relay off=1, on=0 // to hear the noise
nicovv44 4:886ce7eefa6e 124 synchronized = true;
nicovv44 4:886ce7eefa6e 125 mainLoop = false;
nicovv44 4:886ce7eefa6e 126 }
nicovv44 4:886ce7eefa6e 127 }
nicovv44 4:886ce7eefa6e 128 }
nicovv44 4:886ce7eefa6e 129 }
nicovv44 1:f31a46c62d10 130 }
nicovv44 1:f31a46c62d10 131
nicovv44 5:e9766e0573d0 132
nicovv44 1:f31a46c62d10 133
nicovv44 3:a1b11dfd26f3 134
nicovv44 0:1f66eaf1013d 135 }
nicovv44 0:1f66eaf1013d 136
nicovv44 0:1f66eaf1013d 137
nicovv44 0:1f66eaf1013d 138
nicovv44 0:1f66eaf1013d 139
nicovv44 0:1f66eaf1013d 140
nicovv44 0:1f66eaf1013d 141
nicovv44 0:1f66eaf1013d 142
nicovv44 0:1f66eaf1013d 143 // ##############################################
nicovv44 0:1f66eaf1013d 144 // ########## FUNCTIONS #########################
nicovv44 0:1f66eaf1013d 145 // #############################################################################
nicovv44 0:1f66eaf1013d 146 // ISR to stop loping
nicovv44 0:1f66eaf1013d 147 void stopLooping(void) {
nicovv44 0:1f66eaf1013d 148 looping = false;//looping is volatile bool
nicovv44 0:1f66eaf1013d 149 }
nicovv44 0:1f66eaf1013d 150
nicovv44 0:1f66eaf1013d 151 // #############################################################################
nicovv44 5:e9766e0573d0 152 // ISR to update pwmDC with potarDC
nicovv44 5:e9766e0573d0 153 void tickerPWMDCfunction(){
nicovv44 5:e9766e0573d0 154 float valuePotar1;
nicovv44 5:e9766e0573d0 155 float valuePotar2;
nicovv44 5:e9766e0573d0 156 valuePotar1 = potarDC.read();
nicovv44 7:1f7d87007512 157 pwmDC.write(1-valuePotar1);
nicovv44 5:e9766e0573d0 158 valuePotar2 = potarSY.read();
nicovv44 7:1f7d87007512 159 pwmSY.write(1-valuePotar2);
nicovv44 5:e9766e0573d0 160 //lcd.locate(12,0);//(col,row)
nicovv44 5:e9766e0573d0 161 //lcd.printf("%f",valuePotar);
nicovv44 5:e9766e0573d0 162 }
nicovv44 5:e9766e0573d0 163
nicovv44 9:70bfbd406554 164
nicovv44 9:70bfbd406554 165 // #############################################################################
nicovv44 9:70bfbd406554 166 // ISR to update PID
nicovv44 9:70bfbd406554 167 void tickerPIDfunction(){
nicovv44 9:70bfbd406554 168 PID1Output = PIDcontroller1.compute();
nicovv44 9:70bfbd406554 169 pwmDC.write(1-PID1Output); //(1-duty)
nicovv44 9:70bfbd406554 170 pc.printf("PID1:%f\r\n\n", PID1Output);
nicovv44 9:70bfbd406554 171 }
nicovv44 9:70bfbd406554 172
nicovv44 9:70bfbd406554 173
nicovv44 9:70bfbd406554 174 // #############################################################################
nicovv44 9:70bfbd406554 175 void initPID1(){
nicovv44 13:83cc9d66749d 176 //Input
nicovv44 13:83cc9d66749d 177 PIDcontroller1.setInputLimits(0.0, 100.0);
nicovv44 9:70bfbd406554 178 //Pwm output from 0.0 to 1.0
nicovv44 9:70bfbd406554 179 PIDcontroller1.setOutputLimits(0.0, 1.0);
nicovv44 9:70bfbd406554 180 //If there's a bias.
nicovv44 11:b6bee52941ea 181 PIDcontroller1.setBias(0.70);
nicovv44 9:70bfbd406554 182 PIDcontroller1.setMode(true);
nicovv44 9:70bfbd406554 183 //We want the process variable to be 50Hz
nicovv44 9:70bfbd406554 184 PIDcontroller1.setSetPoint(50);//50Hz
nicovv44 9:70bfbd406554 185 }
nicovv44 9:70bfbd406554 186
nicovv44 5:e9766e0573d0 187 // #############################################################################
nicovv44 13:83cc9d66749d 188 void initPID2(){
nicovv44 13:83cc9d66749d 189 //Input
nicovv44 13:83cc9d66749d 190 PIDcontroller2.setInputLimits(0.0, 25.0);
nicovv44 13:83cc9d66749d 191 //Pwm output from 0.0 to 1.0
nicovv44 13:83cc9d66749d 192 PIDcontroller2.setOutputLimits(0.0, 1.0);
nicovv44 13:83cc9d66749d 193 //If there's a bias.
nicovv44 13:83cc9d66749d 194 PIDcontroller2.setBias(0.70);
nicovv44 13:83cc9d66749d 195 PIDcontroller2.setMode(true);
nicovv44 13:83cc9d66749d 196 //We want the process variable to be 50Hz
nicovv44 13:83cc9d66749d 197 PIDcontroller2.setSetPoint(getVolageRMS(gridPin));
nicovv44 13:83cc9d66749d 198 }
nicovv44 13:83cc9d66749d 199
nicovv44 13:83cc9d66749d 200 // #############################################################################
nicovv44 0:1f66eaf1013d 201 float getVolageRMS(AnalogIn ana_pin){
nicovv44 0:1f66eaf1013d 202 float v1;//readed voltage
nicovv44 0:1f66eaf1013d 203 float v1Max = 0;//max readed voltage
nicovv44 0:1f66eaf1013d 204 float VRMS; //RMS voltage
nicovv44 0:1f66eaf1013d 205 looping = true;
nicovv44 1:f31a46c62d10 206 timeout.attach(callback(&stopLooping), 0.020);//T=20ms because f=50Hz
nicovv44 0:1f66eaf1013d 207 while(looping){
nicovv44 0:1f66eaf1013d 208 v1 = ana_pin.read()*3.3;
nicovv44 0:1f66eaf1013d 209 if(v1 > v1Max){
nicovv44 0:1f66eaf1013d 210 v1Max = v1;
nicovv44 0:1f66eaf1013d 211 }
nicovv44 0:1f66eaf1013d 212 }
nicovv44 3:a1b11dfd26f3 213 VRMS = (v1Max+0.685)*9.32/sqrt2;
nicovv44 4:886ce7eefa6e 214 //pc.printf("VRMS: %f\r\n",VRMS);
nicovv44 0:1f66eaf1013d 215 return VRMS;
nicovv44 0:1f66eaf1013d 216 }
nicovv44 0:1f66eaf1013d 217
nicovv44 0:1f66eaf1013d 218
nicovv44 0:1f66eaf1013d 219 // #############################################################################
nicovv44 0:1f66eaf1013d 220 float getVolageReadedMax(AnalogIn ana_pin){
nicovv44 0:1f66eaf1013d 221 float v1;//readed voltage
nicovv44 0:1f66eaf1013d 222 float v1Max = 0;//max readed voltage
nicovv44 0:1f66eaf1013d 223 looping = true;
nicovv44 1:f31a46c62d10 224 timeout.attach(callback(&stopLooping), 0.025);//T=25>20ms because f=50Hz
nicovv44 0:1f66eaf1013d 225 while(looping){
nicovv44 0:1f66eaf1013d 226 v1 = ana_pin.read()*3.3;
nicovv44 0:1f66eaf1013d 227 if(v1 > v1Max){
nicovv44 0:1f66eaf1013d 228 v1Max = v1;
nicovv44 0:1f66eaf1013d 229 }
nicovv44 0:1f66eaf1013d 230 }
nicovv44 0:1f66eaf1013d 231 return v1Max;
nicovv44 0:1f66eaf1013d 232 }
nicovv44 0:1f66eaf1013d 233
nicovv44 1:f31a46c62d10 234
nicovv44 0:1f66eaf1013d 235 // #############################################################################
nicovv44 0:1f66eaf1013d 236 float getFrequency(AnalogIn ana_pin){
nicovv44 0:1f66eaf1013d 237 float freq; //frequency
nicovv44 0:1f66eaf1013d 238 float maxReadedVoltage;//maximum voltage readed by the ADC
nicovv44 0:1f66eaf1013d 239 float readedVoltage;//readed voltage
nicovv44 0:1f66eaf1013d 240 int nbrRisingEdge=0;// number of rising edge detected
nicovv44 0:1f66eaf1013d 241 float T;//Periode
nicovv44 0:1f66eaf1013d 242 Timer timer;
nicovv44 0:1f66eaf1013d 243 maxReadedVoltage = getVolageReadedMax(ana_pin);
nicovv44 11:b6bee52941ea 244 //pc.printf("maxReadedVoltage: %f\r\n",maxReadedVoltage);
nicovv44 0:1f66eaf1013d 245 bool aboveLine = true;
nicovv44 11:b6bee52941ea 246 bool allowedClicTimer = false;
nicovv44 1:f31a46c62d10 247 looping = true;
nicovv44 1:f31a46c62d10 248 timeout.attach(callback(&stopLooping), 1);//try to find rising edges during 1sec max
nicovv44 1:f31a46c62d10 249 while(nbrRisingEdge<2 and looping){
nicovv44 0:1f66eaf1013d 250 readedVoltage = ana_pin.read()*3.3;
nicovv44 0:1f66eaf1013d 251 if(readedVoltage<(maxReadedVoltage/2)){//rising edge detection ready
nicovv44 0:1f66eaf1013d 252 aboveLine = false;
nicovv44 0:1f66eaf1013d 253 }
nicovv44 0:1f66eaf1013d 254 if((maxReadedVoltage/2)<readedVoltage && aboveLine==false){//rising edge detected
nicovv44 11:b6bee52941ea 255 allowedClicTimer = true;
nicovv44 0:1f66eaf1013d 256 aboveLine = true;
nicovv44 11:b6bee52941ea 257 }
nicovv44 11:b6bee52941ea 258 if((maxReadedVoltage*2/3)<readedVoltage && allowedClicTimer==true){//rising edge detected
nicovv44 11:b6bee52941ea 259 allowedClicTimer = false;
nicovv44 0:1f66eaf1013d 260 if(nbrRisingEdge==0)
nicovv44 0:1f66eaf1013d 261 timer.start();
nicovv44 0:1f66eaf1013d 262 if(nbrRisingEdge==1)
nicovv44 0:1f66eaf1013d 263 timer.stop();
nicovv44 0:1f66eaf1013d 264 nbrRisingEdge++;
nicovv44 0:1f66eaf1013d 265 }
nicovv44 11:b6bee52941ea 266
nicovv44 0:1f66eaf1013d 267 }
nicovv44 1:f31a46c62d10 268 if(nbrRisingEdge!=2){
nicovv44 1:f31a46c62d10 269 lcd.locate(13,1);
nicovv44 1:f31a46c62d10 270 lcd.printf("f!%d",nbrRisingEdge);
nicovv44 1:f31a46c62d10 271 }
nicovv44 0:1f66eaf1013d 272 T = timer.read();
nicovv44 0:1f66eaf1013d 273 freq = 1/T;
nicovv44 4:886ce7eefa6e 274 //pc.printf("T: %f\r\n",T);
nicovv44 4:886ce7eefa6e 275 //pc.printf("freq: %f\r\n\n",freq);
nicovv44 11:b6bee52941ea 276 if(looping==false)
nicovv44 11:b6bee52941ea 277 freq = 0;
nicovv44 0:1f66eaf1013d 278 return freq;
nicovv44 4:886ce7eefa6e 279 }
nicovv44 4:886ce7eefa6e 280
nicovv44 4:886ce7eefa6e 281
nicovv44 4:886ce7eefa6e 282 // #############################################################################
nicovv44 4:886ce7eefa6e 283 void displayLCD(float syncRMS, float gridRMS, float syncFreq, float gridFreq){
nicovv44 12:ed2a94c17109 284 lcd.locate(0,0);//(col,row)
nicovv44 12:ed2a94c17109 285 lcd.printf(" ");
nicovv44 9:70bfbd406554 286 lcd.locate(0,1);//(col,row)
nicovv44 9:70bfbd406554 287 lcd.printf(" ");
nicovv44 12:ed2a94c17109 288 lcd.locate(0,0);//(col,row)
nicovv44 12:ed2a94c17109 289 lcd.printf("G:%3.1f@%3.1f", gridRMS, gridFreq);
nicovv44 9:70bfbd406554 290 lcd.locate(0,1);//(col,row)
nicovv44 9:70bfbd406554 291 lcd.printf("S:%3.1f@%3.1f", syncRMS, syncFreq);
nicovv44 9:70bfbd406554 292
nicovv44 1:f31a46c62d10 293 }