//
//    FILE: PID_simulated_heater.ino
//  AUTHOR: drf5n  (based on basic example)
// PURPOSE: demo
//



#define TEMP_READ_DELAY 800 //can only read digital temp sensor every ~750ms
//pid settings and gains
#define OUTPUT_MIN 0
#define OUTPUT_MAX 1100
#define KP 100
#define KI 0
#define KD 0

#define SIM_TIME 100
#define SIM_T_START 20
#define SIM_T_LOSS 2.9e-2
#define SIM_T_HEAT 0.7787

#define PROBE_ERROR 1.0 // +/– 1.0ºC

//pins
#define POT_PIN A0
#define OUTPUT_PIN A1
#define TEMP_PROBE_PIN 5
#define LED_PIN 6

#define REPORT_INTERVAL 100




unsigned long lastTempUpdate; //tracks clock time of last temp update
unsigned long lastTimeUpdate = 0;

float Force=50.0;
float Mass=10.0;
float Motor_Current=255.0;
float End_Speed=0;
float Mean_Speed=0;
float Distance=0;
float Wanted_Position=0;
float Friction=0.98;
float Error_Distance=0;


void setup()
{
  Serial.begin(115200);
  Serial.println(__FILE__);
  Serial.println("Setpoint Input Output Watts");

Wanted_Position=15.0;  

}


void loop()
{


  Error_Distance=Wanted_Position-Distance;
  
Motor_Current= Error_Distance*256.0/15.0;

  inertia(Motor_Current);  
  delay(100);
}








void report(void){

    Serial.println("outputVal");


}

void inertia(float M_Current){
    
    float Max_Speed=2.0;

    float Previous_Speed=0;
        float Acceleration=0;
    unsigned long CurrentTime=0;
    unsigned long DeltaTime=0;
    Acceleration=M_Current/Mass;

    CurrentTime=millis();
    DeltaTime=CurrentTime-lastTimeUpdate;
    lastTimeUpdate=CurrentTime;

    Previous_Speed=End_Speed;

   if(CurrentTime>27130){
      Acceleration=0;
   }

    End_Speed+=DeltaTime*Acceleration/1000;
    End_Speed=End_Speed*Friction;
    if(End_Speed>Max_Speed){
        End_Speed=Max_Speed;
    }


    Mean_Speed=Previous_Speed+0.5*DeltaTime*Acceleration/1000;
    if(Mean_Speed>Max_Speed){
        Mean_Speed=Max_Speed;
    }

    Distance+=DeltaTime*Mean_Speed/1000;

    Serial.print(CurrentTime);
    Serial.print('\t');
    Serial.print(Acceleration);
    Serial.print('\t');
    Serial.print(End_Speed);
    Serial.print('\t');
    Serial.print(Mean_Speed);    
    Serial.print('\t');
    Serial.print(Distance);  
    Serial.println();
    


}

// -- END OF FILE --