main.cpp

00001 #include "mbed.h"
00002 #include "rtos.h"
00003 #include "EthernetInterface.h"
00004 #include "HCSR04.h"
00005 #include "Servo.h"
00006 #include "PID.h"
00007 #include <string>
00008 #include <iostream>
00009 #include <sstream>
00010 
00011 //#define SERIAL
00012 #define BIAS 0.5
00013 #define ETHERNET
00014 
00015 #define SAMPLE_RATE 1 //sample rate in miliseconds
00016 
00017 enum status { IDLE, ADJUSTING, STABLE };
00018 
00019 status statusFlag; //flag to determine behavior
00020 
00021 DigitalOut ledR(LED1);
00022 DigitalOut ledG(LED2);
00023 DigitalOut ledB(LED3);
00024 
00025 HCSR04 ultrassonicSensor(PTC2, PTC3);
00026 Servo motor(PTA2);
00027 float motorPos = 0;
00028 int isRunning = 0;
00029 
00030 //Kc, Ti, Td, interval
00031 //PID controller(0.3461, 21.42, 5.26, 0.001);
00032 //PID controller(1.5, 0, 2, 0.001);
00033 //PID controller(-1.1, 0, 1.7, 0.001);//ok
00034 PID controller(-0.9344, 0, 1.733, 0.001);
00035 
00036 InterruptIn sw2(SW2);
00037 void sw2Callback()
00038 {
00039     motorPos=0+BIAS;
00040     isRunning = !isRunning;
00041 }
00042 InterruptIn sw3(SW3);
00043 void sw3Callback()
00044 {
00045     /*if(motorPos>0)
00046         motorPos-=(float)0.1;*/
00047 }
00048 
00049 Thread ledSwitchThread;
00050 Thread serialOutThread;
00051 Thread controlSystemThread;
00052 
00053 #ifdef ETHERNET
00054 Thread ethernetSendThread;
00055 Thread ethernetReceiveThread;
00056 Thread ethernetKeepAliveThread;
00057 #endif
00058 
00059 float ballDistance = 0.0;
00060 float setpoint = 15;
00061 
00062 #ifdef ETHERNET
00063 
00064 EthernetInterface eth;
00065 TCPSocketConnection sock;
00066 
00067 void ethernetKeepAlive()
00068 {
00069 #ifdef SERIAL
00070     printf("ethernetKeepAliveThread started\n");
00071 #endif
00072     std::stringstream ss;
00073 
00074     while(true) {
00075         if(sock.is_connected()) {
00076             sock.send_all("",1);
00077         } else {
00078             sock.connect("192.168.1.1", 12345);
00079         }
00080         Thread::wait(1000);
00081     }
00082 }
00083 
00084 void ethernetSend()
00085 {
00086 #ifdef SERIAL
00087     printf("ethernetSendThread started\n");
00088 #endif
00089     std::stringstream ss;
00090 
00091     while(true) {
00092         if(sock.is_connected()) {
00093             ss.flush();
00094             ss << "Ball distance: " << ballDistance << "cm\n";
00095             ss << "Setpoint: " << setpoint << "cm\n";
00096             switch(statusFlag) {
00097                 case IDLE:
00098                     ss << "System is idle\n";
00099                     break;
00100                 case ADJUSTING:
00101                     ss << "System is adjusting\n";
00102                     break;
00103                 case STABLE:
00104                     ss << "System is stable\n";
00105                     break;
00106                 default:
00107                     break;
00108             }
00109             sock.send_all((char*)ss.str().data(),ss.str().size());
00110         } else {
00111             sock.connect("192.168.1.1", 12345);
00112 
00113 
00114         }
00115         Thread::wait(5000);
00116     }
00117 }
00118 
00119 void ethernetReceive()
00120 {
00121 #ifdef SERIAL
00122     printf("ethernetReceiveThread started\n");
00123 #endif
00124     char buffer[10];
00125     int ret;
00126     while(true) {
00127         if(sock.is_connected()) {
00128             
00129 
00130             ret = sock.receive(buffer, sizeof(buffer)-1);
00131 #ifdef SERIAL
00132             buffer[ret] = '\0';
00133             printf("Received %d chars from server:\n%s\n", ret, buffer);
00134 #endif
00135 
00136             switch(ret) {
00137                 default:
00138                     break;
00139                 case 1:
00140                     setpoint = (buffer[0]-'0');
00141                     break;
00142                 case 2:
00143                     setpoint = (buffer[0]-'0')*10 + buffer[1]-'0';
00144                     break;
00145             }
00146 
00147 
00148         } else {
00149             sock.connect("192.168.1.1", 12345);
00150         }
00151         Thread::wait(1000);
00152 
00153     }
00154 }
00155 #endif
00156 
00157 void ledSwitch()
00158 {
00159 #ifdef SERIAL
00160     printf("ledSwitch thread started\n");
00161 #endif
00162     while (true) {
00163         switch(statusFlag) {
00164             case IDLE:
00165                 ledR = 1;
00166                 ledG = 1;
00167                 ledB = !ledB;
00168                 Thread::wait(500);
00169                 break;
00170             case ADJUSTING:
00171                 ledR = !ledR;
00172                 ledG = 1;
00173                 ledB = 1;
00174                 Thread::wait(200);
00175                 break;
00176             case STABLE:
00177                 ledR = 1;
00178                 ledG = !ledG;
00179                 ledB = 1;
00180                 Thread::wait(1000);
00181                 break;
00182             default:
00183                 break;
00184         }
00185 
00186     }
00187 }
00188 
00189 void serialOut()
00190 {
00191 #ifdef SERIAL
00192     printf("SerialOut thread started\n");
00193     while(true) {
00194         printf("Ball distance: %fcm\n",ballDistance);
00195         printf("Setpoint: %fcm\n",setpoint);
00196         switch(statusFlag) {
00197             case IDLE:
00198                 printf("System is idle\n");
00199                 break;
00200             case ADJUSTING:
00201                 printf("System is adjusting\n");
00202                 break;
00203             case STABLE:
00204                 printf("System is stable\n");
00205                 break;
00206             default:
00207                 break;
00208         }
00209         Thread::wait(500);
00210     }
00211 #endif
00212 }
00213 
00214 void controlSystem()
00215 {
00216 #ifdef SERIAL
00217     printf("controlSystem thread started\n");
00218 #endif
00219     while(true) {
00220         ballDistance = ultrassonicSensor.distance(CM)+2.5;
00221         
00222         if (ballDistance > 37.5)
00223             ballDistance = 35;
00224         if (ballDistance <0)    
00225             ballDistance = 2.5;
00226             
00227             
00228         controller.setProcessValue(ballDistance);
00229 
00230         if (ballDistance != setpoint) {
00231             statusFlag = ADJUSTING;
00232         } else if (ballDistance == setpoint){
00233             statusFlag = STABLE;
00234         } else if (!isRunning){
00235             statusFlag = IDLE;
00236         }
00237 
00238         //PID CONTROLLER
00239         //motor.write(motorPos);
00240         //controller.setProcessValue(ballDistance);
00241         
00242         controller.setSetPoint(setpoint);
00243         if(isRunning)
00244             motorPos = controller.compute();
00245        
00246        #ifdef SERIAL
00247         //printf("Motor position: %f\n",motorPos);
00248         #endif           
00249         motor = 1-motorPos;
00250 
00251 
00252         Thread::wait(SAMPLE_RATE);
00253     }
00254 }
00255 
00256 
00257 int main()
00258 {
00259     
00260     
00261     Servo myservo(PTA2);
00262 /*
00263 while(1) {    
00264     for(float p=-90; p<=90; p += 45) {
00265         myservo.position(180-p+BIAS);
00266         wait(1);
00267     }
00268 }
00269 */
00270     motor = 0.5;;
00271     wait(1);
00272     
00273     
00274     statusFlag = IDLE;
00275 
00276 #ifdef SERIAL
00277     printf("BALL AND BEAM\n");
00278     printf("APS de Sistemas Operacionais / Controle 2\n");
00279     printf("Alunos: Felipe, Juliana, Rafael\n");
00280 #endif
00281     // input from 0.0 to 35 cm
00282     controller.setInputLimits(-1, 50.0);
00283     //Pwm output from 0.0 to 1.0 (servo)
00284     controller.setOutputLimits(0, 1);
00285     //If there's a bias.
00286     controller.setBias(BIAS);
00287     //controller.setBias(0.3);
00288     controller.setMode(AUTO_MODE);
00289     //We want the process variable to be 15cm (default)
00290     controller.setSetPoint(setpoint);
00291 
00292     sw2.rise(&sw2Callback);
00293     sw3.rise(&sw3Callback);
00294     ledSwitchThread.start(ledSwitch);
00295     #ifdef SERIAL
00296     serialOutThread.start(serialOut);
00297     #endif 
00298     controlSystemThread.start(controlSystem);
00299 
00300 #ifdef ETHERNET
00301     eth.init("192.168.1.2","255.255.255.0","192.168.1.1");
00302     eth.connect();
00303     sock.connect("192.168.1.1", 12345);
00304     sock.set_blocking(0);
00305 #ifdef SERIAL
00306     printf("IP Address is %s\n", eth.getIPAddress());
00307 #endif
00308     
00309     ethernetSendThread.start(ethernetSend);
00310     ethernetReceiveThread.start(ethernetReceive);
00311     ethernetKeepAliveThread.start(ethernetKeepAlive);
00312 #endif
00313 
00314     while(true) {
00315         //nothing
00316     }
00317 
00318 
00319 
00320 
00321 }