Pwm + L298n + As5040 basic servo type loop help

I have the basics of what im working functioning and that is

A: reading an AS5040 magnetic encoder position
B: reading in serial data
C: Writing PWM using the custom PWM lib
…but the last part
D: Controlling the speed and position of the DC motor depending on how far or close the the magnet is compared to the serial input im giving it…doesnt really work

Im thinking Im not storing the value of Angle for if statement in void loop_sv4() or something else im missing…any help would be appreciated

The values from the serial are in the range of 0 - 360 just like the angle…and I do a little basic math in that function to determine and map the PWM …but its erratic and not functioning

////////////////////// as5040
#include <PID_v1.h>  //for tuning
#include <PWM.h>

//AS5040 stuff
const int clockPin = 7; //output to clock
const int CSnPin = 6; //output to chip select
const int inputPin = 2; //read AS5040
int inputstream = 0; //one bit read from pin
long packeddata = 0; //two bytes concatenated from inputstream
long angle = 0; //holds processed angle value
long anglemask = 65472; //0x1111111111000000: mask to obtain first 10 digits with position info
long statusmask = 63; //0x000000000111111; mask to obtain last 6 digits containing status info
long statusbits; //holds status/error information
int DECn; //bit holding decreasing magnet field error data
int INCn; //bit holding increasing magnet field error data
int OCF; //bit holding startup-valid bit
int COF; //bit holding cordic DSP processing error data
int LIN; //bit holding magnet field displacement error data
int debug = 0; //SET THIS TO 0 TO DISABLE PRINTING OF ERROR CODES
int shortdelay = 1; // this is the microseconds of delay in the data clock

//PID stuff
double Setpoint, Input, Output;
PID myPID(&Input, &Output, &Setpoint,2,5,1, DIRECT);

// Serial for VVVV
String readString;

//PWM fast
int ENA = 9;//fast PWM
int IN1 = 12;
int IN2 = 13;
int32_t frequency = 16000; //frequency (in Hz)


void setup_sv4() 
{
  Serial.begin(115200);
  InitTimersSafe(); 
  bool success = SetPinFrequencySafe(ENA, frequency);
  pinMode (ENA, OUTPUT);
  pinMode (IN1, OUTPUT);
  pinMode (IN2, OUTPUT);
  //Serial.println("V4_Arduino_L298N_AS5040");//track what I have loaded
  
}

void setup()
{
  Serial.begin(9600);
  pinMode(clockPin, OUTPUT); // SCK
  pinMode(CSnPin, OUTPUT); // CSn -- has to toggle high and low to signal chip to start data transfer
  pinMode(inputPin, INPUT); // SDA
  
  setup_sv4();
}
void loop_sv4() {

  if (Serial.available())  
  {
    char c = Serial.read();  //gets one byte from serial buffer
    if (c == 'a') 
	{
		if (readString.length() >0) 
		{
			int SerialInput = readString.toInt();  //convert readString into a number
                        int Xaxis = angle;//magnet position
                        
                        if (SerialInput > Xaxis) //if serial is greater than the magnet position
                        {
			int DiffLeft = SerialInput - Xaxis;

                            digitalWrite (IN1, HIGH);
                            digitalWrite (IN2, LOW);
                            int Lspd = map(DiffLeft, 0, 360, 290, 510);//leftspeed should be a percentage of diff
                            pwmWrite  (ENA, (Lspd) );
                            
                            
                        }
                        else
                        {
                        int DiffRight = Xaxis - SerialInput;
                        digitalWrite (IN1, LOW);
                        digitalWrite (IN2, HIGH);
                        int Rspd = map(DiffRight, 0, 360, 290, 510);
                        pwmWrite  (ENA, (Rspd) );
                        readString=""; //clears variable for new input
                        }
                        
		}
	}  
    else if (c == 'b') 
	{

		if (readString.length() >0) 
		{
			int n = readString.toInt();  //convert readString into a number
			readString=""; //clears variable for new input
		}
	}  	
    else if (c == 'c') 
	{

		if (readString.length() >0) 
		{
			int n = readString.toInt();  //convert readString into a number
			readString=""; //clears variable for new input
		}
	}  	
	else 
	{     
		/// be sure this value is >= '0' && <= '9'
		readString += c; //makes the string readString
	}
  }
}



void loop()
{
  loop_sv4();
// CSn needs to cycle from high to low to initiate transfer. Then clock cycles. As it goes high
// again, data will appear on sda
  digitalWrite(CSnPin, HIGH); // CSn high
  digitalWrite(clockPin, HIGH); // CLK high
  delay(shortdelay);// time between readings
  digitalWrite(CSnPin, LOW); // CSn low: start of transfer
  delayMicroseconds(shortdelay); // delay for chip initialization
  digitalWrite(clockPin, LOW); // CLK goes low: start clocking
  delayMicroseconds(shortdelay); // hold low
  for (int x=0; x <16; x++) // clock signal, 16 transitions, output to clock pin
  {
    digitalWrite(clockPin, HIGH); //clock goes high
    delayMicroseconds(shortdelay); //
    inputstream =digitalRead(inputPin); // read one bit of data from pin
    packeddata = ((packeddata << 1) + inputstream);// left-shift summing variable, add pin value
    digitalWrite(clockPin, LOW);
    delayMicroseconds(shortdelay); // end of one clock cycle
  }

  angle = packeddata & anglemask; // mask rightmost 6 digits of packeddata to zero, into angle.
  angle = (angle >> 6); // shift 16-digit angle right 6 digits to form 10-digit value
  angle = angle * 0.3515; // angle * (360/1024) == actual degrees
  //Serial.print("angle: "); // and, finally, print it.
  Serial.println(angle, DEC);

  if (debug)
  {
    statusbits = packeddata & statusmask;
    DECn = statusbits & 2; // goes high if magnet moved away from IC
    INCn = statusbits & 4; // goes high if magnet moved towards IC
    LIN = statusbits & 8; // goes high for linearity alarm
    COF = statusbits & 16; // goes high for cordic overflow: data invalid
    OCF = statusbits & 32; // this is 1 when the chip startup is finished.
    if (DECn && INCn) { Serial.println("magnet moved out of range"); }
    else
    {
      if (DECn) { Serial.println("magnet moved away from chip"); }
      if (INCn) { Serial.println("magnet moved towards chip"); }
    }
    if (LIN) { Serial.println("linearity alarm: magnet misaligned? Data questionable."); }
    if (COF) { Serial.println("cordic overflow: magnet misaligned? Data invalid."); }
  }

  packeddata = 0; // reset both variables to zero so they don't just accumulate
  angle = 0;
  
  
}

sorry had the wrong sketch..fixed now