BalcitaBot.c

#pragma config(Sensor, dgtl1,  rear_end_detection, sensorTouch)
#pragma config(Motor,  port3,           right_wheels,  tmotorVex393HighSpeed_MC29, openLoop)
#pragma config(Motor,  port4,           left_wheels,   tmotorVex393HighSpeed_MC29, openLoop)
#pragma config(Motor,  port5,           arm1,          tmotorVex393HighSpeed_MC29, openLoop)
#pragma config(Motor,  port6,           arm2,          tmotorVex393HighSpeed_MC29, openLoop, reversed)
#pragma config(Motor,  port7,           arm3,          tmotorVex393HighSpeed_MC29, openLoop)
#pragma config(Motor,  port8,           arm4,          tmotorVex393HighSpeed_MC29, openLoop)
#pragma config(Motor,  port9,           arm5,          tmotorVex393HighSpeed_MC29, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

task main()
{
	int vert_stick, hor_stick;
	int left_motors, right_motors;
	int arms;
	int lifting;
	while(true){
		// DRIVING
		// inputs
		vert_stick = vexRT[Ch3];
		hor_stick = vexRT[Ch1];
		// deadzone
		if(-13 < vert_stick && vert_stick < 13){
			vert_stick = 0;
		}
		if(-13 < hor_stick && hor_stick< 13){
			hor_stick = 0;
		}
		// calculate speeds
		left_motors = vert_stick + hor_stick;
		right_motors = - vert_stick + hor_stick;
		// set speeds
		motor[left_wheels] = left_motors;
		motor[right_wheels] = right_motors;

		// SHOOTING
		// buttons

		lifting = vexRT[Ch2Xmtr2];

		if(vexRT[Btn6UXmtr2] == 1){
			arms = -128;
		}
		else if(vexRT[Btn6DXmtr2] == 1){
			arms = 128;
		}
		else{
			arms = -lifting;
		}

		// set speeds
		motor[arm1] = arms;
		motor[arm2] = arms;
		motor[arm3] = arms;
		motor[arm4] = arms;
		motor[arm5] = arms;
	}
}