show sylus this working but potentially wrong code.txt

it is a xyz file of a ball and hoop that i wrote using matlab

// ----------------------------------------
// Filename:
//   control.c
// Description:
//   Control routine that is called periodically
// Author:
//   PMcL
// Date:
//   10-May-07
// Version:
//   1.17

// Control
#include "control.h"

// Control code goes into a special memory location
#pragma CODE_SEG CONTROL_CODE
#pragma DATA_SEG CONTROL_DATA

// ----------------------------------------
// Global variables

// Reference
TFloat Reference;

// Input variables
double X[NB_INPUT_CHANNELS][NB_PAST_VALUES];

// Output variables
double Y[NB_OUTPUT_CHANNELS][NB_PAST_VALUES];

// Intermediate variables
double Z[NB_INTERMEDIATE_VARIABLES][NB_PAST_VALUES];

// Advanced control parameters
TFloat CP[NB_CONTROL_PARAMETERS];

// ----------------------------------------
// Short-cut macros for variables

#define Ref Reference.d

#define CP1 CP[0].d
#define CP2 CP[1].d
#define CP3 CP[2].d
#define CP4 CP[3].d
#define CP5 CP[4].d
#define CP6 CP[5].d
#define CP7 CP[6].d
#define CP8 CP[7].d

#define X1 X[Ch1]
#define X2 X[Ch2]
#define X3 X[Ch3]
#define X4 X[Ch4]

#define Y1 Y[Ch1]
#define Y2 Y[Ch2]

#define Z1 Z[0]
#define Z2 Z[1]
#define Z3 Z[2]
#define Z4 Z[3]

// ----------------------------------------
// Control_DoControl
// 
// Performs a control algorithm from input to output
// Input:
//   none
// Output:
//   none
// Conditions:
//   Calculations must be limited to less than 1 ms

void Control_DoControl(void)
{

// hoop variables

double MAX_Out[] = {2, 0.8, 0, -0.8, -2};

int FAM[5][5] = {{4, 4, 3, 3, 3}, 
                 {4, 3, 3, 3, 1}, 
                 {3, 3, 2, 1, 1}, 
                 {3, 1, 1, 1, 0},
                 {1, 1, 1, 0, 0}};
        
double Error[] = {0, 0, 0, 0, 0};
double Change[] = {0, 0, 0, 0, 0};

double top;
double bottom;      
double calc;
      
double cur_e;
double cur_c;

int i;
int k;

// ball variables

double MAX_Out_ball[] = {-1.8, -0.75, 0, 0.75, 1.8};

int FAM_ball[5][5] = {{4, 4, 3, 3, 3}, 
                      {4, 3, 3, 2, 1}, 
                      {3, 3, 2, 1, 1}, 
                      {3, 2, 1, 1, 0},
                      {1, 1, 1, 0, 0}};
        
double Error_ball[] = {0, 0, 0, 0, 0};
double Change_ball[] = {0, 0, 0, 0, 0};

double top_ball;
double bottom_ball;      
double calc_ball;
      
double cur_e_ball;
double cur_c_ball;

int m;
int n;

// hoop fuzzy

cur_e = Ref - X1[0];
cur_c = (X1[0]-X1[1])/0.02;

if (cur_e < -10) {
        Error[0] = 1;
}
    else if (cur_e < -5) {
            Error[0] = -0.2*(cur_e) -1;
            Error[1] = 0.2*(cur_e) +2;
    }
        else if (cur_e < 0) {
                Error[1] = -0.2*(cur_e);
                Error[2] = 0.2*(cur_e) +1; 
        }
            else if (cur_e < 5){
                   Error[2] = -0.2*(cur_e) +1;
                   Error[3] = 0.2*(cur_e); 
            }
                else if (cur_e < 10) {
                        Error[3] = -0.2*(cur_e) +2;
                        Error[4] = 0.2*(cur_e) -1; 
                }
                    else {
                      Error[4] = 1; 
                    }
                    
if (cur_c < -80) {
        Change[0] = 1;
}
    else if (cur_c < -40) {
            Change[0] = -0.025*(cur_c) -1;
            Change[1] = 0.025*(cur_c) +2;
    }
        else if (cur_c < 0) {
                Change[1] = -0.025*(cur_c) +0;
                Change[2] = 0.025*(cur_c) +1; 
        }
            else if (cur_c < 40){
                   Change[2] = -0.025*(cur_c) +1;
                   Change[3] = 0.025*(cur_c); 
            }
                else if (cur_c < 80) {
                        Change[3] = -0.025*(cur_c) +2;
                        Change[4] = 0.025*(cur_c) +-1; 
                }
                    else {
                      Change[4] = 1; 
                    }



top = 0;
bottom = 0;

for (i = 0; i < 5; i++){
    for (k = 0; k < 5; k++){
        calc = Error[i] * Change[k];
        top = top + calc * MAX_Out[FAM[k][i]];
        bottom = bottom + calc;
    };
};

    

  Z1[0] = top/bottom;
  
// ball fuzzy

cur_e_ball = 0 - X2[0];
cur_c_ball = (X2[0]-X2[1])/0.02;

if (cur_e_ball < -6) {
        Error_ball[0] = 1;
}
    else if (cur_e_ball < -3) {
            Error_ball[0] = -0.333333333333333*(cur_e_ball) -1
;
            Error_ball[1] = 0.333333333333333*(cur_e_ball) +2
;
    }
        else if (cur_e_ball < 0) {
                Error_ball[1] = -0.333333333333333*(cur_e_ball) 
;
                Error_ball[2] = 0.333333333333333*(cur_e_ball) +1
; 
        }
            else if (cur_e_ball < 3){
                   Error_ball[2] = -0.333333333333333*(cur_e_ball) +1
;
                   Error_ball[3] = 0.333333333333333*(cur_e_ball) 
; 
            }
                else if (cur_e_ball < 6) {
                        Error_ball[3] = -0.333333333333333*(cur_e_ball) +2
;
                        Error_ball[4] = 0.333333333333333*(cur_e_ball) -1
; 
                }
                    else {
                      Error_ball[4] = 1; 
                    }
                    
if (cur_c_ball < -300) {
        Change_ball[0] = 1;
}
    else if (cur_c_ball < -150) {
            Change_ball[0] = -0.00666666666666667*(cur_e_ball) -1

;
            Change_ball[1] = 0.00666666666666667*(cur_e_ball) +2

;
    }
        else if (cur_c_ball < 0) {
                Change_ball[1] = -0.00666666666666667*(cur_e_ball)
 
;
                Change_ball[2] = 0.00666666666666667*(cur_e_ball) +1

; 
        }
            else if (cur_c_ball < 150){
                   Change_ball[2] = -0.00666666666666667*(cur_e_ball) +1

;
                   Change_ball[3] = 0.00666666666666667*(cur_e_ball)
 
; 
            }
                else if (cur_c_ball < 300) {
                        Change_ball[3] = -0.00666666666666667*(cur_e_ball) +2

;
                        Change_ball[4] = 0.00666666666666667*(cur_e_ball) -1

; 
                }
                    else {
                      Change_ball[4] = 1; 
                    }
top_ball = 0;
bottom_ball = 0;

for (m = 0; m < 5; m++){
    for (n = 0; n < 5; n++){
        calc_ball = Error_ball[m] * Change_ball[n];
        top_ball = top_ball + calc_ball * MAX_Out_ball[FAM[n][m]];
        bottom_ball = bottom_ball + calc_ball;
    };
};


  Z2[0] = top_ball/bottom_ball;

// output
if (Z1[0] < 0.6 && Z1[0] > 0.05){ 
  Y1[0] = 0.6;
}
    else if (Z1[0] > -0.6 && Z1[0] < -0.05){
      Y1[0] = -0.6;
    }
    else{
      Y1[0] = Z1[0]+Z2[0];
    }

  Y2[0]= Z1[0]+Z2[0];  
  
 }

#pragma CODE_SEG DEFAULT
#pragma DATA_SEG DEFAULT