main.cpp

00001 #include "mbed.h"
00002 #include "millis.h"
00003 #include "Motor.h"
00004 #include "PID.h"
00005 
00006  int val; 
00007 
00008  int encoder0Pos = 0;
00009  int encoder0PinALast = 0;
00010  int n = 0;
00011  float rpm, rpms=0;
00012  float speed;
00013  float speeds;
00014  float maxSpeed = 0.6;
00015  float minSpeed = 0.1;
00016  float speedR;
00017  float pid;
00018 
00019  int start = 1;
00020  
00021  float rpmStandard = 40; // tentuin rpm yang diinginkan
00022  unsigned long int previousMillis = 0;
00023 
00024 // ----------------------------
00025 // inisialisasi encoder 
00026 DigitalIn encoder0PinA(D7);
00027 DigitalIn encoder0PinB(D8);
00028 // ----------------------------
00029 
00030 // -----------------------------
00031 // Inisialisasi PID
00032 // PID(KP,TD,TI,INTERVAL PID)
00033 PID PID(0.985,0.0,0.0,0.001);
00034 // ----------------------------
00035 
00036 // ----------------------------
00037 // INISIALISASI SERIAL
00038 Serial pc(USBTX,USBRX);
00039 // ----------------------------
00040 
00041 // ----------------------------
00042 // INISIALISASI MORTOR
00043 Motor motor1(D3,D2,D4);
00044 // ----------------------------
00045 
00046 
00047 /*
00048     init_speed() untuk mencegah pwm kelebihan batas
00049     batas ditentukan oleh variabel global maxSpeed
00050     ubah sesuai keinginan
00051 */
00052 void init_speed (){
00053     if (speed>maxSpeed){
00054         speed = maxSpeed;
00055     }
00056     
00057     if (speed<minSpeed){
00058         speed = minSpeed;
00059     }
00060 }
00061 
00062 /*
00063     PID_ENCODER(float speed)
00064     speed == pwm
00065     pv merupakan process value dari keadaan nyata
00066         --- 0 untuk keadaan rpm == rpmStandard
00067         --- -1 untuk keadaan rpm < rpmStandard
00068         --- 1 untuk keadaan rpm > rpmStandard
00069 */
00070 void PID_ENCODER(float speed) {
00071     int pv;
00072      if(rpm == rpmStandard) {
00073        pv = 0;
00074      }
00075      else if(rpm < rpmStandard) {
00076       pv = -1;
00077      } else {
00078       pv = 1;    
00079      }
00080      
00081     PID.setProcessValue(pv);
00082     PID.setSetPoint(0);
00083     pid = PID.compute();
00084     speedR = speed + PID.compute(); // compute
00085     if(speedR < minSpeed) {
00086         speedR = minSpeed;
00087     }
00088     motor1.speed(speedR); // change speed of motor 1
00089 }
00090  
00091  int main(void) { 
00092     pc.baud(9600); // set pc_baud
00093     PID.setInputLimits(-1,1); // input limits pv dari -1 sampai 1 yaitu -1,0,1
00094     PID.setOutputLimits(0,0.2); 
00095     // output limits adalah batasan pwm max. karna pwm max diset oleh variabel maxSpeed, jadi ubah2 output limits sesuai batasan
00096     // contoh jika input speed 0.2 dan batasan speed maksimum adalah 0.5, maka setoutputlimits menjadi (0,0.3); karna 0.2 + 0.3 = 0.5
00097     PID.setMode(1.0); // set pid jadi full auto
00098     PID.setBias(0.0); // default
00099     PID.reset(); // default
00100 
00101 
00102     
00103     startMillis();
00104     speed = 0.3; // tentuin speed sesuai keinginan
00105     
00106     while(1){
00107         speeds = speed;
00108         init_speed();
00109         PID_ENCODER(speeds);
00110    
00111         pc.printf("pwm = %.02f\t pid = %.02f \t rp_loop = %.0f\n",speedR, pid, rpm); // output ke serial komputer
00112     
00113         unsigned long int currentMillis = millis();
00114     
00115        n = encoder0PinA;
00116         if ((!encoder0PinALast) && (n))
00117         {
00118             if (!encoder0PinB)
00119             {
00120                 encoder0Pos--;
00121             } else {
00122                 encoder0Pos++;
00123             }
00124         } 
00125         encoder0PinALast = n;
00126         
00127         rpm = 120.0/7.0*encoder0Pos; // hitung rpm
00128         //rpms = rpms + rpm;
00129              
00130     if (currentMillis-previousMillis>=500)
00131     {
00132        previousMillis = currentMillis;
00133       
00134        //pc.printf ("rpms = %.0f \n",rpms);
00135        rpms = 0;
00136        encoder0Pos = 0;
00137     }
00138  }
00139  
00140  }