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()
