Odd flywheel issue

Alright, at our state competition last Saturday we had a weird problem with out flywheel that cost us the chance at getting to Worlds. We are not going to any other competitions so help isn’t urgently needed we just wanted to know what caused the problem for future reference. It started with the night before on Friday, we were trying to tune the autonomous when the launcher suddenly started to twitch as if the motors were fighting each other. We replaced all the electric components on the launcher and made sure the gearing wasn’t catching anywhere and this worked. At state the next day we ran the flywheel and everything ran fine, we went to our first match and the flywheel started twitching again. After this we tore apart the gearbox and ran the motors by themselves, nothing went wrong, we put the gears in the motors and all of them ran fine. So we put the gearbox back on and went to our next match, where the problem happened again. We then discovered that after the flywheel started to run, when it reached its target position two of the motors suddenly reversed. We then replaced all of the electronics on the launcher, even our cortex under the suspicion that there might be a bad port but the problem still persisted. Then we looked at our code, AGAIN (which we hadn’t changed from the previous night btw) and my mentor, Casey Phillips looked at it and couldn’t figure out what was wrong either. This problem still happened even when we took all of our velocity control out of the program. If anyone could help me try and figure this out for future reference it would be much appreciated.

Just curious, what type of velocity control were you using?

It was a very slightly modified version of Jpearman’s TBH code. The only thing we had modified from it (other than motor names and the gain) were some things like priority control from Jpearman’s thread Velocity Calculation Traps (I believe that’s the correct thread)

There’s got to be an issue with the zero-crossing, aka what happens when the current velocity over corrects and exceeds the target velocity. You should post the code so we can take a look.

What type of encoders are they?
If they are quad encoders, what ports are they plugged in?

Is it possible that 2 motors have sign-locking, and 2 don’t? That’s the only thing I can think of on the software side.

We had this problem too. We found (the night before State) that the autonomous would reverse the motors when they the field switch disabled the field, we then added code to turn the motors to 0 and turn off out PID for the launcher. PS: these things always happen before a big competition :slight_smile: .

I checked the zero crossing and everything seemed to be in order. I’m not familiar with sign locking. Maybe if you explained it to me a little more I could check and make sure that two motors don’t have it turned on. As for the competition control the issue happened while we were not plugged into the field and while we were plugged into the field as well.

Have you tried replacing all the motors on the launcher? There could be a short inside a motor causing the “twitching”.

Sign locking is basically where you limit the motor’s output to a positive sign or 0. It prevents damage to the motors, since you don’t want the motors actively fighting the momentum of a flywheel. All flywheel motors with velocity control should have it on. I doubt that it’s your problem, since that would appear just about any time error is negative, even in practice.

Yes we replaced all the electronic components on the launcher.

Did you replace the encoder?

When you assign the motor power to each motor, does it look like this


motor[port1] = motor[port2] = pwr;

or

motor[port1] = pwr;
motor[port2] = pwr;

It shouldn’t matter, as long as the same variable is assigned to all motors. It sounds like a hardware issue with the ports, but I’m not sure.

Our TBH code makes it so that we give a velocity to a motor, followed by the predicted drive value.


fwVelocitySet ( motor name,  velocity,  predicted drive );

We did take off the encoder and it worked fine. All the connections were secure.

We replaced our cortex with a brand new one from one of our sister teams who was there and tried to switch around the ports several times.

Seriously the best answer is to post your code. Your welcome to PM it if you don’t want to post it publicly and I should be able to find the issue.

The issue is certainly programming judging by the very specific symptoms.

Here’s the TBH code.


//Update inteval (in mS) for the flywheel control loop
#define FW_LOOP_SPEED             50
// 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
// Structure to gather all the flywheel ralated data
typedef struct _fw_controller {
	long            counter;                ///< loop counter used for debug

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

	// Encoder
	long            e_current;              ///< current encoder count
	long            e_last;                 ///< current encoder count
	long						encoder_timestamp;      ///< time of last encoder reading; helps prevent issues relating to I2C timing with multiple IEMs
	long						encoder_timestamp_last;

	// velocity measurement
	float           v_current;              ///< current velocity in rpm
	long            v_time;                 ///< Time of last velocity calculation

	// TBH control algorithm variables
	long            target;                 ///< target velocity
	long            current;                ///< current velocity
	long            last;                   ///< last velocity
	float           error;                  ///< error between actual and target velocities
	float           last_error;             ///< error last time update called
	float           gain;                   ///< gain
	float           drive;                  ///< final drive out of TBH (0.0 to 1.0)
	float           drive_at_zero;          ///< drive at last zero crossing
	long            first_cross;            ///< flag indicating first zero crossing
	float           drive_approx;           ///< estimated open loop drive


	// final motor drive
	long            motor_drive;            ///< final motor control value
} fw_controller;

// Make the controller global for easy debugging
static  fw_controller   flywheel;

/*-----------------------------------------------------------------------------*/
/** @brief      Set the flywheen motors                                        */
/** @param[in]  value motor control value                                      */
/*-----------------------------------------------------------------------------*/
void
FwMotorSet( int value )
{
	motor Motor_FW1 ] = value;
	motor Motor_FW2 ] = value;
}

/*-----------------------------------------------------------------------------*/
/** @brief      Get the flywheen motor encoder count                           */
/*-----------------------------------------------------------------------------*/
long
FwMotorEncoderGet()
{
	getEncoderAndTimeStamp(Motor_FW1, encoder_counts, encoder_timestamp)
}

/*-----------------------------------------------------------------------------*/
/** @brief      Set the controller position                                    */
/** @param[in]  fw pointer to flywheel controller structure                    */
/** @param[in]  desired velocity                                               */
/** @param[in]  predicted_drive estimated open loop motor drive                */
/*-----------------------------------------------------------------------------*/
void
FwVelocitySet( fw_controller *fw, int velocity, float predicted_drive )
{

	// set target velocity (motor rpm)
	fw->target        = velocity;

	// Set error so zero crossing is correctly detected
	fw->error         = fw->target - fw->current;
	fw->last_error    = fw->error;

	// Set predicted open loop drive value
	fw->drive_approx  = predicted_drive;
	// Set flag to detect first zero crossing
	fw->first_cross   = 1;
	// clear tbh variable
	fw->drive_at_zero = 0;
}

/*-----------------------------------------------------------------------------*/
/** @brief      Calculate the current flywheel motor velocity                  */
/** @param[in]  fw pointer to flywheel controller structure                    */
/*-----------------------------------------------------------------------------*/
void
FwCalculateSpeed( fw_controller *fw )
{
	//float motor_velocity_t;
	int     delta_ms;
	int     delta_enc;

	// Get current encoder value
	fw->e_current = FwMotorEncoderGet();

	// 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 = fw->encoder_timestamp - fw->encoder_timestamp_last;
	fw->encoder_timestamp_last = fw->encoder_timestamp;


	// Change in encoder count
	delta_enc = (fw->e_current - fw->e_last);

	// save last position
	fw->e_last = fw->e_current;

	// Calculate velocity in rpm
	motor_velocity_t = (1000.0 / delta_ms) * delta_enc * 60.0 / fw->ticks_per_rev;
	fw->current = (fw->current * 0.8) + (motor_velocity_t * 0.2);
}

/*-----------------------------------------------------------------------------*/
/** @brief      Update the velocity tbh controller variables                   */
/** @param[in]  fw pointer to flywheel controller structure                    */
/*-----------------------------------------------------------------------------*/
void
FwControlUpdateVelocityTbh( fw_controller *fw )
{
	// calculate error in velocity
	// target is desired velocity
	// current is measured velocity
	fw->error = fw->target - fw->current;

	// Use Kp as gain
	fw->drive =  fw->drive + (fw->error * fw->gain);

	// Clip - we are only going forwards
	if( fw->drive > 1 )
		fw->drive = 1;
	if( fw->drive < 0 )
		fw->drive = 0;
	// Check for zero crossing
	if( sgn(fw->error) != sgn(fw->last_error) ) {
		// First zero crossing after a new set velocity command
		if( fw->first_cross ) {
			// Set drive to the open loop approximation
			fw->drive = fw->drive_approx;
			fw->first_cross = 0;
		}
		else
			fw->drive = 0.5 * ( fw->drive + fw->drive_at_zero );

		// Save this drive value in the "tbh" variable
		fw->drive_at_zero = fw->drive;
	}

	// Save last error
	fw->last_error = fw->error;
}

/*-----------------------------------------------------------------------------*/
/** @brief     Task to control the velocity of the flywheel                    */
/*-----------------------------------------------------------------------------*/
task
FwControlTask()
{
	fw_controller *fw = &flywheel;

	// Set the gain
	fw->gain = 0.08
	// We are using Torque geared motors
	// Set the encoder ticks per revolution
	fw->ticks_per_rev = MOTOR_TPR_393T;

	while(1)
	{
		// debug counter
		fw->counter++;

		// Calculate velocity
		FwCalculateSpeed( fw );

		// Set current speed for the tbh calculation code
		fw->current = fw->v_current;

		// Do the velocity TBH calculations
		FwControlUpdateVelocityTbh( fw ) ;

		// Scale drive into the range the motors need
		fw->motor_drive  = (fw->drive * FW_MAX_POWER) + 0.5;

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

		// and finally set the motor control value
		FwMotorSet( fw->motor_drive );

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

Here’s the driver control code that pertains to the TBH.

task usercontrol()
{
       nMotorEncoder[Motor_FW1] = 0;

	//task priority ranges from 0/lowest priority  to  255/highest priority//
	//set FwControlTask to a priority setting of 10 because normal priority is 7//
	startTask(FwControlTask, 10);

	//Motor_FW2 will follow Motor_FW1//
	slaveMotor(Motor_FW2, Motor_FW1);

	while(true)
	{
		if( vexRT Btn8UXmtr2 ] == 1 ) //FULL FIELD SHOT//
		{
			FwVelocitySet( &flywheel, 55, 0.65 );
		}
		if( vexRT Btn8RXmtr2 ] == 1 ) //MEDIUM SHOT//
		{
			FwVelocitySet( &flywheel, 39, 0.27 );
		}
		if( vexRT Btn8DXmtr2 ] == 1 ) //SHORT/BAR SHOT//
		{
			FwVelocitySet( &flywheel, 35, 0.24 );
		}
		if( vexRT Btn8LXmtr2 ] == 1 ) //STOP//
		{
			FwVelocitySet( &flywheel, 0, 0.0 );
		}
	}
wait1Msec(20);
}

However, I have heard that if you use a certain file in RobotC for an extended length of time that the file might start to get some problems in it because of all the information that’s been stored in that file. My mentor suggested this to me after state because he had had a similar problem to what we were having several years earlier before I joined his team. One of my teammates has the robot though but when we get it back I may try to move my code into a new file.