Basic PID library

#pragma region VEXcode Generated Robot Configuration
// Make sure all required headers are included.
#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>
#include <math.h>
#include <string.h>


#include "vex.h"

using namespace vex;

// Brain should be defined by default
brain Brain;


// START V5 MACROS
#define waitUntil(condition)                                                   \
  do {                                                                         \
    wait(5, msec);                                                             \
  } while (!(condition))

#define repeat(iterations)                                                     \
  for (int iterator = 0; iterator < iterations; iterator++)
// END V5 MACROS


// Robot configuration code.
motor Motor1 = motor(PORT4, ratio18_1, false);




// Helper to make playing sounds from the V5 in VEXcode easier and
// keeps the code cleaner by making it clear what is happening.
void playVexcodeSound(const char *soundName) {
  printf("VEXPlaySound:%s\n", soundName);
  wait(5, msec);
}

#pragma endregion VEXcode Generated Robot Configuration

// Include the V5 Library
#include "vex.h"

// Allows for easier use of the VEX Library
using namespace vex;

int main (){
  Motor1.setMaxTorque(20, percent);
  Motor1.spin(forward);
  float errorSum = 0.0;
    float target = 50.0;
    float pGain = 0.4;
    float iGain = 0.0;
    float dGain = 0.05;
    while(true){
    float r1 = Motor1.position(degrees);
    wait(10, msec);
    float r2 = Motor1.position(degrees);

    float error = target - r2;
    errorSum = error + errorSum;
    float deltaV = (r2 - r1) / 0.01;

    Motor1.setVelocity((error * pGain) + (errorSum * iGain) + (deltaV * dGain), percent);
    }

    return 0;
}