Code.c

/* The P is a really small value, so lets say the high speed is 80 and the low speed is 20, if you are 1000 rpm
 * away it might go at 110 but as you get close it will decrease to 80, and if you are 1000 rpm above it would 
 * at 0 and as you get slower and approach the target speed you will get closer to 20 just going 127 and 0 will
 * cause you to overshoot and drop over and over again, using P ontop of it will help reduce the amount of drop
 * and overshoot while still having the fast recovery time of bang bang.
 *
 * Formula: highspeed + difference in rpm * P constant, lowspeed + differnece in rpm * P constant
 */

float kP = 0.0;
float veloTime = 0;

int speedA = 75;
int speedB = 0;
int veloA = 1200;
int veloB = 400;
int motorSpeedA = 0;
int motorSpeedB = 0;
int currVelo = 0;
int posA = 0;
int posB = 0;
int TPR = 392;

void FwCalculateSpeed( fw_controller *fw )
{
    int     delta_ms;
    int     delta_enc;
    
    // Get current encoder value
    posA = nMotorEncoderA;

    // This is just used so we don't need to know how often we are called
    // how many mS since we were last here
    delta_ms   = nSysTime - veloTime;
    veloTime = nSysTime;
    
    // Change in encoder count
    delta_enc = (posA - posB);
    
    // save last position
    posB = posA;
    
    // Calculate velocity in rpm
    curVelo = (1000.0 / delta_ms) * delta_enc * 60.0 / TPR;
}

task bbP(){
    motorSpeedA = speedA + currVelo * kP;
    motorSpeedB = speedB + currVelo * kP;
    
    if(currVelo < veloA) {
        motor[m1] = motorSpeedA;
    } else if(currVelo > veloB) {
        motor[m1] = motorSpeedB;
    }
}