main.cpp

00001 #include "mbed.h"
00002 #include "Queue.h"
00003 #include "TextLCD.h"
00004 #include "pid.h"
00005 #include <Serial.h>
00006 #include "EthernetInterface.h"
00007 
00008 //definitions
00009 typedef struct {
00010     double input;
00011 } message_t;
00012 
00013 bool useUDP = false;
00014 const int heightResolution = 1000;
00015 const int BAUDRATE = 115200;
00016 const char* host_address = "10.1.1.101";
00017 const int host_port = 7;
00018 const int cameraFPS = 60;
00019 const int displayRefreshTime = 200;
00020 const double setPoint = 0.5;
00021 const double Kp = 0.02;
00022 const double Ki = 0;
00023 const double Kd = 0;
00024 const double configStep = 0.001;
00025 const double configFastStep = configStep*10;
00026 
00027 const int simulationSeconds = 10;
00028 
00029 enum {btnRIGHT, btnUP, btnDOWN, btnLEFT, btnSELECT, btnNONE, btnSIZE};
00030 enum {menuSETPOINT, menuPID, menuSIMULATE, menuSIZE};
00031 enum {parameterKP, parameterKI, parameterKD, parameterSIZE};
00032 enum {simulSTEP, simulRAMP, simulNONE, simulSIZE};
00033 
00034 //Pins
00035 PwmOut          outPin(D3);
00036 DigitalOut      led1(LED1);
00037 //DigitalOut      led2(LED2);
00038 DigitalOut      led3(LED3);
00039 AnalogIn        buttons(A0);
00040 
00041 //Threads
00042 Thread lcdThread;
00043 Thread pidThread;
00044 Thread serialThread;
00045 Thread hmiThread;
00046 Thread setPointThread;
00047 Thread udpThread;
00048 
00049 //Global variables
00050 TextLCD lcd(D8, D9, D4, D5, D6, D7); // rs, e, d4-d7
00051 Pid* pidController;
00052 MemoryPool<message_t, 16> mpool;
00053 Queue<message_t,16> messageQueue;
00054 
00055 //Menu settings
00056 int menu = menuSETPOINT;
00057 int parameter = parameterKP;
00058 int simul = simulNONE;
00059 bool setPointSimul = false;
00060 
00061 
00062 // Network interface
00063 EthernetInterface net;
00064 
00065 //=============================Thread Methodes==================================
00066 void setPointMethode(void)
00067 {
00068     while(1) //toggle every 20 seconds between 2 setPoint values
00069     {
00070         while(!setPointSimul) Thread::wait(2000);
00071             
00072         if(setPointSimul)
00073         {
00074             pidController->setSetPoint(0.2);
00075             Thread::wait(20000);
00076             if(setPointSimul)
00077             {
00078                 pidController->setSetPoint(0.8);
00079                 Thread::wait(20000);  
00080             }
00081         } 
00082     }
00083 }
00084 void LcdMethode(void)
00085 {
00086     while(true)
00087     {
00088         double lastPwm = pidController->getLastPwm();
00089         double lastInput = pidController->getLastInput();
00090         
00091         if(menu == menuSETPOINT)
00092         {
00093             lcd.cls();
00094             lcd.printf("set   in    out");
00095             lcd.locate(0,1);
00096             lcd.printf("%d", int(1000*pidController->getSetPoint()));
00097             lcd.locate(6,1);
00098             lcd.printf("%d", int(1000*lastInput));
00099             lcd.locate(12,1);
00100             lcd.printf("%d", int(1000*lastPwm));
00101         }
00102         else if(menu == menuPID)
00103         {
00104             lcd.cls();
00105             lcd.printf(" Kp    Ki    Kd");
00106             lcd.locate(6*parameter, 0);
00107             lcd.printf("*");
00108             lcd.locate(0,1);
00109             lcd.printf("%d", int(1000*pidController->getKp()));
00110             lcd.locate(6,1);
00111             lcd.printf("%d", int(1000*pidController->getKi()));
00112             lcd.locate(12,1);
00113             lcd.printf("%d", int(1000*pidController->getKd()));
00114         }
00115         else if(menu == menuSIMULATE)
00116         {
00117             lcd.cls();
00118             if(simul == simulNONE)
00119             {
00120                 if(setPointSimul)
00121                     lcd.printf("u:STEP * d:RAMP");
00122                 else
00123                     lcd.printf("u:STEP _ d:RAMP");
00124             }
00125             else
00126                 lcd.printf("set  $in$   out");
00127             lcd.locate(0,1);
00128             lcd.printf("%d", int(1000*pidController->getSetPoint()));
00129             lcd.locate(6,1);
00130             lcd.printf("%d", int(1000*lastInput));
00131             lcd.locate(12,1);
00132             lcd.printf("%d", int(1000*lastPwm));
00133         }
00134         Thread::wait(displayRefreshTime);
00135     }
00136 }
00137 
00138 //------------------------------------------------------------------------------
00139 
00140 void PidMethode(void)
00141 {
00142     while(true)
00143     {
00144         osEvent ev = messageQueue.get(osWaitForever); 
00145         if (ev.status == osEventMessage)
00146         {
00147             message_t *message = (message_t*)ev.value.p;
00148             
00149             double input = message->input;
00150             
00151             double newPwm = pidController->getPwm(input);
00152             
00153             outPin.write(newPwm);
00154             
00155             mpool.free(message);
00156         }
00157     }   
00158 }
00159 //------------------------------------------------------------------------------
00160 
00161 void SerialMethode(void)
00162 {
00163     Serial serial(USBTX, USBRX);
00164     serial.baud(BAUDRATE);
00165     serial.format(8, SerialBase::None, 1);
00166     double input = 0;
00167     
00168     while(true)
00169     {
00170         input = double(serial.getc()+(serial.getc()<<8)) / heightResolution;
00171         message_t *message = mpool.alloc();
00172         message->input = input;
00173         messageQueue.put(message);
00174     }   
00175 }
00176 //------------------------------------------------------------------------------
00177 
00178 void UdpMethode(void)
00179 {
00180     UDPSocket socket(&net);
00181     socket.bind(host_port);
00182     SocketAddress socketAddress;
00183     socket.set_blocking(true);
00184     uint16_t packet = 0;
00185     double input = 0;
00186     while(true)
00187     {
00188         //socket.sendto(host_address, host_port, (const void*)input, sizeof(input));
00189         //input += 1;
00190         socket.recvfrom(&socketAddress, &packet, sizeof(packet));
00191         
00192         input = double(packet) / heightResolution;
00193         message_t *message = mpool.alloc();
00194         message->input = input;
00195         messageQueue.put(message);
00196         Thread::wait(300);
00197     }   
00198 }
00199 
00200 //------------------------------------------------------------------------------
00201 
00202 unsigned readButtons()
00203 {
00204         double buttonsValue = buttons.read();
00205         
00206         unsigned button = btnNONE;
00207         if(buttonsValue < 0.08) //0.000
00208             button = btnRIGHT;
00209         else if(buttonsValue < 0.28) //0.170
00210             button = btnUP;
00211         else if(buttonsValue < 0.51) //0.397
00212             button = btnDOWN;
00213         else if(buttonsValue < 0.78) //0.621
00214             button = btnLEFT;
00215         else if(buttonsValue < 0.97) //0.936
00216             button = btnSELECT;
00217         else                         //1.000
00218             button = btnNONE;
00219             
00220         return button;
00221     
00222 }
00223 //------------------------------------------------------------------------------
00224 //human-machine interface
00225 void hmiMethode(void)
00226 {
00227     
00228     unsigned button = btnNONE;
00229     bool fastChange = false;
00230     while(true)
00231     {
00232         
00233         button = readButtons();
00234         while(button == btnNONE)
00235         {
00236             button = readButtons();
00237             Thread::wait(10);
00238             fastChange = false;
00239         }
00240                
00241         double prevSetPoint = pidController->getSetPoint();
00242         double newSetPoint = prevSetPoint;
00243         
00244         if(button == btnSELECT)
00245         {
00246             menu = (menu+1) % menuSIZE;
00247             simul = simulNONE;
00248         }
00249         else if(menu == menuSETPOINT)
00250         {
00251             if(button == btnUP)
00252                 newSetPoint += fastChange ? configFastStep : configStep;
00253             else if(button == btnDOWN)
00254                 newSetPoint -= fastChange ? configFastStep : configStep;
00255             else if(button == btnLEFT)
00256                 newSetPoint -= fastChange ? 10*configFastStep : 10*configStep;
00257             else if(button == btnRIGHT)
00258                 newSetPoint += fastChange ? 10*configFastStep : 10*configStep;
00259                 
00260             pidController->setSetPoint(newSetPoint);
00261         }
00262         else if(menu == menuPID)
00263         {
00264             if(button == btnUP)
00265             {
00266                 if(parameter == parameterKP)
00267                     pidController->addKp(fastChange ? configFastStep : configStep);
00268                 else if(parameter == parameterKI)
00269                     pidController->addKi(fastChange ? configFastStep : configStep);
00270                 else if(parameter == parameterKD)
00271                     pidController->addKd(fastChange ? configFastStep : configStep);
00272             }
00273             else if(button == btnDOWN)
00274             {
00275                 if(parameter == parameterKP)
00276                     pidController->addKp(fastChange ? -configFastStep : -configStep);
00277                 else if(parameter == parameterKI)
00278                     pidController->addKi(fastChange ? -configFastStep : -configStep);
00279                 else if(parameter == parameterKD)
00280                     pidController->addKd(fastChange ? -configFastStep : -configStep);
00281             }
00282             else if(button == btnLEFT)
00283                 parameter = (parameter+1) % parameterSIZE;
00284             else if(button == btnRIGHT)
00285                 pidController->setParameters(Kp, Ki, Kd);
00286         }
00287         
00288         else if(menu == menuSIMULATE)
00289         {
00290             if(button == btnUP) //step
00291             {
00292                 simul = simulSTEP;
00293                 for(unsigned i = 0; i < cameraFPS * simulationSeconds; i++)
00294                 {
00295                     double input =  i < cameraFPS * simulationSeconds / 2 ? 0 : 1;
00296                     message_t *message = mpool.alloc();
00297                     message->input = input;
00298                     messageQueue.put(message);
00299                     
00300                     Thread::wait(1000/cameraFPS);    
00301                 }
00302                 simul = simulNONE;
00303             }
00304             else if(button == btnDOWN) //ramp
00305             {
00306                 simul = simulRAMP;
00307                 for(unsigned i = 0; i < cameraFPS * simulationSeconds; i++)
00308                 {
00309                     double input =  double(i) / ((cameraFPS * simulationSeconds)-1);
00310                     message_t *message = mpool.alloc();
00311                     message->input = input;
00312                     messageQueue.put(message);
00313                     
00314                     Thread::wait(1000/cameraFPS);    
00315                 }
00316                 simul = simulNONE;
00317             }
00318             else if(button == btnLEFT)
00319                 setPointSimul = true;
00320             else if(button == btnRIGHT)
00321                 setPointSimul = false;
00322             
00323         }
00324         unsigned repeatCount = 0;
00325         while(button != btnNONE && !fastChange)
00326         {
00327             repeatCount++;
00328             button = readButtons();
00329             Thread::wait(10);
00330             if(repeatCount > 50)
00331                 fastChange = true;
00332         }
00333         Thread::wait(100);
00334     }   
00335 }
00336 
00337 //=============================Main Thread======================================
00338 // main() runs in its own thread in the OS
00339 
00340 int main() {
00341     
00342     //led1.period(4.0f);      // 4 second period
00343     
00344     lcd.cls();
00345     lcd.printf("HELLO");
00346     
00347     Thread::wait(2000);
00348     if(readButtons() == btnDOWN)
00349         useUDP = true;
00350     
00351     if(useUDP)
00352     {
00353         lcd.cls();
00354         lcd.printf("Connecting...");
00355         net.connect();
00356 
00357         // Show the network address
00358         const char *ip = net.get_ip_address();
00359         lcd.cls();
00360         lcd.printf("%s", ip ? ip : "No IP");
00361         if(!ip)
00362             while(1); //terminate program
00363             
00364         Thread::wait(5000);
00365         udpThread.start(UdpMethode);
00366     }
00367     else
00368         serialThread.start(SerialMethode);
00369     
00370     pidController = new Pid(Kp, Ki, Kd);
00371     pidController->setSetPoint(setPoint);
00372     
00373     lcdThread.start(LcdMethode);
00374     pidThread.start(PidMethode);
00375     hmiThread.start(hmiMethode);
00376     setPointThread.start(setPointMethode);
00377     
00378     while (true) {
00379         Thread::wait(1000);
00380         led3 != led3;
00381     }
00382 }
00383 
00384 //==============================================================================
00385 
00386 
00387