/* ======== USER INTERFACE TEMPLATE ============= */
/*  Insert below the code for your scaling, saturation block, and controllers.*/

/*  Variables may be declared on the box border, as shown for the input
 "Tms" and the output "BallPosn". Variables can also be declared inline as was done for "Temp1". */

float Temp1;
float eGearAng;

// T = 0.001

float e;
float outerErr;

float u_old_coeff = 0.968181072658685;
float e_coeff = -3.451295475976174;
float e_old_coeff = 3.342328438925280;

float ThRef_old_coeff = 0.997503121098627;
float outerErr_coeff = 6.992484394506866;
float outerErr_old_coeff = -6.990037453183520;

float ThRefSat;

/*  Shift registers permit previous values of variables to be saved.
The output variable "e" is wired to a shift register input on the For Loop border.
The inputs "e1" and "e2"are wired  to the corresponding shift register outputs.
 "e1" holds the value of "e" from the previous iteration and "e2" holds the value of "e1" from  the previous iteration. */

/* Place your sensor SCALING here */
/* NO scaling is provided for the demo */

BallPosn = 0.103*posV-0.342; //0.105*posV-0.342;  /* V to V */
//ServoAng = angV/(-0.589) + 7.234  + 0.3; /* V to V */
ServoAng = angV/(-0.65317)  + 6.817 + 0.055;
/* SCALING end */

if (Loop  < 3) /* all shift registers cleared after 3rd iteration; this statement initializes the shift registers */
   {u = e = outerErr =ThRef = posV= angV =ServoAng= BallPosn= 0;}
else
{
if (Manual)    /*manual motor voltage control*/
   {   u = MotV;}
    else     /*control algorithm*/
   {

/* CAUTION: DO NOT load the output of a nonlinear block (e.g., saturator, offset) into a SHIFT REGISTER,
to avoid introducing a nonlinearity into your controller loop. Create separate variables to hold nonlinear values.*/

/* Place your outer loop BALL POSITION CONTROLLER below */
outerErr =  ref - BallPosn;
// question b)
//ThRef = ThRef_old_coeff*ThRef1 - outerErr_coeff*outerErr - outerErr_old_coeff*outerErr1;

// question c)
//ThRef =  1.993*ThRef1 - 0.993*ThRef2 - 30*outerErr + 59.98599*outerErr1 - 29.98599007*outerErr2;

// question d)
ThRef = 1.996*ThRef1 - 0.996*ThRef2 - 19*outerErr + 37.987985*outerErr1 - 18.98798506*outerErr2;

ThRefSat = ThRef;

if (ThRefSat > 0) {
ThRefSat += 0.083;
} else if (ThRefSat < 0) {
ThRefSat -= 0.083;
}

/* Place your gear angle SATURATOR below */
if(ThRefSat < (-0.7)) {
ThRefSat = -0.7;
} else if(ThRefSat > (0.7)) {
ThRefSat = 0.7;
}

/* Place your inner loop GEAR ANGLE CONTROLLER below */
e = ThRefSat - ServoAng;

u = u_old_coeff*u1 + e_coeff*e + e_old_coeff*e1;
   }
}

/* ThRef, ThRef1, e, e1 are present, but not used in this demo.
However, they will be necessary (at a minimum) when the controllers will be implemented. */