/*
Program     : Percobaan Kontrol Open Loop Adaptive W +B menggunakan Arduino Mega
DiBuat Oleh : EruP-RMK
Keluaran    : v1.00 / v3.00
Tanggal     : 12 September 2018 - 8 Agustus 2020 / 21 Juni 2023 /6 Agustus 2023
Untuk       : Praktikum Kontrol Cerdas 2 POLITEKNIK ELEKTRONIKA NEGERI SURABAYA (PENS)

Perangkat   : Arduino Mega, Plant Emulasi, Koneksi ADC Emulasi dan Pin Port I/O
              (sebagai PWM) Emulasi
*/

float SettingPoint=0;       // Setting Point
float Kecepatan=0;          // Kecepatan Plant (Motor) saat ini
float PWM=0;                // nilai PWM yang dikeluarkan 8 bit (0-255)
float adc=0;                // nilai ADC hasil pembacaan sensor 10 bit (0-1023)

const int MOTOR=3;          // Nomor PIN untuk output PWM (emulasi PWM oleh Arduino)
const int SENSOR=A0;        // Nomot PIN ADC

float TmMotor=1;            // Motor naik dari 0% sampai 63% selama berapa detik
                            // Ganti nilai Tm dengan NomorUrut/10

float out_plant=0;          // Keluaran dari plant, hasil dari Tachometer (0v - 5v)
float oMotor=0;             // Keluaran Motor saat ini (0 rpm sampan Max rpm)

float BebanMotor=0;         // Pembebanan motor, 0% sampai 100%
float TeganganMotorMax=5;   // Tegangan dari Motor, dianggap Motor 5V
float KecepatanMax=3000;    // Kecepatan tertinggi dari sistem
float RpmMax=2000;          // Kecepatan tertinggi Diaplay
float RpmMin=0;             // Kecepatan terrendah Diaplay

#define PeriodeSampling 20  // waktu sampling 20ms

unsigned long t1, t2, S;    // menghitung interval antar sampling
                            // untuk perhitungan plant
unsigned long t_lalu;       // waktu sampling sebelumnya

int   Periode=0;            // panjang periode (Setting Point on/off)
                            // dalam satuan sampling, Periode=0 -> tidak periodik
int   Random=0;             // nilai Setting Point acak
int   Index=0;              // penghitung sampling (Loop)
float SetP=0;               // BackUp Setting Point
float Error, MSE=RpmMax;    // Error saat ini dan Error Kuadrad rata-rata 
float W=0, B=0, Out;        // Parameter Kontrol dalam bentuk Bobot W dan Bias B

float Lineritas(float x)    // Menghitung ketidak-linearan Km dari plant berdasarkan
{                           // input, hasil regresi Polinomial pengujian Km plant real
  // y=-0.4541*I2^6-15.024*I2^5+49.092*I2^4-54.251*I2^3+21.936*I2^2-0.6329*I2+0.0057
  if(x < 0) x = 0;
  if( x > 1) x = 1;
  return -0.4541*pow(x,6)-15.024*pow(x,5)+49.092*pow(x,4)-54.251*pow(x,3)+21.936*x*x
         -0.6329*x+0.0057;
}

float plant(float Tegangan)               // Emulasi plant secara software
{                                         // Menggunakan pendekatan pengisian Kapasitor
                                          // Dengan formula Km Tm (lihat teori)
  t2=millis();                            // berapa waktu sekarang ? Dalam satuan ms
  S=t2-t1;                                // Hitung interval antar sampling (t1 & t2)
  float tSampling=float(S)/1000.0;        // Interval untuk menghitung waktu Sampling
  t1=t2;                                  // untuk perhitungan berikutnya
  
  float KmMotor=(1-BebanMotor/100)                  // Km Plant akibat pembebanan
                *KecepatanMax/TeganganMotorMax;     // dan perbandingan rpm/volt plant
  float V=Tegangan/TeganganMotorMax;                // Perbandingan Vin dari Vin Max
  float KmLineritas=Lineritas(V);                   // Hitung Ketidak-linearan Km plant
  float iMotor=Tegangan*KmMotor*KmLineritas;        // Hitung rpm yang masuk plant
  oMotor=oMotor+(iMotor-oMotor)*tSampling/TmMotor;  // Hitung rpm keluaran plant
  if( oMotor < 0) oMotor = 0;                       // plant tidak bisa mundur      
  return oMotor;
}

int _analogRead(int ch)               // ADC Emulasi, input dari Tachometer, 0v - 5v
{                                     // sesuai dengan 0rpm sampai KecepatanMax Plant
  float Vi=out_plant*5/KecepatanMax;  // Sensor Tachometer, dari rmp ke Vi (0v-5v)
  return Vi*1023/5;                   // ADC 10 bit, Referensi 5v resolusi 10 bit
}                                     // Input 0v-5v diubah menjadi 0-1023

void _analogWrite(int pin, int PWM)   // DAC Emulasi, output ke Driver Plant
{                                     // Tegangan 0v-5v
  float Vo=PWM*5.0/255.0;             // dari PWM (0-255) diubah ke Vo (0v-5v)
  out_plant=plant(Vo);                // dari Vo (0v-5v) masuk ke Plant menjadi rpm
}

void Kontrol()                  // Proses utama dari sistem Kontrol,
{                               // hanya input output untuk observasi Sistem
  adc=_analogRead(SENSOR);          // Baca tegangan sensor menggunakan ADC 
                                    // sebanding dengan kecepatan/keluaran Plant
                                    // dari Tachometer 0-MaxRpm menjadi Vin ADC 0v-5v
                                    // keluaran ADC 0-1023 (ADC 10 bit VRef 5v)
  Kecepatan=adc*KecepatanMax/1023;  // Konversi dari biner ADC ke Kecepatan rpm
  
  Error=SettingPoint-Kecepatan;     // Hitung Error
  MSE=Error*Error*0.001+MSE*0.999;  // Hitung MSE menggunakan teknik Smoothing
  if(MSE>RpmMax) MSE=RpmMax;        // Batasi nilai MSE, biar tidak terlalu besar
  
  W=W+1e-7*Error*SettingPoint;    // Lakukan proses Update W berdasar Delta Rule
  B=B+1e-3*Error;                 // Lakukan proses Update B
  
  Out=SettingPoint * W + B;       // Output = Input (dari SetPoint) x Bobot (W)
  PWM=Out*255/KecepatanMax;       // Tentukan nilai PWM (data biner) berdasarkan
                                  // Setting Point yang telah ditentukan
  if(PWM>255) PWM=255;            // Nilai PWM Maksimal 255
  if(PWM<0) PWM=0;                // Nilai PWM Minimal 0
  _analogWrite(MOTOR,PWM);        // Kirim ke PLANT melalui PORT I/O PWM emulasi
                                  // dalam bentuk program dalam Arduino)
  analogWrite(LED_BUILTIN,PWM);   // tampilkan juga dalam bentuk nyala LED,
                                  // untuk mengetahui besarnya sinyal PWM
}

float cut(float Data)           // Membatasi tampilan pada Plotter
{                               // harus diantara RpmMin dan RpmMax
  if(Data>RpmMax) Data=RpmMax;  // Jika melebihi RpmMax, batasi
  if(Data<RpmMin) Data=RpmMin;  // atau kurang dari RpmMin, batasi
  return Data;  
}

void Tampilkan()                  // Untuk mengirimkan data-data sistem ke PC
{                                 // melalui komunikasi Serial
  Serial.print("SetPoint=");      // Tampilkan Label "SetPoint=1000" (misalkan) di atas             
  Serial.print(SettingPoint,0);   // gambar grafik. Angka "0" artinya tanpa pecahan
  Serial.print(" Kecepatan=");    // Tampilkan Label Kecepatan
  Serial.print(Kecepatan,0);      // Tiap label harus dipisah dengan spasi " "
  Serial.print(" W=");
  Serial.print(W);
  Serial.print(" B=");
  Serial.print(B);
  Serial.print(" PWM=");
  Serial.print(PWM,0);
  Serial.print(" Error=");
  Serial.print(Error,0);
  Serial.print(" MSE=");
  Serial.print(MSE,0);
  Serial.print(" Min Max");
  Serial.print(" tSampling=");  // Tampilkan juga Waktu Sampling berdasarkan
  Serial.print(S);              // pengukuran waktu antar sampling (cek bagian Plant)
  Serial.println("ms");         // Untuk melihat, apakah waktu sampling sesuai
                                // dengan yang diinginkan
                                // Jika waktu sampling tertulis melebihi yang
                                // diinginkan, artinya waktu sampling yang diinginkan
                                // terlalu cepat, yang tidak dapat diikuti oleh
                                // kecepatan sistem

  Serial.print(SettingPoint,0);   // Kirim data grafik ke program monitor
                                  // melalui komunikasi serial (Serial Plotter)
  Serial.write(' ');              // tiap data harus dipisah dengan spasi " "
  Serial.print(cut(Kecepatan),0); // Kirim data Kecepatan (bilangan bulat)
  Serial.write(' ');              // tiap data harus dipisah dengan spasi " "
  Serial.print(W);                // tanda 0 artinya tidak ada nilai pecahan
  Serial.write(' ');
  Serial.print(B);
  Serial.write(' ');
  Serial.print(PWM,0);                            
  Serial.write(' ');
  Serial.print(cut(Error),0);     // Tampilkan Error
  Serial.write(' ');              // Dan MSE, jika melebihi
  Serial.print(cut(MSE),0);       // max, potong
  Serial.write(' ');
  Serial.print(RpmMin);           // Berikan nilai Min dan Max
  Serial.write(' ');              // agar skala grafik tetap tidak berubah
  Serial.println(RpmMax);         // dan akhiri dengan karakter <LF>
}

void BacaSerial()                     // Menerima data perintah dari PC
{                                     // melalui komunikasi serial
  if(Serial.available())              // Apakah ada penerimaan data serial dari PC ?
  {
    switch(Serial.read())             // akhiri setiap perintah dengan ";" contoh "s1000;"
    {                                 // atau "<CR>", contoh "s1000<CR>"
      case 's':                       // Jika Ya, apakah dimulai huruf depan 's' ?
        SetP=Serial.parseInt();       // Jika Ya, baca data integer berikutnya
        if(SetP<0) SetP=0;            // dan simpan sebagai Setting Point
        else                          // Misal data yang diterima, "s1000<CR>"
        if(SetP>RpmMax)SetP=RpmMax;   // Set Carriage Return pada terminal,
        Index=0;                      // agar saat enter, lansung dikirimkan CR
        break;
      case 't':                           // 't0.001' - 't10' Tm 0.01 sampai Tm 10.00
        TmMotor=Serial.parseFloat();      // Tm dihitung dalam detik, untuk setiap
        if(TmMotor<0.001) TmMotor=0.001;  // output plant naik dari 0% sampai 63%
        else if(TmMotor>10) TmMotor=10;   // dari kecepatan tertinggi dari output plant
        break;                            // (kondisi steady state dari plant)
      case 'l':                             // 'l0' - 'l100' tanpa load (0%)
        BebanMotor=Serial.parseInt()/100.0; // sampai load penuh (plant berhenti) 100%
        if(BebanMotor<0) BebanMotor=0;      // Mensimulasikan efek pembebanan pada plant
        else
        if(BebanMotor>100) BebanMotor=100;
        break;
      case 'p':                           // 'p0' periodik mati, 'p100' periodik
        Periode=Serial.parseInt();        // dihitung persatuan sampling
        Periode=(Periode/2)*2;            // harus genap
        if(Periode<0) Periode=0;          // Min 0 (mati)
        else
        if(Periode>1000) Periode=1000;    // Max Periode 1000 sampling
        Index=0;                          // setiap pergantian periode, mulai hitungan
        break;                            // dari nol
      case 'r':                           // 'r0' random mati, 'r1' random aktif
        Random=Serial.parseInt();         // Setting Point diberikan secara ramdom
        if(Random<0) Random=0;            // digunakan saat melakukan pelatihan sistem
        else if(Random>1) Random=1;       // adaptif atau NN
        randomSeed(analogRead(0));
        break;
       case 'w':
        W=Serial.parseFloat();
        break;
       case 'b':
        B=Serial.parseFloat();
        break;
      case 'm':                             // 'm0' - 'm10000'
        RpmMax=Serial.parseInt();           // Menentukan batas tertinggi dari sistem
        if(RpmMax<0) RpmMax=0;              // hanya untuk memudahkan proses menampilkan
        else if(RpmMax>10000) RpmMax=10000; // sinyal sistem kontrol
        break;
    }
  }   
}

void SetPoint()                 // Untuk pengaturan Setting Point
{                               // Statis (tetap), Periodik atau Random
  if(Index==0)                                      // setiap awal hitungan
    SettingPoint=(Random==0?SetP:random(0,RpmMax)); // tentukan Setting Point
  if(Periode>0&&Index*2==Periode)                   // dalam mode Periodik
    SettingPoint=(Random==0?0:random(0,RpmMax));    // maka tiap 1/2 periode diubah
                                                    // 0 atau sesuai bilangan random
                                                    
  if(Periode>0)                 // Jika setting point dibuat Periodik
  {
    Index++;                    // naikkan hitungan
    if(Index>=Periode)          // Maka setiap hitungan mencapai periode
      Index=0;                  // kembalikan hitungan ke 0
  }
}

void setup()                                // Bagian yang pertama dikerjakan
{                                           // hanya satu kali
  Serial.begin(115200);                     // sesuaikan dengan serial pada Komputer
  Serial.println(" ");                      // Cetak Label (Legenda) kosong
  t_lalu=t1=millis();                       // catat waktu mulai,
                                            // untuk perhitungan Periode Sampling
  randomSeed(analogRead(0)*analogRead(1));  // inisialisasi bilangan random
}

void loop()                     // Bagian yang dikerjakan secara
{                               // terus-menerus oleh Arduino
  BacaSerial();                 // Baca perintah serial dari PC             
  unsigned long t=millis();     // Baca waktu saat ini, dalam ms
  if(t-t_lalu>=PeriodeSampling) // Apakah dengan waktu sebelumnya
  {                             // sudah terpaut 1 periode sampling ?
    SetPoint();                 // Jika ya, atur nilai Setting Point
    Kontrol();                  // Jika ya, lakukan operasi sampling
                                // untuk menjalankan kontrol
    Tampilkan();                // Tampilkan data-data sistem ke PC
                                // melalui komunikasi serial
    t_lalu=t;                   // dan catat waktu sekarang sebagai waktu lalu,
                                // untuk perhitungan 1 sampling berikutnya
  }
}