driver_arcade.c

#pragma config(Hubs,  S1, HTMotor,  HTMotor,  none,     none)
#pragma config(Hubs,  S4, HTServo,  HTMotor,  none,     none)
#pragma config(Sensor, S2,     touch,          sensorTouch)
#pragma config(Sensor, S3,     IRSEEKER,       sensorI2CCustom)
#pragma config(Motor,  motorA,           ,             tmotorNXT, openLoop, encoder)
#pragma config(Motor,  motorB,           ,             tmotorNXT, openLoop, encoder)
#pragma config(Motor,  motorC,           ,             tmotorNXT, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C1_1,     motor_left,    tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C1_2,     motor_right,   tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_1,     motorLift,     tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_2,     motorFlag,     tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S4_C2_1,     motorFly,      tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S4_C2_2,     motorI,        tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S4_C1_1,    servoBucket,          tServoStandard)
#pragma config(Servo,  srvo_S4_C1_2,    servoBlock,           tServoStandard)
#pragma config(Servo,  srvo_S4_C1_3,    servoFlagAdjust,      tServoStandard)
#pragma config(Servo,  srvo_S4_C1_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S4_C1_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S4_C1_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

// Robotics and Programming Guild Code
#include "JoystickDriver.c" //brings in controller functions

// Give Joystick Buttons Good Names
#define	JOY_BUTTON_X		01
#define	JOY_BUTTON_A		02
#define	JOY_BUTTON_B		03
#define	JOY_BUTTON_Y		04
#define	JOY_BUTTON_LB		05
#define JOY_BUTTON_LT		07
#define	JOY_BUTTON_RB		06
#define JOY_BUTTON_RT		08

/* =====================================================
	 = Program constant definitions
	 ===================================================== */
#define JOY_THRESHOLD			20		// Allow controllers to have small dead spot

#define DRIVE_POWER				90		// Default Drive Motor Power
#define MID_POWER				60		// If Button 7 Pressed drive at 60% of current
#define SLOW_POWER 				30		// If Button 5 Pressed drive at 30% of current

#define LIFT_POWER 				90		// Default Lift Motor Power
#define FLAG_POWER 				90		// Default Flag Power
#define FLY_POWER				75		// Default Fly Power

#define BUCKET_INIT 			60		// Bucket Servo Setting for Start
#define BUCKET_LIFT				5		// Bucket Servo Setting to Lift Blocks
#define BUCKET_DROP				205		// Bucket Servo Setting to Drop Blocks
#define BUCKET_MAX				210		// Maximum Servo Setting for the Bucket
#define BUCKET_MIN				0		// Minimum Servo Setting for the Bucket Servo
#define BUCKET_CHANGE_RT		1		// Speed that the bucket will increment

#define BLOCK_INIT				255		// Block Servo Setting for Start
#define BLOCK_FLING				80	// Block Servo Setting for Block Fling
#define BLOCK_REST				255		// Block Servo Setting when not in use

/* =====================================================
	 = Program global variables
	 ===================================================== */
// bool bucketMoving = false;

/* =====================================================
	 = JoyStick 2 Activators Functions
	 ===================================================== */
void bucketSet(int servoSetting)
{
	if( (servoSetting <= BUCKET_MAX) && (servoSetting >= BUCKET_MIN) ) {
		int iCRate = servoChangeRate[servoBucket];	// Save change rate
		servoChangeRate[servoBucket] = 0; 					// Max Speed
		servo[servoBucket] = servoSetting;					// Set servo position
		wait1Msec(20);
		servoChangeRate[servoBucket] = iCRate;			// Reset the servo
	}
}
void bucketAdjust(int servoAdjustment)
{
	int servoSetting = servo[servoBucket] - servoAdjustment;
	if( (servoSetting <= BUCKET_MAX) && (servoSetting >= BUCKET_MIN) ) {
		servo[servoBucket] = servoSetting;
		wait1Msec(5);
	}
}
void bucketInit()
{
	servoChangeRate[servoBucket] = 1;
	bucketSet(BUCKET_INIT);
}
/* =====================================================
	 = Run Controller 2 - Activators
	 =
	 =	Mappings:
	 =		joy1_y1 	: control lift
	 =		joy1_y2 	: alternative bucket control
	 =		LT			: move the flag out
	 =		LB			: move the flag in
	 =		RB			: spin flag motor backward
	 =		RT			: spin flag motor forward
	 =		A 			: raise the bucket
	 =		B 			: lower the bucket
	 =		X			: set bucket to preset
	 =		Y 			: set bucket to preset
	 ===================================================== */
void doController2()
{
	// Raise and Lower the Lift
	if(abs(joystick.joy2_y1) > (JOY_THRESHOLD)) {
		if( (joystick.joy2_y1 < 0 && SensorValue[touch] != 1) || (joystick.joy2_y1 > 0) ) {
			motor[motorLift] = (joystick.joy2_y1 / 127) * LIFT_POWER;
		}
		else {
			motor[motorLift] = 0;
		}
	}
	else{
		motor[motorLift] = 0;
	}

	// Raise and Lower the Bucket
	if(joy2Btn(JOY_BUTTON_X)){
		bucketSet(BUCKET_DROP);
	}
	else if(joy2Btn(JOY_BUTTON_B)){
		bucketSet(BUCKET_LIFT);
	}
	else if(joy2Btn(JOY_BUTTON_A)) {
		bucketAdjust(-BUCKET_CHANGE_RT);
	}
	else if (joy2Btn(JOY_BUTTON_Y)) {
		bucketAdjust(BUCKET_CHANGE_RT);
	}
	else if(abs(joystick.joy2_y2) > JOY_THRESHOLD) {
		bucketAdjust( (joystick.joy2_y2>0 ? BUCKET_CHANGE_RT : -BUCKET_CHANGE_RT) );
	}

	// Run the flag spin motor forward, backward or stop
	if(joy2Btn(JOY_BUTTON_RB)) {
		motor[motorFlag] = -FLAG_POWER;
	}
	else if(joy2Btn(JOY_BUTTON_RT)) {
		motor[motorFlag] = FLAG_POWER;
	}
	else {
		motor[motorFlag] = 0;
	}

	// Run the flag adjusting servo forward, backward or stop
	if(joy2Btn(JOY_BUTTON_LB)) {
		servo[servoFlagAdjust] = 256;
	}
	else if(joy2Btn(JOY_BUTTON_LT)) {
		servo[servoFlagAdjust] = 0;
	}
	else {
		servo[servoFlagAdjust] = 127;
	}
}

/* =====================================================
	 = JoyStick 1 Activators Functions
	 ===================================================== */

// Arcade drive function using formula below
// Get X and Y from the Joystick, do whatever scaling and calibrating you need to do based on your hardware.
// Invert X
// Calculate R+L (Call it V): V =(100-ABS(X)) * (Y/100) + Y
// Calculate R-L (Call it W): W= (100-ABS(Y)) * (X/100) + X
// Calculate R: R = (V+W) /2
// Calculate L: L= (V-W)/2
// Do any scaling on R and L your hardware may require.
void arcadeDrive(int joy_x, int joy_y)
{
	int powerAdjust;
	if(joy1Btn(JOY_BUTTON_LT)) {
		powerAdjust = SLOW_POWER;
	}
	else if(joy1Btn(JOY_BUTTON_LB)){
		powerAdjust = MID_POWER;
	}
	else {
		powerAdjust = DRIVE_POWER;
	}

	// scale X and Y to the drive range of -100 to 100 with the correct power adjustment
	int scaled_X = (joy_x / 127) * powerAdjust;
	int scaled_Y = (joy_y / 127) * powerAdjust;

	// if either joy_x or joy_y is above the threshold
	if(abs(joy_y) > JOY_THRESHOLD) {
		// set the motors
		motor[motor_right] = scaled_Y;
		motor[motor_left] = scaled_Y;
	}
	else if(abs(joy_x) > JOY_THRESHOLD) {
	// set the motors
		motor[motor_right] = -scaled_X;
		motor[motor_left] = scaled_X;
		}
	else {
		// stop the motors
		motor[motor_left] = 0;
		motor[motor_right] = 0;
	}
}

// Manage Servo
void blockSet(int blockSetting) {
	if(servo[servoBlock] > blockSetting) {
		servoChangeRate[servoBlock] = 10;
	}
	else {
		servoChangeRate[servoBlock] = 0;
	}
	servo[servoBlock] = blockSetting;
}
void blockInit() {
	blockSet(BLOCK_INIT);
}

/* =====================================================
	 = Run Controller 1 - Drive Motors
	 =
	 =	Mappings:
	 =		joy1_y1 	: control drive motors using arcade drive
	 =		joy1_y2 	:
	 =		LB			: reduce drive power to middle setting
	 =		LT			: reduce drive power to slow setting
	 =		RB			: control fly motor
	 =		RT			: control reverse fly motor
	 =		A 			: set the block servo to resting point
	 =		B 			:
	 =		X			:
	 =		Y 			: set the block server to deliver point
	 ===================================================== */
void doController1()
{

	arcadeDrive(joystick.joy1_x2, joystick.joy1_y1);

	// control fly motor
	if(joy1Btn(JOY_BUTTON_RT)) {
		motor[motorFly] = FLY_POWER;
	}
	else if(joy1Btn(JOY_BUTTON_RB)) {
		motor[motorFly] = -FLY_POWER;
	}
	else {
		motor[motorFly] = 0;
	}

	// Control the flinger arm
	if(joy1Btn(JOY_BUTTON_Y)) {
		blockSet(BLOCK_FLING);
	}
	else if(joy1Btn(JOY_BUTTON_A)) {
		blockSet(BLOCK_REST);
	}
}

/* =====================================================
	 = General FTC / Robot functions
	 ===================================================== */
void initializeRobot(){
	bDisplayDiagnostics = false;
	bFloatDuringInactiveMotorPWM = false;
	blockInit();
	bucketInit();
	servo[servoFlagAdjust] = 127;
	return;
}

void allMotorsStop(){
	motor[motor_left] = 0;
	motor[motor_right] = 0;
	motor[motorLift] = 0;
	motor[motorFlag] = 0;
	motor[motorFly] = 0;
	servo[servoFlagAdjust] = 127;
}

/* =====================================================
	 = Main RobotC Function
	 ===================================================== */
task main() {
	initializeRobot();

 	waitForStart(); // Wait for the beginning of driver phase.

	//infinite loop for control
	while( true ) {
		if(!bDisconnected){
			getJoystickSettings(joystick);
			doController1();
			doController2();
		}
		else{
			allMotorsStop();
		}
	}
}