//
//    FILE: PID_simulated_heater.ino
//  AUTHOR: drf5n  (based on basic example)
// PURPOSE: demo
//
//  This simulates a 20W heater block driven by the PID
//  Wokwi https://wokwi.com/projects/356437164264235009
//
//  https://github.com/RobTillaart/PID_RT/issues/5
//
// See also https://wokwi.com/projects/357374218559137793

#include "PID_RT.h" // https://github.com/RobTillaart/PID_RT

PID_RT PID;  

//const int PWM_PIN = 3;  // UNO PWM pin
const int INPUT_PIN1 = A0;  // target/setpoint tuning
const int INPUT_PIN2 = A1;  // true angle tuning

float thetamax=10.0;  //deg

float targetAngle=thetamax;//   //deg
float trueAngle=0.0;  //deg
float measuredAngle=trueAngle;  //deg
//float setPoint=targetAngle;
float input = measuredAngle;
byte  pwm=0;
//float output=pwm;

void setup()
{
  Serial.begin(115200);
  Serial.println(__FILE__);
  //pinMode(PWM_PIN, OUTPUT);

  PID.setPoint(targetAngle);
  PID.setOutputRange(0, 255);  // PWM range
  PID.setInterval(2000); //update time in ms
  PID.setK(10, 0, 0); //PID coefficients
  PID.start();
  }

void loop()
{
  //targetAngle = thetamax*analogRead(A0)/1023; //pot A0
  PID.setPoint(targetAngle);

  trueAngle = simPlant(pwm); // simulate angle
  //trueAngle = thetamax*analogRead(A1)/1023; //pot A1

  measuredAngle=trueAngle;
  input = measuredAngle;
  //Serial.println(PID.getOutput());
  if(PID.compute(input))
  {
    pwm = PID.getOutput();
    Serial.println(pwm);
    //analogWrite(PWM_PIN, pwm);
  }
  report1();
}

void report1(void){
  static uint32_t last = 0;
  const int dt = 1000;
  if (millis() - last > dt){
    last += dt;
    //Serial.print(millis()/1000.0); 
    Serial.print(PID.getSetPoint());
    Serial.print(' '); 
    Serial.print(input);
    Serial.print(' '); 
    Serial.println(pwm);
  }
}

/*
float driverModel(byte pwm, float Vmax){
  //attention: 3.7 or 5.0 V !?
  //maximum current?
  return Vmax*pwm/255;  //outputs dc voltage
}

float motorModel(float Vdc, float Vmax){
  return Vdc*rpm/Vmax;//outputs rpm
}

float lift(){
  return 0.0;
}
*/

float simPlant(byte pwm2){    // simulate the aeropendulum
   float g = 9.81; //
   float m = 0.1 ; // 
   float L = 0.5; //
   float lift=0.5*(m*g)/L/m/255;
   static float theta = 0; //start theta
   static float lasttheta = theta; //
   static float omega = 0;
   static float lastomega = omega; //
   static uint32_t lastTime = 0;
   uint32_t dt = 100; // ms
   if(millis() - lastTime >= dt){
      omega = lastomega + (-g/L*sin(lasttheta*3.14/180)+ lift*pwm2)*dt/1000*180/3.14;
      //Serial.println(-g/L*sin(lasttheta*3.14/180)+ lift*pwm2);
      theta = lasttheta + omega*dt/1000;

      //Serial.print(theta);
      //Serial.print(",");
      //Serial.println(omega);
      lastTime += dt;
      lasttheta=theta;
      lastomega=omega;
   }
   return theta;
}


// -- END OF FILE --