volatile int c = 0;
const byte interruptPin = 3;
const byte ledPin = 3;


volatile float last_time = 0;
volatile float current_time = 0;


volatile float freq = 1;
volatile float delaytime = 1000/freq;
volatile int brightness = 255;
String input = "";
int steering_angle = 0;
int speed_ref = 0;
int old_speed_ref = -1;
int old_steering_angle = -1;

void setup() {
  // put your setup code here, to run once:
    pinMode(ledPin, OUTPUT);
    pinMode(interruptPin, INPUT_PULLUP);
    //attachInterrupt(digitalPinToInterrupt(interruptPin), blink, RISING);
    Serial.begin(9600);
    Serial.println("setup");
}


void loop() {
    if (Serial.available() > 0) { 
        input = Serial.readStringUntil('\n');
        int split_index = input.indexOf('-');
        steering_angle = input.substring(0,split_index).toInt();
        speed_ref = input.substring(split_index+1).toInt();        
        if (old_speed_ref != speed_ref || old_steering_angle != steering_angle){
            //pid.set_velocity(int_input);
            //steering_servo.write(steering_angle);
            old_speed_ref = speed_ref;
            old_steering_angle = steering_angle;
            Serial.print("Speed ref:");
            Serial.println(speed_ref);
            Serial.print("Steering angle:");
            Serial.println(steering_angle);
        }
    }
}