/*
             CREATOR: DAn williams
                DATE: Nov 15 2004
         DESCRIPTION: feedback sytem simulator
            FILENAME: plant.c
             STARTED: Nov 14 2004
    OPERATING SYSTEM: Linux
1st VERSION FINISHED:

this is basically a collection of stuff that makes it easy to simulate feedback systems

=======================| RC Circuits |=======================

     feedback? 
     
     Vin ------ R ------- Vout
                    |
                    C
                    |
                  GND

     Iout = 0
     I(resistor) = Vin - Vout
     Vout = V(capacitor) = I(resistor)*t/C
 
     Vnew = Vold + Difference * k
     data[Vout] = data[Vin] + ((data[Vout] - data[Vin]) * 200/1000); // RC style responce T ~ 4.48142011
     
     in elec calcs they do it from the top... like this
     
     Vnew = Vnew - Difference * k
     data[Vout] = data[Vin] - ((data[Vin] - data[Vout]) * 0.8); 
     
     its really the same thing. For an RC circuit, K = exp( -t / T )
     more verbosly coded as...
     
     // 10K ohms
     #define R 10000.0
     // 100uF
     #define C (100.0/1000000.0)
     data[Vout] = data[Vin] - ( (data[Vin] - data[Vout]) * exp( -SAMPLETIME / ( R * C ) ) );          
     
===================| Signal generators |====================

  here is how to make a signal that is 0 until t=1 and then changes to 2:
          
    data[CTRL] = step( 1, 0, 2); // step(start, low, high)     
    data[CTRL] = sinewave(1, 1); //   sinewave(amplitude, frequincy)
    
  here is an example of creating a step in frequincy
  at t = 1 the period goes from 6.28 to 3.14 : 
  
    data[CTRL] = sinewave(1, step(10, 1, 2)); // dosn't align verry pretty
    
  here is an example of creating a square wave:  
    data[CTRL] = sinewave(1, 1) > 0; 
    
==================| Real world simulations |==================

  In the real world values have limits, be they the power rails
    of an op-amp circuit, or the bits in a byte. To account for 
    these limits there is a limit macro, examples:
    
  +-15V power rails; op amp circuit:
  value = limit(value, -15, 15);
  
  8 bit digital system, integer:
  value = limit(value, 0, 255);
  
  16 bit signed digital system:
  value = limit(value, -32768, 32767);
  

==============================================================

*/

#define  DATATYPE   double
#define  DATAPRINT  "%f"

#include "plant.h"

#define itterations   600
#define SAMPLETIME    0.1
#define VARIABLES     8

int main(int argc, char** argv) {
  // local VARIABLES
 
  csvfile_t   output;
  char *      titles[VARIABLES] = {"t", "control", "velocity", "position",  "drive", "P", "D", "I"   };
  char        Ki, Kp, Kd;
 
  
  // variable indexes
  #define     CTRL   1

  #define     VEL    2
  #define     POS    3
  #define     DRV    4  
  
  #define     PROP   5
  #define     DERIV  6  
  #define     INT    7
              
  DATATYPE    data[VARIABLES];
  int         regs[VARIABLES];
  int         itteration, n;  
  
  printf(" This program simulates feedback systems \n");
  printf(" Written by Dan Williams nov 14/2004 \n");


 /************
 
  Do some argument paramiters
  
 ************/
 
 if (argc > 2) {
    printf("Using your paramiters\n");
    Ki = atoi(argv[1]);
    Kp = atoi(argv[2]);
    Kd = atoi(argv[3]);
 } else {
    Ki = 1;
    Kp = 8;
    Kd = 32;
 }
  
  // get the output file ready
  if (initcsv(&output, VARIABLES, titles) != OK) {
    printf( "Unable to open output file\n" );
    return 0;
  }

  // all initial data = 0
  for (n = 0;  n < VARIABLES; n++) data[n] = 0;  
  for (n = 0;  n < VARIABLES; n++) regs[n] = 0;  
  time   = 0;
  
  for(itteration = 0; itteration < itterations; itteration++) {
     // reccomended that this stay the same 
     data[0] = time;
     
     // --------------------| your system here |-----------------
     data[CTRL] = step(1, 0, 64);
      // data[CTRL] = (sinewave(16, 0.5) > 0)* 20;
          
     regs[DERIV] = regs[PROP];             // save error for deriv calc

     // prop.     
     regs[PROP] = data[CTRL] - regs[POS];  // error
      
     // deriv.
     regs[DERIV] = regs[DERIV] - regs[PROP] ;           // calc diff in position
     
     // integral
     if ((itteration % 10) == 0) {
       regs[INT] =  limit(regs[INT] + regs[PROP], -128, 127);              // integrate error     
     }
     
    regs[DRV] = limit((regs[INT]/8 ), -128, 127) ;
   
    data[DRV]   = regs[DRV];      
    data[PROP]  = regs[PROP];
    data[DERIV] = regs[DERIV];
    data[INT]   = regs[INT];
     
     // This bit simulates a system with mass that accumulates position.
     #define Tow 5.0
     data[VEL] = data[DRV] - ( (data[DRV] - data[VEL]) * exp( -SAMPLETIME / Tow ) );     
     data[POS] = (int)limit(data[POS] + data[VEL], -128, 127);
     
     regs[POS] = data[POS];
  
     // ---------------------------------------------------------
     writecsv(&output, data);  // write the data to file
     time += SAMPLETIME;       // increment time
  }

  closecsv(&output);
  
  return 0 ;
}


//=========================| subroutines |===============================

//----------- input generators

// generate a step
DATATYPE step( float startTime, DATATYPE minlevel, DATATYPE maxlevel ) {
  if (time >= startTime) {
    return maxlevel;
  } else {
    return minlevel;
  }
}


// impulse


// sinwave
DATATYPE sinewave( float amplitude, float frequincy) {
   return  amplitude * sin(frequincy * time);
}

//------------- csv file output 

// initcsv
Error_t initcsv(csvfile_t * data, int varcount, char ** titles) {

  int n;
  
  if ((data->output = fopen("output.csv", "wt")) == NULL) {  
    printf("Unable to open output file.\n");
    return OPENFAIL;
  }
  
  data->varcount = varcount;
  
  for (n = 0; n < data->varcount; n++) {
    if (n > 0) fprintf(data->output, ",");
    fprintf(data->output, "%s", titles[n]);
  }
  fprintf(data->output, "\n");
  
  return OK;

}

// writecsv
Error_t writecsv(csvfile_t * data, DATATYPE * array) {

  int n;
   
  for (n = 0; n < data->varcount; n++) {
    if (n > 0) fprintf(data->output, ",");
    fprintf(data->output, DATAPRINT, array[n]);
  }
  fprintf(data->output, "\n");
  return OK; 
}


// closecsv
void closecsv(csvfile_t * data) {
  fclose( data->output );
  return;
}