/*
Program     : Percobaan Observasi Plant menggunakan Arduino Mega
DiBuat Oleh : EruP-RMK
Keluaran    : v1.00 / v2.00 / v3.00
Tanggal     : 12 September 2018 - 8 Agustus 2020 - 19 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, dalam 0rpm sampai RpmMax
float oMotor=0;           // Keluaran Motor saat ini (0 rpm sampai RpmMax)

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

float Lineritas(float x)    // Menghitung ketidak-linearan Km dari plant berdasarkan input
{                           // hasil dari 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 InputOutput()                    // Proses utama dari sistem,
{                                     // hanya input output untuk observasi Plant
  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
  PWM=SettingPoint*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 Kecepatan PWM Min Max WaktuSampling=");
  //Serial.print(S);
  //Serial.println("ms");
  
  Serial.print(SettingPoint,0);   // Kirim data ke program monitor
                                  // melalui komunikasi serial
  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(PWM,0);            // tanda 0 artinya tidak ada nilai pecahan
  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 ?
  {
    if(Serial.read()=='s')            // Jika Ya, apakah dimulai huruf depan 's' ?
    {
      SettingPoint=Serial.parseInt(); // Jika Ya, baca data integer berikutnya
      if(SettingPoint>RpmMax)
        SettingPoint=RpmMax;          // Tidak boleh melebihi rpm Max
    }                                 // dan simpan sebagai Setting Point
  }                                   // Misal data yang diterima, "s1000<CR>"
                                      // Set Carriage Return pada terminal,
                                      // agar saat enter, lansung dikirimkan CR
}

void setup()                                // Bagian yang pertama dikerjakan oleh 
{                                           // Arduino, hanya satu kali
  Serial.begin(115200);                     // sesuaikan dengan COM: serial pada PC
  Serial.println("SetPoint Kecepatan PWM"); // Cetak Label (Legenda)
  t_lalu=t1=millis();                       // catat waktu mulai,
                                            // untuk perhitungan Periode Sampling
}

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 ?
    InputOutput();              // 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
  }
}