urget help needed with c++ code

[TikiTech]

We are having issues at our only competition.
After 4 matches of normal operation in autonomous and teleop we are no longer able to function. Now after autonomous the robot will just sit. We have had the Ftc check and we have proper field communication. We have had many teams, mentors and others look at our code nothing is helping. We have removed all the autonomous code and the robot will not do anything. We do not have any issues on the practice field or in the pits. It only happens on the field. We have replaced the cRio and the issue still exists. I am not sure how to post the code. Should I paste the code in as text?
I believe it is a few 100 lines. We are running c++ . This is extremely critical. We do not have a programming mentor to help. I feel quite certain that it is not hardware since it works fine on the practice field.
Any ideas would be appreciated. We only have 3 more matches and the students spirits are low and I want them to do well. If we cannot get moving eliminations will not happen for us.

Thanks for any help or ideas. I doubt I will sleep trying to work this out by the morning..

Aloha

[BornaE]

Posting your code will probably help.

[TikiTech]

#include "WPILib.h"
#include "Math.h"
//#include "VisionProcess.cpp"

int shot = 5;
Victor *intakeL;
Victor *intakeR;
Gyro *gyro;
Encoder *encoderL;
Encoder *encoderR;
ADXL345_I2C *accel;
//DriverStationLCD *driverStation;
DigitalInput *pressure;
Relay *presser;
Servo *cameraTilt;
Servo *cameraTurn;
AnalogChannel *armPotent;
class RobotDemo : public SimpleRobot
{
Talon yLeft;
Talon sLeft;
Talon yRight;
Talon sRight;
Joystick stick; // only joystick
Joystick assistick;
Jaguar armL;
Jaguar armR;
Solenoid shooterL;
Solenoid shooterR;
Relay shifter1;
Relay shifter2;
static const float Kp = 0.05;
int ang;

public:
RobotDemo():
yLeft(1),
sLeft(2),
yRight(3),
sRight(4),
stick(1), // as they are declared above.really are they ? not sure maybe.
assistick(2),
armL(5),
armR(6),
shooterL(1),
shooterR(2),
shifter1(2),
shifter2(3)
{
GetWatchdog().SetEnabled(false);
yLeft.SetExpiration(0.1);
sLeft.SetExpiration(0.1);
yRight.SetExpiration(0.1);
sRight.SetExpiration(0.1);
armL.SetExpiration(0.1);
armR.SetExpiration(0.1);
intakeL = new Victor(7);
intakeR = new Victor(10);
gyro = new Gyro(1);
encoderL = new Encoder(6,7);
encoderR = new Encoder(8,9);
pressure = new DigitalInput(5);
presser = new Relay(1);
cameraTilt = new Servo(9);
cameraTurn = new Servo(8);
armPotent = new AnalogChannel(2);
//driverStation = DriverStationLCD->GetInstance();
//ADXL345_I2C- = new Accelerometer(1);
}

/**
* Drive left & right motors for 2 seconds then stop
*/

void RobotInit()
{
//SmartDashboard::init();
encoderL->Start();
encoderR->Start();
}
void Autonomous()//range: 6ft-
{
//cout<<"in auton \n";
encoderL->Reset();
encoderR->Reset();
Wait(0.5);
int t = 0;
while((fabs(encoderL->GetRaw())<4500||fabs(encoderR->GetRaw())<4500) && t<200)
{
yLeft.Set(0.7);
sLeft.Set(-0.7);
yRight.Set(-0.7);
sRight.Set(0.7);
Wait(0.005);
t++;
}
t = 0;
while((fabs(encoderL->GetRaw())<4700||fabs(encoderR->GetRaw())<4700) && t<200)
{
yLeft.Set(0.4);
sLeft.Set(-0.4);
yRight.Set(-0.4);
sRight.Set(0.4);
Wait(0.005);
}
yLeft.Set(0.0);
sLeft.Set(0.0);
yRight.Set(0.0);
sRight.Set(0.0);
armL.Set(0.5);
armR.Set(0.5);
Wait(0.8);
armL.Set(0.0);
armR.Set(0.0);
Wait(0.2);
shooterL.Set(true);
shooterR.Set(true);
Wait(0.3);
shooterL.Set(false);
shooterR.Set(false);
Wait(0.2);
t = 0;
while((fabs(encoderL->GetRaw())>0||fabs(encoderR->GetRaw())>0) && t<150)
{
yLeft.Set(-0.7);
sLeft.Set(0.7);
yRight.Set(0.7);
sRight.Set(-0.7);
intakeL->Set(0.6);
intakeR->Set(-0.6);
Wait(0.005);
t++;
}
t = 0;
while((fabs(encoderL->GetRaw())<250||fabs(encoderR->GetRaw())<250) && t<150)
{
yLeft.Set(-0.5);
sLeft.Set(0.5);
yRight.Set(0.5);
sRight.Set(-0.5);
Wait(0.005);
t++;
}
yLeft.Set(0.0);
sLeft.Set(0.0);
yRight.Set(0.0);
sRight.Set(0.0);
Wait(1.5);
intakeL->Set(0.0);
intakeR->Set(0.0);
Wait(0.5);
t = 0;

while((fabs(encoderL->GetRaw())<2000||fabs(encoderR->GetRaw())<2000) && t<150)
{
yLeft.Set(0.7);
sLeft.Set(-0.7);
yRight.Set(-0.7);
sRight.Set(0.7);
Wait(0.005);
t++;
}

t = 0;
while((fabs(encoderL->GetRaw())<2100||fabs(encoderR->GetRaw())<2100) && t<150)
{
yLeft.Set(0.4);
sLeft.Set(-0.4);
yRight.Set(-0.4);
sRight.Set(0.4);
Wait(0.005);
t++;
}
yLeft.Set(0.0);
sLeft.Set(0.0);
yRight.Set(0.0);
sRight.Set(0.0);
Wait(0.3);
shooterL.Set(true);
shooterR.Set(true);
Wait(0.3);
shooterL.Set(false);
shooterR.Set(false);
//cout<< "out of auton \n";
}

/**
* Runs the motors with arcade steering.
*/
//////////DRIVESTICK CONFIG////////// ********** = TO DO //////////ASSISTICK CONFIG//////////
// Btn 1 A Button //null // Btn 1 1 Button //
// Btn 2 B Button //SHOOTER Safety Button (Both buttons // Btn 2 2 Button //
// Btn 3 X Button //null must be pressed // Btn 3 3 Button //
// Btn 4 Y Button //SHOOTER Fire Button to shoot) // Btn 4 4 Button //
// Btn 5 L Bumper //TOOL OUT/PASS ********** //
// Btn 6 R Bumper //TOOL INTAKE **********
// Btn 7 Back Btn //null
// Btn 8 Start Btn //null
// Btn 9 L Joy Click//null
// Btn 10 R Joy Click//null
// Axis 1 L X Axis //null
// Axis 2 L Y Axis //DRIVE +Forward/Reverse-
// Axis 3 + L Trigger //ARM DOWN **********
// Axis 3 - R Trigger //ARM UP **********
// Axis 4 R X Axis //STEER +Right/Left-
// Axis 5 R Y Axis //null


void OperatorControl()
{
//cout<<"In Operator Contrl";
//int logcounter = 200;
//int *CamTurn;
//int *CamTilt;
//bool *Pv;
int h=1;
//float *P;
int *angle;
while (IsOperatorControl())
{
/*if(logcounter >0)
{
cout<<"running userctrl \n";
logcounter--;
}*/
*angle = int(gyro->GetAngle());

if(((stick.GetRawAxis(2))>0.15 || stick.GetRawAxis(2)<(-0.15)) || ((stick.GetRawAxis(4)>0.10) || (stick.GetRawAxis(4)<-0.10)))
{
/*if(logcounter >0)
{
cout<<"running drive \n";
logcounter--;
}*/
yLeft.Set(-stick.GetRawAxis(2)+(stick.GetRawAxis(4)));
yRight.Set(stick.GetRawAxis(2)+(stick.GetRawAxis(4 )));
sLeft.Set(stick.GetRawAxis(2)-(stick.GetRawAxis(4)));
sRight.Set(-stick.GetRawAxis(2)-(stick.GetRawAxis(4)));
}
else if((fabs(stick.GetRawAxis(2))>0.15) && ((fabs(stick.GetRawAxis(4))<0.10)))
{
/*if(logcounter >0)
{
cout<<"running straightdrive \n";
logcounter--;
}*/
yLeft.Set(-stick.GetRawAxis(2)-(*angle*Kp));
yRight.Set(stick.GetRawAxis(2)+(*angle*Kp));
sLeft.Set(stick.GetRawAxis(2)+(*angle*Kp));
sRight.Set(-stick.GetRawAxis(2)-(*angle*Kp));
}
else
{
yLeft.Set(0);
yRight.Set(0);
sLeft.Set(0);
sRight.Set(0);
}
if(assistick.GetRawButton(5)) //ARM UP
{
//cout<<"arm up \n";
armR.Set(-0.5);
armL.Set(-0.5);
}
else if(assistick.GetRawButton(6)) //ARM DOWN
{
//cout<<"arm down \n";
armR.Set(0.5);
armL.Set(0.5);
}
else if(fabs(stick.GetRawAxis(3)) > 0.3 && !assistick.GetRawButton(5) && !assistick.GetRawButton(6)){
armR.Set(stick.GetRawAxis(3));
armL.Set(stick.GetRawAxis(3));
}
else{
armR.Set(0.0);
armL.Set(0.0);
}

if(!pressure->Get())
{
presser->Set(presser->kOn);
}
else if(stick.GetRawButton(1)||pressure->Get())
{
presser->Set(presser->kOff);
}

if(stick.GetRawButton(10))
{
shifter1.Set(shifter1.kOn);
shifter2.Set(shifter2.kOff);
Wait(0.2);
}
else if(stick.GetRawButton(9))
{
shifter1.Set(shifter1.kOff);
shifter2.Set(shifter2.kOn);
Wait(0.2);
}
else
{
shifter1.Set(shifter1.kOff);
shifter2.Set(shifter2.kOff);
}

if(stick.GetRawButton(4) && stick.GetRawButton(3) && !assistick.GetRawButton(2))// && stick.GetAxis(3)>0)
{
shooterL.Set(true);
shooterR.Set(true);
Wait(0.25);
}
else if(stick.GetRawButton(4) && stick.GetRawButton(3) && assistick.GetRawButton(2))// && stick.GetAxis(3)>0)
{
shooterL.Set(true);
shooterR.Set(true);
Wait(0.10);
}

else
{
shooterL.Set(false);
shooterR.Set(false);
}

if(stick.GetRawButton(6) || assistick.GetRawButton(7)) //TOOL INTAKE
{
//cout<<"intake on\n";
intakeL->Set(0.8);
intakeR->Set(-0.8);
}

else if((stick.GetRawButton(5) || assistick.GetRawButton(8)) && !assistick.GetRawButton(7)) //TOOL OUT/PASS
{
//cout<< "pass on \n";
intakeL->Set(-1.0);
intakeR->Set(1.0);
}
else
{
intakeL->Set(0);
intakeR->Set(0);
}
if (h%48 == 0)
{
//*P = armPotent->GetAverageValue();
//*Pv = presser->Get();
//*CamTilt = int(cameraTilt->Get());
//*CamTurn = int(cameraTurn->Get());
//*Pot = int(armPotent->GetValue()); *G = int(gyro->GetAngle());
//cout<<("LeftEncoder: "&& *L &&" RightEncoder: "&& *R &&" Gyro:"&& *G &&" Pressure Switch: "&& *Ps &&" Compressor: "&& *Pv);
///*cout<<("ArmPotentiometer: "); cout<<(*Pot);cout<<("\ncamTurn: ");cout<<(*CamTurn);cout<<("\nGyroscope Angle: ");cout<<(*G);cout<<"\n";
//cout<<"ArmPotent: "<<*P<<"\n";
}
h++;
Wait(0.03); // wait for a motor update time
}
//cout<< "out of usercntrl \n";
}

void Test() {

}
};

START_ROBOT_CLASS(RobotDemo);

[Kevin Sevcik]

Something in your auton code looks wrong. I'm bolding the line your should add:

while((fabs(encoderL->GetRaw())<4500||fabs(encoderR->GetRaw())<4500) && t<200)
{
yLeft.Set(0.7);
sLeft.Set(-0.7);
yRight.Set(-0.7);
sRight.Set(0.7);
Wait(0.005);
t++;
}
t = 0;
while((fabs(encoderL->GetRaw())<4700||fabs(encoderR->GetRaw())<4700) && t<200)
{
yLeft.Set(0.4);
sLeft.Set(-0.4);
yRight.Set(-0.4);
sRight.Set(0.4);
Wait(0.005);
t++;
}

Also, in OperatorControl, you're using "angle" as a pointer, which seems unnecessary and dangerous. Change " *angle " everywhere you see it to just " angle ".

I don't see anything obviously wrong with your code. I'd recommend taking out all those Wait calls in OperatorControl, because you'll lose control of the robot for the entire duration of that Wait. So if you're turning and you trigger that Wait(0.25), you'll keep turning for 0.25 sec, no matter what you do on the controls.

Generally, I'd recommend trying the robot in Practice mode, so it runs through Auton and into Teleop in the same sequence it would on the field. I'd also recommend running NetConsole while you're on the field, so you can get any debugging statements or exceptions that will tell you what's killed your robot.

[TikiTech]

I will forward this to my student programmer. Keeping my fingers crossed.
The strange thing is that in practice mode in the pits and on the practice field we are not having any issues. It is only on the FMS that it doesn't work. the only thing that it will do is run the auto correctly and then no teleop. If the auto code is removed then we do not do anything. Looks nice sitting there though.. I am hoping it is related to the *wait command you mentioned. We will find out in 3 hours when we go back to our last day of competition.
Thank you very much for your time and suggestions.

Aloha

[TikiTech]

Thank you for your help!!

After showing the ideas here it was decided that it was better to start from scratch. A much simpler code was written from scratch with no more waits nor improper use of *

Downloaded the code and from then on, all was well..

Except some missed possession calls in the semi finals and a replay I am still not sure why....

Aloha!