So my brother and I have spent a few hours trying to troubleshoot our code for a new robot for VEX IQ Pitching In. The RobotC code is below. The problem we are experiencing is that, basically, all the motors don’t act as they should. I know this is ambiguous, but the problem varies from test to test. One time it will be the intake won’t raise, and another time it will hold the motor when it is stopped, but when it is supposedly going up at “100% power” it will not be trying to raise it at all (however it has no problems going down) and it is as if it is set to coast because you can move it easily when it says it’s going at 100% power. We have tried re-downloading the firmware, checking for updates, re-installing the VEXos Utility, replacing the brain, wire, motor, and changing the port. We have tried restarting the brain and RobotC, and doing many other things to try and fix it, but nothing works. We have gone through out code several times, proofing it. Even when we tried just a simple program with just one command, it still had this problem. We have eliminated all possibility of it being hardware, and we are wondering if it could be RobotC that is the problem. We have noticed a number of odd occurrences with RobotC, such as that when we restart it, the code works much better. I understand that RobotC is probably not the problem, but we have eliminated virtually everything else, so I don’t know what else could be wrong. We are trying it in VEX Code IQ Blocks to see if that fixes it. I was wondering if anyone had any ideas of what could be wrong and/or how to fix it?
#pragma config(Sensor, port4, CatDown, sensorVexIQ_Touch)
#pragma config(Motor, motor1, CatR, tmotorVexIQ, PIDControl, encoder)
#pragma config(Motor, motor2, DriveL, tmotorVexIQ, PIDControl, driveLeft, encoder)
#pragma config(Motor, motor6, CatL, tmotorVexIQ, PIDControl, reversed, encoder)
#pragma config(Motor, motor8, DriveR, tmotorVexIQ, PIDControl, reversed, driveRight, encoder)
#pragma config(Motor, motor10, intake2, tmotorVexIQ, PIDControl, reversed, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
//geared 1:5 on arm
//NEW CHANGE: Added presets
//const int LowGear = .4;
//const int MiddleGear = .7;
//const int HighGear = 1.0;
const int ThrottleDeadband = 10; //the deadband on the throttle joystick
const int TurnSpeedDeadband = 20; //the deadband on hte turn joystick
const int ArmSpeedDeadband = 20; //the deadband on the arm joystick
const int PresetCancelValue = 95; //the value to which the arm joystick must be moved in order to cancel the currect preset mvoements
const int ZeroingMaxCurrent = 350;
float Throttle = 0;
float TurnSpeed = 0;
float ArmSpeed = 0;
int PresetState = 1;
int reloadState = 0;
int shootState = 0;
const int presetPos[4] = {-295,-10,-200,-300}; //groiund/intake2, high/dump
const int shootPos = 1050; //the number of degrees beyond the sensor being triggered to spin in order to prepare for shooting
const int loadTime = 100; //the amount of time to keep the intake2 at the top to allow balls to flow into the shooter before re-lowering it
// int x; //used in for loops
void ZeroArm()
{
/*
resetMotorEncoder(CatL);
resetMotorEncoder(CatR);
// setMotorBrakeMode(CatL,motorHold);
// setMotorBrakeMode(CatR,motorHold);
// setMotorBrakeMode(intake2,motorHold);
*/
delay (20);
setMotor(CatL,100);
setMotor(CatR,100);
while(!getBumperValue(CatDown)){}
setMotorTarget(CatL,getMotorEncoder(CatL)+shootPos,100);
setMotorTarget(CatR,getMotorEncoder(CatR)+shootPos,100);
while(abs(getMotorTarget(CatL) - getMotorEncoder(CatL)) > 25){}
stopAllMotors();
// playNote(noteA,2,100);
// setMotorCurrentLimit(intake2,1000);
setMotor(intake2,30);
delay(2000);
// while(getMotorCurrent(intake2) < ZeroingMaxCurrent){}
stopAllMotors();
// delay(100);
resetMotorEncoder(intake2);
// setMotorBrakeMode(intake2,motorHold);
delay(50);
}
void diagnostics()
{
while(!getJoystickValue(BtnFDown))
{
setMotor(DriveL,getJoystickValue(ChA)+getJoystickValue(ChC));
setMotor(DriveR,getJoystickValue(ChA)-getJoystickValue(ChC));
setMotor(intake2,getJoystickValue(ChD));
armControl(CatL,BtnLUp,BtnLDown,100);
armControl(CatR,BtnLUp,BtnLDown,100);
}
}
task main()
{
//ZeroArm();
if(getJoystickValue(BtnEUp) && getJoystickValue(BtnEDown))
{
diagnostics();
}
ZeroArm();
//main loop:
while(true)
{
displayMotorValues(line1, intake2);
//preset state trigger
if(getJoystickValue(BtnLUp))
{
shootState = 1;
PresetState = 1;
}
if(getJoystickValue(BtnLDown) && reloadState == 0)
{
reloadState = 1;
}
if(getJoystickValue(BtnRUp) && reloadState == 0)
{
shootState = 1;
reloadState = 1;
}
if(getJoystickValue(BtnRDown))
{
PresetState = 1;
}
//read joysticks:
Throttle = getJoystickValue(ChA);
TurnSpeed = getJoystickValue(ChC);
ArmSpeed = getJoystickValue(ChD);
//Deadbands
if(abs(Throttle) < ThrottleDeadband)
{
Throttle = 0;
}
if(abs(TurnSpeed) < TurnSpeedDeadband)
{
TurnSpeed = 0;
}
if(abs(ArmSpeed) < ArmSpeedDeadband)
{
ArmSpeed = 0;
}
//Re-scale
if(Throttle > 0)
{
Throttle = (100/(100-ThrottleDeadband))*(Throttle-100)+100;
}
if(Throttle < 0)
{
Throttle = (100/(100-ThrottleDeadband))*(Throttle+100)-100;
}
if(ArmSpeed < 0)
{
ArmSpeed = (100/(100-ArmSpeedDeadband))*(ArmSpeed+100)-100;
}
if(ArmSpeed > 0)
{
ArmSpeed = (100/(100-ArmSpeedDeadband))*(ArmSpeed-100)+100;
}
if(TurnSpeed < 0)
{
TurnSpeed = (100/(100-TurnSpeedDeadband))*(TurnSpeed+100)-100;
}
if(TurnSpeed > 0)
{
TurnSpeed = (100/(100-TurnSpeedDeadband))*(TurnSpeed-100)+100;
}
//reloadSeq
switch(reloadState)
{
case 0:
break;
case 1:
if(shootState == 0)
{
reloadState = 2;
PresetState = 2;
}
break;
case 2:
if(PresetState == 0)
{
reloadState = 3;
resetTimer(T1);
}
else
{
PresetState = 2;
}
break;
case 3:
stopMotor(CatL);
stopMotor(CatR);
if(getTimer(T1,milliseconds) > loadTime)
{
reloadState = 4;
PresetState = 1;
}
break;
case 4:
if(PresetState == 0)
{
reloadState = 0;
playNote(noteA,2,100);
}
else
{
PresetState = 1;
}
break;
}
//Add curve
ArmSpeed = ArmSpeed*abs(ArmSpeed)*0.01;
Throttle = Throttle*abs(Throttle)*0.01;
TurnSpeed = TurnSpeed*abs(TurnSpeed)*0.01;
setMotor(DriveL,Throttle+TurnSpeed);
setMotor(DriveR,Throttle-TurnSpeed);
// setMotor(intake2,ArmSpeed);
// setMotorBrakeMode(intake2,motorHold);
if(abs(getJoystickValue(ChD)) > PresetCancelValue)
{
PresetState = 0;
}
if(PresetState == 0)
{
setMotor(intake2,ArmSpeed);
}
else
{
/*if(getMotorEncoder(intake2) > presetPos[PresetState-1])
{
setMotor(intake2,-100);
}
else
{
setMotor(intake2,100);
}*/
setMotorTarget(intake2,presetPos[PresetState-1],100);
if(abs(getMotorEncoder(intake2) - presetPos[PresetState-1]) < 10)
{
PresetState = 0;
reloadState = 0;
}
}
//Catapult code
switch(shootState)
{
case 0:
break;
case 1:
setMotor(CatL,100);
setMotor(CatR,100);
if(getBumperValue(CatDown))
{
resetMotorEncoder(CatL);
resetMotorEncoder(CatR);
shootState = 2;
}
break;
case 2:
setMotorTarget(CatL,shootPos,100);
setMotorTarget(CatR,shootPos,100);
if(abs(getMotorEncoder(CatL) - shootPos) < 20)
{
shootState = 0;
}
break;
}
}
}
Thanks!