#include <PID_v1.h>
#include <Encoder.h>
#include "BasicStepperDriver.h"

// Motor steps per revolution. Most steppers are 200 steps or 1.8 degrees/step
#define MOTOR_STEPS 200

// All the wires needed for full functionality
#define ENBL 7
#define DIR 8
#define STEP 9

// Since microstepping is set externally, make sure this matches the selected mode
// 1=full step, 2=half step etc.
#define MICROSTEPS 1
// 2-wire basic config, microstepping is hardwired on the driver
BasicStepperDriver stepper(MOTOR_STEPS, DIR, STEP);

//Mechanical Sytem Properties
  //full pendulum rotation =8000 Encoder counts
#define rot_Encoder_A 20    // rotary Encoderpin A an Pin 20
#define rot_Encoder_B 21    // rotary Encoderpin B an Pin 21
#define lin_Encoder_A 18    // linear Encoderpin A an Pin 18
#define lin_Encoder_B 19    // linear Encoderpin B an Pin 19
Encoder rotEnc(rot_Encoder_A, rot_Encoder_B);   
Encoder linEnc(lin_Encoder_A, lin_Encoder_B);   

//Variable Definitions

  //Pin Definitions
   byte pin_photoint=13;
   double a = 0.80;  // Dämpfungsparameter im Filter
   
 
 //Linear and angle Limits for Controller
    long limit_lin_min= -1800 ;    //1600 min Limit (in direction torwards limit switch--> right)
    long limit_lin_max=  1800 ;   // 40000 max Limit (left)
    double limit_angle= 300; //limit at which it becomes impossible to catch the pendulum again if exeeded, active control of pendulum is stopped and cart is stopped if exeeded. Limit works in both direction (+ and -) at the equilibrium point
   
 //Deadband Variables
     byte output_deadband= 40;
 
    
  //Miscellaneous Variables
    int rot_offset=0;   //Offset from Vertical, System inherent error. Use if System always tends to run in one direction
    double angle, y=0;
    double lin_position;
      
//PID Setup
  //PID Variables 
    double rot_kp= 6.2;
    double rot_ki= 84.0;
    double rot_kd= 0.0;
    double lin_kp= 0.0000001; 
    double lin_ki= 0.00000001; 
    double lin_kd= 0.0;
    double Output= 0.0;
    double rot_Setpoint = 0.0;
    double lin_Setpoint = 0.0; // 21000 is approx. middle of track
         
 //PID Controller initialisation
    PID rot_Controller(&angle,&Output,&rot_Setpoint,rot_kp,rot_ki,rot_kd,DIRECT);
    PID lin_Controller(&lin_position,&rot_Setpoint,&lin_Setpoint,lin_kp,lin_ki,lin_kd,DIRECT); 

void setup() {
  pinMode(ENBL, OUTPUT);
  digitalWrite(ENBL,1);
  Serial.begin(115200); 
  pinMode(pin_photoint,INPUT);
  
//Initialize Encoder counts
  rotEnc.write(-4000); //set when pendulum is hanging straight down due to gravity
  linEnc.write(lin_Setpoint);  
 
 //PID Settings
     rot_Controller.SetMode(AUTOMATIC);
     rot_Controller.SetOutputLimits(-500,500); 
     rot_Controller.SetSampleTime(5); //2
     rot_Controller.SetTunings(rot_kp,rot_ki,rot_kd);
     
     lin_Controller.SetMode(AUTOMATIC);
     lin_Controller.SetOutputLimits(-300,300); 
     lin_Controller.SetSampleTime(5); //2
     lin_Controller.SetTunings(lin_kp,lin_ki,lin_kd);
}

void loop() {
  angle = rotEnc.read();
  lin_position = linEnc.read();
          if ((lin_position>limit_lin_min) && (lin_position<limit_lin_max)){       //perform as long carriage is within the linear limits  
                   if (angle < limit_angle && angle > (limit_angle*-1)){              //perform as long as pendulum is within the angular limits  
                                 //Execute linear PID controller
                                 lin_Controller.Compute();
                                 //Execute angular PID controller
                                  rot_Controller.Compute();
                                  y = a * y + (1.0 - a) * Output;
                                  Serial.println(y);        // Ausgabe der Stellgröße auf den sereillen Plotter
                                  digitalWrite(ENBL,0);
                                  if(y > output_deadband){
                                     stepper.begin(y, MICROSTEPS);
                                     stepper.move(-MICROSTEPS);
                                  }
                                  else if(y < -output_deadband){
                                     stepper.begin(abs(y), MICROSTEPS);
                                     stepper.move(MICROSTEPS);
                                 }
                             } //stop carrigage if pendulum has tipped out of the angular limits
                    else {
                         digitalWrite(ENBL,1);
                    }
                }  //stop carrigage out of the linear limits
                else {
                        digitalWrite(ENBL,1);
                }   
 } //End Loop