#include <joyos.h>
#include <lib/pid.h>

#define GOAL_HEADING 45.0
#define FORWARD_VEL 150

int16_t limitVel(float vel){
  if (vel < 0) return 0;
  if (vel > 255) return 255;
  return (int16_t)vel;
}

void setVelocities(float correctionAmount){
  motor_set_vel(0, limitVel(FORWARD_VEL - correctionAmount));
  motor_set_vel(1, limitVel(FORWARD_VEL + correctionAmount));
}


int umain(){
  float kP = 2; //proportional gain
  float kI = 0; //integral gain
  float kD = -0.5; //derivative gain

  struct pid_controller headingPID;

  init_pid( &headingPID,
            kP,
            kI,
            kD,
            &gyro_get_degrees,
            &setVelocities);

  headingPID.goal = 45.0;
  headingPID.enabled = true;

  while (true){
    update_pid(&headingPID);
  }
  return 0;
}

int usetup(){
  gyro_init (11, 1400000, 500L);
  return 0;
}