ELEC3850ProjectSynchronousMachine01 / Mbed 2 deprecated ProjectCode01

Main code of our project.

Dependencies:   TextLCD mbed PID

main.cpp@12:ed2a94c17109, 2018-10-17 (annotated)

Committer:
nicovv44
Date:
Wed Oct 17 07:54:42 2018 +0000
Revision:
12:ed2a94c17109
Parent:
11:b6bee52941ea
Child:
13:83cc9d66749d
Display is better

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