//
//    FILE: PID_simulation
//  AUTHOR:  mn
// PURPOSE: demo
//
// https://wokwi.com/projects/412312875773779969//  
// adapted from https://wokwi.com/projects/356437164264235009
////
// See also https://wokwi.com/projects/357374218559137793

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

PID_RT PID;  //instantiate controller object

#define PWM 3  //PWM controller output
#define INPUT_PIN1 A0  //manual setpoint (target angle 0-thetamax deg)
#define INPUT_PIN2  A1  //manual sensor reading (measured angle 0-thetamax deg)
#define INPUT_PIN3  A2  //manual PWM controller output (0-255)

//plant (aeropendulum) start state
float trueAngle=0.0;  //deg   //start angle
float trueOmega=0.0; //deg/s start omega

//controller maximum ratings
float thetamax=20.0;  //deg  (controller input max)
float pwmmax=255; // (controller output max)

//controller setpoint
float targetAngle=trueAngle;//   //deg
//float setPoint=targetAngle;

//controller input (sensor reading)
float measuredAngle=trueAngle;  //deg
//float input = measuredAngle;

//controller output
float  pwm; 
//float output=pwm;

void setup()
{
  Serial.begin(115200);
  Serial.println(__FILE__);
  Serial.println("SetPoint(deg) Sensor(deg) PWM(0-255)");

  pinMode(PWM, OUTPUT);

  //set up controller
  PID.setPoint(targetAngle);
  PID.setOutputRange(0, pwmmax);  // PWM range
  PID.setInterval(100); //update time in ms
  PID.setK(10, 0, 0); //PID coefficients
  PID.start();
  }

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

  //update plant
  analogWrite(PWM, pwm); //real life
  //trueAngle = simPlant(); // simulate plant and angle
  trueAngle = thetamax*analogRead(A1)/1023; //regulated plant

  //measurement
  measuredAngle=readSensor();

  //show values
  show();

  delay(20); //throttle
}

void controller(){
  PID.setPoint(targetAngle);
   if(PID.compute(measuredAngle)){
    pwm = PID.getOutput();
  }
}

float simPlant(){    // simulate the aeropendulum
   float g = 9.81; //
   float m = 0.1 ; // 
   float L = 0.5; //
   float pwm2lift_coefficient=0.5*(m*g)/L/m/pwmmax; //% of gravitational torque
   static float theta = trueAngle; //start angle (deg)
   static float lasttheta = theta; //deg
   static float omega = 0;
   static float lastomega = omega; //deg/s
   static uint32_t lastTime = 0;
   uint32_t dt = 100; // ms //update period
   if(millis() - lastTime >= dt){
      theta = lasttheta + lastomega*dt/1000;
      omega = lastomega + 180/3.14*(-g/L*sin(lasttheta*3.14/180)+ pwm2lift_coefficient*pwm)*dt/1000;
      lastTime += dt;   //bookkeeping references
      lasttheta=theta;
      lastomega=omega;
   }
   return theta;
}

float readSensor(){
  return trueAngle; //change to real or virtual measurement
}

void show(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(),1);
    Serial.print(' '); 
    Serial.print(measuredAngle,1);
    Serial.print(' '); 
    Serial.println(byte(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;
}
*/

// -- END OF FILE --