ELEC3850ProjectSynchronousMachine01 / Mbed 2 deprecated ProjectCode01

Main code of our project.

Dependencies:   TextLCD mbed PID

main.cpp@14:3f7e54ee1211, 2018-10-17 (annotated)

Committer:
nicovv44
Date:
Wed Oct 17 09:10:32 2018 +0000
Revision:
14:3f7e54ee1211
Parent:
13:83cc9d66749d
Child:
15:e1a76426fb6a
Work with user button.

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