Engineering-VEX-Robotic-Arm

Code for the robot arm we made in Mr. Buono's Principles of Engineering class.

#pragma config(Sensor, in1, pinchPoten, sensorPotentiometer) #pragma config(Sensor, in2, pitchUp, sensorReflection) #pragma config(Sensor, in3, pitchDown, sensorReflection) #pragma config(Sensor, in4, rotateRight, sensorReflection) #pragma config(Sensor, in5, rotateLeft, sensorReflection) #pragma config(Sensor, in6, extendPoten, sensorPotentiometer) #pragma config(Sensor, dgtl1, wristLimit, sensorTouch) #pragma config(Sensor, dgtl2, xtnLimit, sensorTouch) #pragma config(Sensor, dgtl3, wristUpBump, sensorTouch) #pragma config(Sensor, dgtl4, wristDownBump, sensorTouch) #pragma config(Motor, port2, pitchMotor, tmotorVex393_MC29, openLoop) #pragma config(Motor, port3, extendMotor, tmotorVex393_MC29, openLoop) #pragma config(Motor, port4, rotationMotor, tmotorVex393_MC29, openLoop) #pragma config(Motor, port8, wristMotor, tmotorVex393_MC29, openLoop) #pragma config(Motor, port9, pinchMotor, tmotorVex393_MC29, openLoop, reversed) //!!Code automatically generated by 'ROBOTC' configuration wizard !!//

task main() { //Start Task Main bool isExtendMotorOn; bool isExtendLimit; isExtendMotorOn = false;

bool isPinchMotorOn;
isPinchMotorOn = false;

bool isWristMotorOn;
bool isWristLimit;
isWristMotorOn = false;

bool isRotationMotorOn;
isRotationMotorOn = false;

bool isPitchMotorOn;
isPitchMotorOn = false;


while(1 == 1){ // Start While Loop
	// Code for extension arm

	if((SensorValue(xtnLimit)) == 0){
		isExtendLimit = false;
		} else {
		isExtendLimit = true;
	}

	if ((isExtendMotorOn) == false){
		if((SensorValue(extendPoten) < 1000)&& (isExtendLimit == false)){
			startMotor(extendMotor, 44);
			waitInMilliseconds(1000);
			startMotor(extendMotor, 127);
			isExtendMotorOn = true;
		}
		if ((SensorValue(extendPoten) > 3000)){
			startMotor(extendMotor, -44);
			waitInMilliseconds(1000);
			startMotor(extendMotor, -127);
			isExtendMotorOn = true;
			isExtendLimit = false;
		}
	}
	if((isExtendMotorOn) == true){
		if ((SensorValue(extendPoten) > 1000) && (SensorValue(extendPoten) < 3000 )){
			stopMotor(extendMotor);
			isExtendMotorOn = false;
		}
		if ((isExtendLimit) == true){
			stopMotor(extendMotor);
			isExtendMotorOn = false;
			isExtendLimit = false;
		}
	}

	// End extension arm code

	//Code for pinch motor

	if ((isPinchMotorOn) == false){
		if((SensorValue(pinchPoten) > 3000)){
			startMotor(pinchMotor, 30);
			isPinchMotorOn = true;
		}
		if ((SensorValue(pinchPoten) <1000)){
			startMotor(pinchMotor, -30);
			isPinchMotorOn = true;
		}
	}
	if((isPinchMotorOn) == true){
		if ((SensorValue(pinchPoten) <3000) && (SensorValue(pinchPoten) >1000)){
			stopMotor(pinchMotor);
			isPinchMotorOn = false;
		}
	}

	// End pinch motor



	// Code for wrist motor


	if((SensorValue(wristLimit)) == 0){
		isWristLimit = false;
		} else {
		isWristLimit = true;
	}

	if ((isWristMotorOn) == false){
		if((SensorValue(wristDownBump) == 1) && (SensorValue(wristUpBump) == 0) && (isWristLimit == false)){
			startMotor(wristMotor, 55);
			waitInMilliseconds(250);
			startMotor(wristMotor, 70);
			isWristMotorOn = true;
		}
		if ((SensorValue(wristUpBump) == 1) && (SensorValue(wristDownBump) == 0)){
			startMotor(wristMotor, -55);
			waitInMilliseconds(250);
			startMotor(wristMotor, -70);
			isWristMotorOn = true;
			isWristLimit = false;
		}
	}
	if((isWristMotorOn) == true){
		if ((SensorValue(wristDownBump) == 0) && (SensorValue(wristUpBump) == 0)){
			stopMotor(wristMotor);
			isWristMotorOn = false;
		}
		if ((isWristLimit) == true){
			stopMotor(wristMotor);
			isWristMotorOn = false;
			isWristLimit = false;
		}
	}


	// End wrist motor


	// Code for rotation motor

	if ((isRotationMotorOn) == false){
		if((SensorValue(rotateRight) > 400)){
			startMotor(rotationMotor, 40);
			isRotationMotorOn = true;
		}
		if ((SensorValue(rotateLeft) > 400)){
			startMotor(rotationMotor, -40);
			isRotationMotorOn = true;
		}
	}
	if((isRotationMotorOn) == true){
		if ((SensorValue(rotateLeft) < 400) && (SensorValue(rotateRight) < 400)){
			stopMotor(rotationMotor);
			isRotationMotorOn = false;
		}
	}

	// End rotation motor

	//Code for pitch motor


	if ((isPitchMotorOn) == false){
		if((SensorValue(pitchUp) > 400) && (isWristLimit == false)){
			startMotor(pitchMotor, 60);
			waitInMilliseconds(250);
			startMotor(pitchMotor, 70);
			isPitchMotorOn = true;
		}
		if ((SensorValue(pitchDown) > 400)){
			startMotor(pitchMotor, -60);
			waitInMilliseconds(250);
			startMotor(pitchMotor, -70);
			isPitchMotorOn = true;
		}
	}
	if((isPitchMotorOn) == true){
		if ((SensorValue(pitchDown) < 400) && (SensorValue(pitchUp) < 400)){
			stopMotor(pitchMotor);
			isPitchMotorOn = false;
		}
	}

	// End pitch motor

} //End While Loop

}