Our auto stacking code is not working. Our Scissor lift just goes up to the max height then stops, then it runs the chainbar code.
#pragma config(Sensor, in1, POT, sensorPotentiometer)
#pragma config(Sensor, dgtl7, rightEncoder, sensorQuadEncoder)
#pragma config(Sensor, dgtl9, leftEncoder, sensorQuadEncoder)
#pragma config(Sensor, dgtl11, Sonar1, sensorSONAR_inch)
#pragma config(Motor, port1, rightMotorB, tmotorVex393HighSpeed_HBridge, openLoop, reversed, driveRight)
#pragma config(Motor, port2, rightMotorF, tmotorVex393HighSpeed_MC29, openLoop, reversed, driveRight)
#pragma config(Motor, port3, ConeLift, tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor, port4, ScissorRight, tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor, port5, ScissorLeft, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port6, ConeLift2, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port7, ArmMotors, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port8, ConeGrabber, tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor, port9, leftMotorF, tmotorVex393HighSpeed_MC29, openLoop, driveLeft)
#pragma config(Motor, port10, leftMotorB, tmotorVex393HighSpeed_HBridge, openLoop, driveLeft)
//!!Code automatically generated by ‘ROBOTC’ configuration wizard !!//
void ChainBar( int ArmPos)
{
wait1Msec(1000);
while(SensorValue[POT] > 700)
{
motor[ConeLift] = -127;
motor[ConeLift2] = -127;
}
}
void ScissorUp( int Speed1, int Speed2)
{
if(abs(SensorValue[leftEncoder]) > abs(SensorValue[rightEncoder]))
{
motor[ScissorRight] = Speed2;
motor[ScissorLeft] = Speed1;
}
if(abs(SensorValue[leftEncoder]) < abs(SensorValue[rightEncoder]))
{
motor[ScissorRight] = Speed1;
motor[ScissorLeft] = Speed2;
}
if(abs(SensorValue[leftEncoder]) == abs(SensorValue[rightEncoder]))
{
motor[ScissorRight] = Speed1;
motor[ScissorLeft] = Speed1;
}
}
/-----------------------------------------------------------------------------/
/* ScissorLift Adjustment /
/-----------------------------------------------------------------------------*/
void ScissorLift( int Dist )
{
while(SensorValue[Sonar1] < Dist && SensorValue[rightEncoder] < 90 && SensorValue[Sonar1] != -1)
{
ScissorUp( 90, 90 );
}
}
/-----------------------------------------------------------------------------/
/* ScissorLift Down /
/-----------------------------------------------------------------------------*/
void ScissorDown (int Speed1, int Speed2)
{
if(abs(SensorValue[leftEncoder]) > abs(SensorValue[rightEncoder]))
{
motor[ScissorRight] = -Speed2;
motor[ScissorLeft] = -Speed1;
}
if(abs(SensorValue[leftEncoder]) < abs(SensorValue[rightEncoder]))
{
motor[ScissorRight] = -Speed1;
motor[ScissorLeft] = -Speed2;
}
if(abs(SensorValue[leftEncoder]) == abs(SensorValue[rightEncoder]))
{
motor[ScissorRight] = -Speed1;
motor[ScissorLeft] = -Speed1;
}
}
task usercontrol()
{
resetSensor(rightEncoder);
resetSensor(leftEncoder);
while (true)
{
if(vexRT[Btn5UXmtr2] == 1)
{
ScissorLift ( 18 );
ChainBar( 300 );
}
}
}