void PIDArmControl()
{
if((vexRT[Btn6D] - vexRT[Btn6U])!= 0)
{
setArmSpeed((vexRT[Btn5D] - vexRT[Btn5U]) * maxArm);
disable(PID);
}
else
{
setArmSpeed(calcPID(PID));
}
if(vexRT[Btn7U] == 1)
{
setSetpoint(PID, highScore);
enable(PID);
}
else if (vexRT[Btn7D] == 1)
{
setSetpoint(PID, lowScore);
enable(PID);
}
else if (vexRT[Btn7L] == 1)
{
setSetpoint(PID, midScore);
enable(PID);
}
else if (vexRT[Btn7R] == 1)
{
disable(PID);
}
}
task autonomous() {
// Autonomous
setIntakeSpeed(127); //drop buckie ball
setArmSpeed(100);
wait1Msec(1000);
setArmSpeed(5);
driveArcade(70,0); //drive forward
wait1Msec(1100);
driveArcade(0,0);
setIntakeSpeed(0);
setArmSpeed(20);
wait1Msec(1000);
driveArcade(-70,0); //drive back
wait1Msec(1200);
driveArcade(0,0);
wait1Msec(5000);
driveArcade(70,0); //drive forward
wait1Msec(1300);
driveArcade(0,0);
wait1Msec(1000);
driveArcade(-70,0); //drive back
wait1Msec(1300);
driveArcade(0,0);
}
task usercontrol() {
// User control initialization
while (true) { // User control loop
int driveX = scaleInput(vexRT[Ch4]);
int driveY = scaleInput(vexRT[Ch3]);
int armSpeed = vexRT[Ch2];
int intakeSpeed = 127*((vexRT[Btn5U]|vexRT[Btn5D])-(vexRT[Btn6U]|vexRT[Btn6D]));
/* string potenString, gyroString;
s sprintf(potenString, "%3f%c", SensorValue[poten]);
sprintf(gyroString, "%3f%c", SensorValue[gyro]);
displayString(potenString, gyroString);*/
if (SensorValue[poten] > ARMAX) armSpeed = 0; // Limit the Poten from dying.
driveX = turnPID.enabled ? calculate(turnPID, SensorValue[gyro]) : driveX;
armSpeed = armPID.enabled ? calculate(armPID, SensorValue[poten]) : armSpeed;
if (abs(driveY) < 5) driveY = 0; // Deadband
if (abs(driveX) < 5) driveX = 0;
driveArcade(driveY, driveX);
setArmSpeed(armSpeed);
setIntakeSpeed(intakeSpeed);
}
}
/*
int lock(int final_pos) //locks arm to final_pos potentiometer point
{
int current_pos = SensorValue(PotArm); //reads potentiometer
int direction; //up or down?
int arm_diff = abs(current_pos - final_pos);//difference between current and desired points
if(arm_diff > 100) { //potentiometer is very precise, so
if (current_pos > final_pos) {//arm too high
direction = 1; //will move down
}
else if(current_pos < final_pos) { //arm too low
direction = -1; //will move up
}
setArmSpeed(FULL * direction);
return 0;
}
else { //in hindsight, I don't think this is necessary
setArmSpeed(0);
return 1;
}
}
*/
int lock(int final_pos) //locks arm to final_pos potentiometer point
{
int current_pos = SensorValue(PotArm); //reads potentiometer
int direction; //up or down?
int arm_diff = abs(current_pos - final_pos);//difference between current and desired points
if(arm_diff > 200) { //potentiometer is very precise, so
if (current_pos > final_pos) {//arm too high
direction = 1; //will move down
}
else if(current_pos < final_pos) { //arm too low
direction = -1; //will move up
}
setArmSpeed(FULL * direction);
return 0;
}
else { //in hindsight, I don't think this is necessary
setArmSpeed(0);
return 1;
}
}
task autonomous()
{
pre_auton();
while(true)
{
if(vexRT[AButton] == 1)
{
setArmSpeed(calcPID(PID));
encoderDriveStraight(44.0/12.0);
setCollectorSpeed(MAX_TREAD);
wait10Msec(400);
setCollectorSpeed(0);
setSetPoint(PID, armGround);
enable(PID);
encoderDriveStraight(-44.0/12.0);
}
}
}
void lock_msec(int speed, int duration){
setArmSpeed(speed);
wait1Msec(duration);
setArmSpeed(0);
}