TBH Algorithm Help!

We are trying to implement the TBH algorithm into our dual flywheels. I have attached the program that we are currently running. However, the algorithm is not working. The dual flywheels are in the ports 4,5,8,9 with the Encoder on RLF (Port 9). If you could please take a look at the code to see if something is wrong, that would be awesome! Thank you everyone!!

#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, I2C_1, righty, sensorQuadEncoderOnI2CPort, , AutoAssign )
#pragma config(Motor, port2, DL, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port3, DR, tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor, port4, LLF, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port5, LLR, tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor, port6, BC, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port7, LIFT, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port8, RLR, tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor, port9, RLF, tmotorVex393_MC29, openLoop, encoderPort, I2C_1)
//!!Code automatically generated by ‘ROBOTC’ configuration wizard !!//

// I apologize on behalf of my predecessor for the motor names

#pragma competitionControl(Competition)
#pragma autonomousDuration(20)
#pragma userControlDuration(100)

#include “Vex_Competition_Includes.c”

//TBH Algorithm based on the work of our lord and savior James Pearman
//Original code- https://gist.github.com/jpearman/6761b53919101f07c400
//Should check that to see what I inevitably mess up

// Update interval (in ms) for the flywheel control loop
#define LOOP_SPEED 25

// Maximum power we want to send to the flywheel motors
#define MAX_POWER 127

// encoder counts per revolution depending on motor type
#define MOTOR_TPR_269 240.448
#define MOTOR_TPR_393R 261.333
#define MOTOR_TPR_393S 392
#define MOTOR_TPR_393T 627.2
#define MOTOR_TPR_QUAD 360.0

// Structure to gather all the needed data
typedef struct
{
long loopCounter; ///< loop counter used for debug

// encoder tick per revolution
float ticksPerRev;

// Encoder stuff
long currentEncoderCount;                   ///< current encoder count
long lastEncoderCount;                      ///< last encoder count
float currentRPM;                           ///< current velocity in rpm - calculated from encoder stuff
long lastEncoderCheckTime;                  ///< Time of last velocity calculation

// TBH control algorithm variables
long targetVelocity;                        ///< target velocity
long currentVelocity;                       ///< current velocity
long lastVelocity;                          ///< velocity at last calculation
float error;                                ///< error between actual and target velocities
float lastError;                            ///< error from last calculation
float gain;                                 ///< gain -> we set this, determines rate of acceleration
float drive;                                ///< percentage of max output sent to motors (0.0 to 1.0)
float driveAtZero;                          ///< drive at last “zero crossing”
long firstCross;                            ///< flag indicating whether zero crossing has happened yet
float driveApprox;                          ///< estimated drive needed

// final motor drive
long motorDrive;            ///< final motor control value

} tbhController;

static tbhController shooter; ///<the actual variable for the struct
bool slowMode = false; ///<to keep track of whether shooter should be slow or fast (start fast)

//Speed variables - will have to be tested heavily once we know what we’re doing
const int fast = 50;
const int slow = 25;

//Predicted Drive variables; general guess on what the output should be to start guessing from
const float probsFast = 0.5;
const float probsSlow = 0.25;

//Sets shooter motors to value
void setShooterValue(int value)
{
motor[RLF] = value;
motor[LLF] = value;
motor[LLR] = value;
motor[RLR] = value;
}

//May have to change this to account for two IMEs
long getShooterEncoder()
{
return getMotorEncoder(RLF);
}

//Unsure if these variables need to be pointers, but don’t want to see what happens if they’re not
void setVelocity(tbhController *launcher, int velocity, float predictedDrive)
{
launcher->targetVelocity = velocity;

// Set error so zero crossing is correctly detected
launcher->error = launcher->targetVelocity - launcher->currentVelocity;
launcher->lastError = launcher->error;

launcher->driveApprox  = predictedDrive;

// firstCross set to 1 to signify that we have not crossed zero yet
launcher->firstCross = 1;
launcher->driveAtZero = 0;

}

void calculateSpeed(tbhController *launcher)
{
int delta_ms;
int delta_enc;

// Get current encoder value
launcher->currentEncoderCount = getShooterEncoder();

// Check time this was last called for calculation stuff
delta_ms = nSysTime - launcher->lastEncoderCheckTime;
launcher->lastEncoderCheckTime = nSysTime;

// Change in encoder count
delta_enc = (launcher->currentEncoderCount - launcher->lastEncoderCount);

// save last position
launcher->lastEncoderCount = launcher->currentEncoderCount;

// Calculate velocity in rpm
launcher->currentRPM = (1000.0 / delta_ms) * delta_enc * 60.0 / launcher->ticksPerRev;

}

void updateTBH(tbhController *launcher)
{
// calculate error in velocity between target (desired) and current velocity
launcher->error = launcher->targetVelocity - launcher->currentVelocity;

// Adjust drive based on error calculated above, at speed determined by gain
launcher->drive =  launcher->drive + (launcher->error * launcher->gain);

// Make sure drive is in proper range
if(launcher->drive > 1)
      launcher->drive = 1;
if(launcher->drive < 0)
      launcher->drive = 0;

// Check for zero crossing (Note to self: sgn checks the sign on the number)
if(sgn(launcher->error) != sgn(launcher->lastError))
{
    // If firstCross is 1, meaning that zero error has not been reached yet, this evaluates to true
    if(launcher->firstCross)
    {
        // Set drive to the open loop approximation
        launcher->drive = launcher->driveApprox;
        launcher->firstCross = 0;
    }
    else
        launcher->drive = 0.5 * (launcher->drive + launcher->driveAtZero);

    // Save this drive value in the "tbh" variable
    launcher->driveAtZero = launcher->drive;
}

// Save last error
launcher->lastError = launcher->error;

}

task shooterControlTask()
{
tbhController *launcher = &shooter;

// Set the gain
launcher->gain = 0.00025;

// We are using the default gearing on the motors
launcher->ticksPerRev = MOTOR_TPR_393T;

while(true)
{
    // debug counter
    launcher->loopCounter++;

    // Calculate velocity
    calculateSpeed(launcher);

    // Set current speed for the tbh calculation code
    launcher->currentVelocity = launcher->currentRPM;

    // Do the velocity TBH calculations
    updateTBH(launcher);

    // Scale drive into the range the motors need - unsure of exact reasoning here(?)
    launcher->motorDrive  = (launcher->drive * MAX_POWER);

    // Contingency plan should terrible things happen
    if(launcher->motorDrive > 127)
    	launcher->motorDrive = 127;
    if(launcher->motorDrive < -127)
    	launcher->motorDrive = -127;

    // and finally set the motor control value
    setShooterValue(launcher->motorDrive);

    // Run at somewhere between 20 and 50mS
    wait1Msec(LOOP_SPEED);
}

}

void pre_auton()
{
resetMotorEncoder(LLF);
}

// We haven’t even messed with this part yet
task autonomous()
{
startTask(shooterControlTask);
setVelocity(&shooter, fast, probsFast);
wait1Msec(4000);
while (true)
{
motor[BC] = 127;
motor[LIFT] = 127;
wait1Msec(1500);

	motor[BC] = 0;
	motor[LIFT] = 0;
	wait1Msec(1500);
}

}

task usercontrol()
{
startTask(shooterControlTask);
setVelocity(&shooter, fast, probsFast); ///<Sorry about terrible variable names

//Ball intake is always going!
motor[BC] = 127; 
while(true)
{
//Drive the robot!
motor[DL] = vexRT[Ch3] + vexRT[Ch1];
motor[DR] = vexRT[Ch3] - vexRT[Ch1];

//Whether or not to operate lift
if (vexRT[Btn6U] == 1)
{
	motor[LIFT] = 127;
}
else
{
	motor[LIFT] = 0;
}

if (vexRT[Btn7U] == 1)
{
	slowMode = !slowMode;
	if (slowMode)
		setVelocity(&shooter, slow, probsSlow);
	else
		setVelocity(&shooter, fast, probsFast);
}
wait1Msec(10);  ///<This might need to be longer, actually?
}

}

-Matt

So what’s the problem? I tried your code and it does work, however, you will need to set predicted drive values and gain for your system. The Btn7U check needs some TLC (wait for release) as it will toggle “slowMode” quickly if pressed.