TBH code having issues

My team has a 25:1 dual flywheel with two torque motors on each side. I have one IEM on each side. I’ve been having issues with the tbh code I am trying to implement. When I attempt to turn on the flywheel with any speed setting, the motors go up to 127 and don’t stop unless I turn off the cortex. Each IEM shows the solid green light, which indicates the motors going at 127. I’m not sure what is causing this and I really need to fix it so I can start tuning for our competition next Saturday. I’ve gone through the code a few times and replaced the IEMs, but there may be something I’m not picking up. Also, this code is based on the tbh code posted by Adam T in this thread. Any help is highly appreciated! My code is in the next post.

#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, I2C_1,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Sensor, I2C_2,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Motor,  port1,           INB,           tmotorVex393_HBridge, openLoop)
#pragma config(Motor,  port2,           FFL,           tmotorVex393_MC29, openLoop, encoderPort, I2C_1)
#pragma config(Motor,  port3,           FBL,           tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port4,           DFR,           tmotorVex393_MC29, openLoop, reversed, encoderPort, None)
#pragma config(Motor,  port5,           DBR,           tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port6,           DFL,           tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port7,           DBL,           tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port8,           FFR,           tmotorVex393_MC29, openLoop, reversed, encoderPort, I2C_2)
#pragma config(Motor,  port9,           FBR,           tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port10,          INF,           tmotorVex393_HBridge, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#pragma platform(VEX)

//based on /u/Adam T's code on vexforums
//Competition Control and Duration Settings
#pragma competitionControl(Competition)
#pragma autonomousDuration(20)
#pragma userControlDuration(120)

#include "Vex_Competition_Includes.c"   //Main competition background code...do not modify!


// Update inteval (in mS) for the flywheel control loop
#define FW_LOOP_SPEED              25

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

// encoder counts per revolution depending on motor
#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

// encoder tick per revolution
float           ticks_per_rev;          ///< encoder ticks per revolution

// Encoder
long            encoder_countsR;         ///< current encoder count
long            encoder_counts_lastR;    ///< current encoder count
long            encoder_countsL;
long            encoder_counts_lastL;

// velocity measurement
float           motor_velocityR;         ///< current velocity in rpm
float           motor_velocityL;
long            nSysTime_lastR;          ///< Time of last velocity calculation
long            nSysTime_lastL;

// TBH control algorithm variables
long            target_velocityR;        ///< target_velocity velocity
long            target_velocityL;
float           current_errorR;          ///< error between actual and target_velocity velocities
float           current_errorL;          ///< error between actual and target_velocity velocities
float           last_errorR;             ///< error last time update called
float           last_errorL;
float           gain;                    ///< gain
float           driveR;                  ///< final drive out of TBH (0.0 to 1.0) RIGHT
float           driveL;                  /// LEFT
float           drive_at_zeroR;          ///< drive at last zero crossing RIGHT
float           drive_at_zeroL;          ///LEFT
long            first_crossR;            ///< flag indicating first zero crossing RIGHT
long            first_crossL;            /// LEFT
float           drive_approxR;           ///< estimated open loop drive RIGHT
float           drive_approxL;           ///LEFT

// final motor drive
long            motor_driveR;            ///< final motor control value RIGHT
long            motor_driveL;            /// LEFT





void pre_auton()
{
  // Set bStopTasksBetweenModes to false if you want to keep user created tasks running between
  // Autonomous and Tele-Op modes. You will need to manage all user created tasks if set to false.
  bStopTasksBetweenModes = true;

	// All activities that occur before the competition starts
	// Example: clearing encoders, setting servo positions, ...
}


/*Set the flywheel motors RIGHT  */
void
FwMotorSetR( int valueR )
{
	slaveMotor(port9, port8);
	motor FFR ] = valueR;
}

/*Set the flywheen motors LEFT */
void
FwMotorSetL( int valueL )
{
	slaveMotor(port3, port2);
	motor FFL ] = valueL;
}


/*Get the flywheen motor encoder count RIGHT*/
long
FwMotorEncoderGetR()
{
	return( nMotorEncoder FFR ] );
}


/*Get the flywheen motor encoder count LEFT */
long
FwMotorEncoderGetL()
{
	return( nMotorEncoder FFL ] );
}


/*Set the controller position RIGHT */
void
FwVelocitySetR( int velocityR, float predicted_driveR )
{
	// set target_velocity velocity (motor rpm)
	target_velocityR = velocityR;

	// Set error so zero crossing is correctly detected
	current_errorR = target_velocityR - motor_velocityR;
	last_errorR    = current_errorR;

	// Set predicted open loop drive value
	drive_approxR  = predicted_driveR;
	// Set flag to detect first zero crossing
	first_crossR   = 1;
	// clear tbh variable
	drive_at_zeroR = 0;
}


/*Set the controller position LEFT  */
void
FwVelocitySetL( int velocityL, float predicted_driveL )
{
	// set target_velocity velocity (motor rpm)
	target_velocityL = velocityL;

	// Set error so zero crossing is correctly detected
	current_errorL = target_velocityL - motor_velocityL;
	last_errorL    = current_errorL;

	// Set predicted open loop drive value
	drive_approxL  = predicted_driveL;
	// Set flag to detect first zero crossing
	first_crossL   = 1;
	// clear tbh variable
	drive_at_zeroL = 0;
}



/*Calculate the current flywheel motor velocity*/
void
FwCalculateSpeed()
{
	int     delta_msR;
	int     delta_msL;
	int     delta_encR;
	int     delta_encL;

	// Get current encoder value
	encoder_countsR = FwMotorEncoderGetR();
	encoder_countsL = FwMotorEncoderGetL();

	// 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_msR = nSysTime - nSysTime_lastR;
	nSysTime_lastR = nSysTime;
	delta_msL = nSysTime - nSysTime_lastL;
	nSysTime_lastL = nSysTime;

	// Change in encoder count
	delta_encR = (encoder_countsR - encoder_counts_lastR);
	delta_encL = (encoder_countsL - encoder_counts_lastL);

	// save last position
	encoder_counts_lastR = encoder_countsR;
	encoder_counts_lastL = encoder_countsL;

	// Calculate velocity in rpm
	motor_velocityR = (1000.0 / delta_msR) * delta_encR * 60.0 / ticks_per_rev;
	motor_velocityL = (1000.0 / delta_msL) * delta_encL * 60.0 / ticks_per_rev;
}

/*Update the velocity tbh controller variables RIGHT*/
void
FwControlUpdateVelocityTbhR()
{
	// calculate error in velocity
	// target_velocity is desired velocity
	// current is measured velocity
	current_errorR = target_velocityR - motor_velocityR;

	// Calculate new control value
	driveR =  driveR + (current_errorR * gain);

	// Clip to the range 0 - 1.
	// We are only going forwards
	if( driveR > 1 )
		driveR = 1;
	if( driveR < 0 )
		driveR = 0;

	// Check for zero crossing
	if( sgn(current_errorR) != sgn(last_errorR) ) {
		// First zero crossing after a new set velocity command
		if( first_crossR ) {
			// Set drive to the open loop approximation
			driveR = drive_approxR;
			first_crossR = 0;
		}
		else
			driveR = 0.5 * ( driveR + drive_at_zeroR );

		// Save this drive value in the "tbh" variable
		drive_at_zeroR = driveR;
	}

	// Save last error
	last_errorR = current_errorR;
}


/*Update the velocity tbh controller variables */
void
FwControlUpdateVelocityTbhL()
{
	// calculate error in velocity
	// target_velocity is desired velocity
	// current is measured velocity
	current_errorL = target_velocityL - motor_velocityL;

	// Calculate new control value
	driveL =  driveL + (current_errorL * gain);

	// Clip to the range 0 - 1.
	// We are only going forwards
	if( driveL > 1 )
		driveL = 1;
	if( driveL < 0 )
		driveL = 0;

	// Check for zero crossing
	if( sgn(current_errorL) != sgn(last_errorL) ) {
		// First zero crossing after a new set velocity command
		if( first_crossL ) {
			// Set drive to the open loop approximation
			driveL = drive_approxL;
			first_crossL = 0;
		}
		else
			driveL = 0.5 * ( driveL + drive_at_zeroL );

		// Save this drive value in the "tbh" variable
		drive_at_zeroL = driveL;
	}

	// Save last error
	last_errorL = current_errorL;
}

/*Task to control the velocity of the flywheel */
task
FwControlTask()
{

	// Set the gain
	gain = 0.00050;

	// We are using torque motors
	// Set the encoder ticks per revolution
	ticks_per_rev = MOTOR_TPR_393T;

	while(1)
	{
		// Calculate velocity
		FwCalculateSpeed();

		// Do the velocity TBH calculations
		FwControlUpdateVelocityTbhR() ;
		FwControlUpdateVelocityTbhL() ;

		// Scale drive into the range the motors need
		motor_driveR  = (driveR * FW_MAX_POWER) + 0.5;
		motor_driveL  = (driveL * FW_MAX_POWER) + 0.5;

		// Final Limit of motor values - don't really need this
		if( motor_driveR >  127 ) motor_driveR =  127;
		if( motor_driveR < -127 ) motor_driveR = -127;
		if( motor_driveL >  127 ) motor_driveL =  127;
		if( motor_driveL < -127 ) motor_driveL = -127;

		// and finally set the motor control value
		FwMotorSetR( motor_driveR );
		FwMotorSetL( motor_driveL );

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

task autonomous()
{
	  
}

task usercontrol()
{
	// User control code here, inside the loop

	// Start the flywheel control task
	startTask( FwControlTask );


	// Main user control loop
	while(1)
	{
	//Drive
  	 //Right side of the robot is controlled by the right joystick, Y-axis
    motor[DFR] = vexRT[Ch2];
    motor[DBR]  = vexRT[Ch2];
    //Left side of the robot is controlled by the left joystick, Y-axis
    motor[DFL] = vexRT[Ch3];
    motor[DBL]  = vexRT[Ch3];

   //Intake
    //front
	if(vexRT[Btn6U]==1)
		{
			motor[INB]=120;
		}
		else if(vexRT[Btn6D]==1)
		{
			motor[INB]=-120;
		}
		else
		{
			motor[INB]=0;
		}



       //back
			if(vexRT[Btn5D]==1)
		{
			motor[INF]=120;
		}
		else if(vexRT[Btn5U]==1)
		{
			motor[INF]=-120;
		}
		else
		{
			motor[INF]=0;
		}


		// Different speeds set by buttons
		if( vexRT Btn8L ] == 1 )
		{
			FwVelocitySetR( 138, 0.52);
			FwVelocitySetL( 138, 0.52);
		}
		//if( vexRT Btn8U ] == 1 )
		//{
		//	FwVelocitySetR( 120, 0.38 );
		//	FwVelocitySetL( 120, 0.38 );
		//}
		 if( vexRT Btn8R ] == 1 )
		{
			FwVelocitySetR( 67,0.25);
			FwVelocitySetL( 67,0.25);
		}
		 if( vexRT Btn8U ] == 1 )
		{
			FwVelocitySetR( 53,0.21);
			FwVelocitySetL( 53,0.21);
		}
		 if( vexRT Btn8D ] == 1 )
		{
			FwVelocitySetR( 0, 0 );
			FwVelocitySetL( 0, 0 );
		}
	// Don't hog the cpu :)
		wait1Msec(10);

	}
}

I tried your code with just one motor in port2 (first IME). It works for me with an unloaded motor. Do you see encoder counts changing in the debugger? Are the IMEs installed correctly? Good cables, etc. The green leds should flash occasionally when the motor is not running. Also, not related, but probably best to only call slaveMotor once at the beginning of user control.

I think you may want to initialize some of these variables. Those values could be anything from the time calculation to DriveR or DriveL.

If DriveR was something >1 that would be stuck at 1 to start and cascading throughout keeping the motors at 127.
That makes your current error possibly weird and that then cascades too going down by half but always clipped to 127 motor power.

What does the debugger say about your variables? Care to log some data and see what is happening?

Ok, I see a problem.

FwVelocitySetR( 138, 0.52);

You want to run at 138 rpm, but…

	// We are using torque motors
	// Set the encoder ticks per revolution
	ticks_per_rev = MOTOR_TPR_393T

Max speed for a torque motor is somewhere around 100~110 rpm.

@jpearman The green light flashes twice on I2C_2 and once on I2C_1 every few seconds. Additionally, all my cables are connected properly. I moved slaveMotor and changed the target velocity. Staying below 100 did not fix the issue, unfortunately. Even the 0 velocity/off button does not work.

@Team80_Giraffes Where exactly would I have to initialize those variables? Since this code is adapted from jpearman’s code and he said that my code worked for him on one motor, I’m not too sure that is the issue. Still, I will be able to do some datalogging tomorrow, if that will help.

I’m going to replace the cortex tomorrow and see if that might be the problem.

Update: I got it to work. I replaced the cortex and the problem persisted, so I tried a single motor, as jpearman did, and the code worked. I set each side of the flywheel to different buttons and that worked, but only when I ran a side without running the other side. I then placed the FwSetVelocity for each side within a function and placed that function mapped to a button in the user control section. This worked. Not sure why, but my code works now, as long I set speed for both sides in a function preceding the user control code.

Thanks for the advice @jpearman and @Team80_Giraffes

do you really need to assign one motor as a slave?

i used a version of Adam T’s dual flywheel adaption of the Flywheel velocity revisited code, and that basically creates 2 independent processes.

Also, do you need to take absolute values of the encoder values if using quad encoders? They don’t reflect over, when reversing motors right?

I have a few questions about using TBH:

  1. Can you use the Quadrature Encoders? I am currently using IME’s however I was wondering whether there is an advantage of using the Quadrature Encoders over these as this seems to be what a lot of teams do with PID

  2. When coming to tune the TBH I am unsure about what part I need to change. I have been told that I need to change the


	gain = 0.00050;

but at the same time I am unsure what


FwVelocitySetL( 53,0.21);

the 0.21 in the user control section does and whether this needs to be changed when tuning?

Thanks :slight_smile:

How did you place FwSetVelocity for each side in a function? I am having the exact same issue as you are having but I can’t seem to find a solution around it !

}

I am trying this with 6 motors and slaving two motors off one IME on each side of the flywheel and having this exact issue… I have tried using Quadrature Encoders to see if this would make any difference however the same problem still occurs. I have based the code off Adam T however I am stuck on knowing what to add or change in order to make it work with a 6 motor flywheel.

My user control code is :

task usercontrol()
{
	startTask( FwControlTask );



	// Main user control loop
	while(1)
	{	
		if( vexRT Btn8L ] == 1 )
		{
			FwVelocitySetR( 138, 0.52); //////// MAXIMUM? ////////
			FwVelocitySetL( 138, 0.52); /////////////////////////
		}
		if( vexRT Btn8R ] == 1 )
		{
			FwVelocitySetR( 40,0.52);	//////// CLOSE RANGE ////////
			FwVelocitySetL( 40,0.52); /////////////////////////////
		}
		if( vexRT Btn8U ] == 1 )
		{
			FwVelocitySetR( 53,0.52); ////////
			FwVelocitySetL( 53,0.52);
		}
		if( vexRT Btn8D ] == 1 )
		{
			FwVelocitySetR( 0, 0 );
			FwVelocitySetL( 0, 0 );
		}

	}

And the code I have for the velocity of each side is…

void
FwMotorSetR( int valueR )
{
	slaveMotor(port6, port9);
	motor FFR ] = valueR;
}

/*Set the flywheen motors LEFT */
void
FwMotorSetL( int valueL )
{
	slaveMotor(port8, port7);
	motor FFL ] = valueL;
}


/*Get the flywheen motor encoder count RIGHT*/
long
FwMotorEncoderGetR()
{
	return( nMotorEncoder FFR ] );
}


/*Get the flywheen motor encoder count LEFT */
long
FwMotorEncoderGetL()
{
	return( nMotorEncoder FFL ] );
}


/*Set the controller position RIGHT */
void
FwVelocitySetR( int velocityR, float predicted_driveR )
{
	// set target_velocity velocity (motor rpm)
	target_velocityR = velocityR;

	// Set error so zero crossing is correctly detected
	current_errorR = target_velocityR - motor_velocityR;
	last_errorR    = current_errorR;

	// Set predicted open loop drive value
	drive_approxR  = predicted_driveR;
	// Set flag to detect first zero crossing
	first_crossR   = 1;
	// clear tbh variable
	drive_at_zeroR = 0;
}


/*Set the controller position LEFT  */
void
FwVelocitySetL( int velocityL, float predicted_driveL )
{
	// set target_velocity velocity (motor rpm)
	target_velocityL = velocityL;

	// Set error so zero crossing is correctly detected
	current_errorL = target_velocityL - motor_velocityL;
	last_errorL    = current_errorL;

	// Set predicted open loop drive value
	drive_approxL  = predicted_driveL;
	// Set flag to detect first zero crossing
	first_crossL   = 1;
	// clear tbh variable
	drive_at_zeroL = 0;
}

If you have six motors, two motors will need to be slaved to each “master” motor. I only see one call to slaveMotor for each side. Also, put the call to slaveMotor somewhere else, you don’t need to keep calling it, once in pre_auton is enough. When posting code it’s really hard to debug when you only post a few lines, for example, I have no idea what port FFR is. Always post the #pragma config statements if there is other code you want to keep secret.

Here is my code

#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, I2C_2,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Motor,  port1,           intake1,       tmotorVex393_HBridge, openLoop)
#pragma config(Motor,  port2,           frontRight,    tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port3,           backRight,     tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port4,           frontLeft,     tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port5,           backLeft,      tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port6,           FBR,           tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port7,           FFL,           tmotorVex393_MC29, openLoop, reversed, encoderPort, I2C_1)
#pragma config(Motor,  port8,           FBL,           tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port9,           FFR,           tmotorVex393_MC29, openLoop, reversed, encoderPort, I2C_2)
#pragma config(Motor,  port10,          intake2,       tmotorVex393_HBridge, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//


The forum isn’t letting me post all of the code,maybe too many characters?

Also I am using a y cable on each side of the flywheel so only need to slave the motor once I assume?

However I have added all the code to codeshare on this URL… https://codeshare.io/fDOaJ

I also have a TBH code, but I am having issues with setting the rpm of my flywheel. Once I hit the max speed, my flywheel will not slow down to hit the close range speed. Any ideas or help?
if( vexRT Btn8L ] == 1 )
{
FwVelocitySetR( 144, 0.55 );
FwVelocitySetL( 144, 0.55 );
}
if( vexRT Btn8U ] == 1 )
{
FwVelocitySetR( 50, 0.2 );
FwVelocitySetL( 50, 0.2 );
}

@VoltRobotics5194B
Hey very sorry about the late reply. I’m not usually logged in, so I didn’t see this till now. idk if you still need this, but here’s what i did

void setVelocityMid(){
	FwVelocitySetR( 46, 0.51 );
	FwVelocitySetL( 46, 0.51 );
}

then in the user control

if( vexRT Btn8R ] == 1 )
		{
			setVelocityMid();
		}

also gain is what you tune. The FwVelocitySet is for setting the velocity you want the flywheel run at.

probably going to need to see all the code to help with this, are you using the original version of the FwVelocitySet function or has it been modified?

I believe I am using the original version of the FwVelocitySet Function it is heavily based off of Adam T’s original TBH code.

Small clarification, my original code.
https://vexforum.com/t/flywheel-velocity-control-revisited/30480/1
https://vexforum.com/t/flywheel-velocity-control/29892/1

It seems like everyone is using it (which is fine and why I post it anyway :slight_smile: ).

Anyway, point is if there is a bug in the implementation then we can only determine that by seeing the whole program.