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 
.
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.