#pragma config(Hubs, S1, HTMotor, HTMotor, HTServo, none)
#pragma config(Hubs, S2, HTMotor, HTMotor, none, none)
#pragma config(Sensor, S1, , sensorI2CMuxController)
#pragma config(Sensor, S2, , sensorI2CMuxController)
#pragma config(Sensor, S3, IRSeeker2, sensorHiTechnicIRSeeker1200)
#pragma config(Motor, mtr_S1_C1_1, motor_flag, tmotorTetrix, PIDControl, encoder)
#pragma config(Motor, mtr_S1_C1_2, motor_lin2, tmotorTetrix, PIDControl, encoder)
#pragma config(Motor, mtr_S1_C2_1, motor_left1, tmotorTetrix, PIDControl, encoder)
#pragma config(Motor, mtr_S1_C2_2, motor_left2, tmotorTetrix, PIDControl, encoder)
#pragma config(Motor, mtr_S2_C1_1, motor_right1, tmotorTetrix, PIDControl, encoder)
#pragma config(Motor, mtr_S2_C1_2, motor_right2, tmotorTetrix, PIDControl, encoder)
#pragma config(Motor, mtr_S2_C2_1, notused, tmotorTetrix, PIDControl, encoder)
#pragma config(Motor, mtr_S2_C2_2, motor_lin4, tmotorTetrix, PIDControl, encoder)
#pragma config(Servo, srvo_S1_C3_1, servo1, tServoStandard)
#pragma config(Servo, srvo_S1_C3_2, servo2, tServoStandard)
#pragma config(Servo, srvo_S1_C3_3, servo3, tServoContinuousRotation)
#pragma config(Servo, srvo_S1_C3_4, servo4, tServoNone)
#pragma config(Servo, srvo_S1_C3_5, servo5, tServoNone)
#pragma config(Servo, srvo_S1_C3_6, servo6, tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
#include "JoystickDriver.c";
/**
* Helper function to scale analog joystick values for tetrix dc motors.
*/
void initializeRobot() //don't touch
{//absolutely nothing
//Set all motor rotation counts to 0
nMotorEncoder(motor_lin2) =0;//E
nMotorEncoder(motor_lin4) =0;//G
}
int scaleForMotor(int joyValue)
{
const int DEADZONE = 5;
const int MAX_MOTOR_VAL = 154; //don't touch
const float MAX_JOY_VAL = 127;
//check for deadzone
if(abs(joyValue) < DEADZONE) {
return 0;
}
//calculate scaled value
int direction = joyValue / abs(joyValue); // 1 or -1
float ratio = ((joyValue * joyValue) / (MAX_JOY_VAL * MAX_JOY_VAL));
int scaledVal = (ratio * MAX_MOTOR_VAL) * direction; //don't touch
return scaledVal;
}
task main()
{
initializeRobot();
waitForStart(); //wait for field control system to start
while(true)
{
int Dm = nMotorEncoder(motor_left1); //Save the current rotation values to integers Dm, Em, Fm, and Gm for future
int Em = nMotorEncoder(motor_left2);
int Fm = nMotorEncoder(motor_right1);
int Gm = nMotorEncoder(motor_right2);
//Display rotation values of the motors on the 1st, 2nd, 3rd, and 4th lines of the NXT display.
nxtDisplayTextLine(2, "%d", nMotorEncoder(motor_lin2));
nxtDisplayTextLine(4, "%d", nMotorEncoder(motor_lin4));
getJoystickSettings(joystick); //receive inputs from controllers
motor[motor_left1] = scaleForMotor(joystick.joy1_y1/-127*150);
motor[motor_left2] = scaleForMotor(joystick.joy1_y1/-127*150);
motor[motor_right1] = scaleForMotor(joystick.joy1_y2/127*150);
motor[motor_right2] = scaleForMotor(joystick.joy1_y2/127*150); //tank controls
if (joy2Btn(4) == 1) //controller2_Y_slide_up
{
motor(motor_lin2) = -60;
motor(motor_lin4) = 60;
nxtDisplayTextLine(1, "slide down"); //print to NXT display
}
else if (joy1Btn(8) == 1) //controller1_lefttrigger_sweeper_in
{
motor(motorA) = -60;
motor(motorB) = -60;
nxtDisplayTextLine(1, "sweeper in");
}
else if (joy1Btn(1) == 1) //controller1_Y_armdown
{
//servo(servo3) = 1;
nxtDisplayTextLine(1, "sweeper in");
}
else if (joy2Btn(2) == 1) //controller2_A_slide_down
{
if (nMotorEncoder(motor_lin4) > -8300) //made up number for now
{
motor(motor_lin2) = 60;
motor(motor_lin4) = -60;
nxtDisplayTextLine(1, "slide up");
}
}
else if (joy1Btn(7) == 1) //controller1_lefttrigger_Y_sweeperout
{
motor(motorA) = 60;
motor(motorB) = 60;
nxtDisplayTextLine(1, "sweeper out");
}
else if (joy1Btn(2) == 1) //controller1_A_bucket_down
{
int count = 1;
while(count == 1)
{
servo(servo1) = 120;
servo(servo2) = 136;
nxtDisplayTextLine(1, "bucket down");
if (joy1Btn(2) == 1)
{
count++;
}
}
}
else if (joy1Btn(3) == 1) //controller1_B_bucket_up
{
int count = 1;
while(count == 1)
{
servo(servo1) = 86;
servo(servo2) = 170;
nxtDisplayTextLine(1, "bucket down");
if (joy1Btn(3) == 1)
{
count++;
}
}
}
else if (joy1Btn(4) == 1) //controller1_Y_armmid
{
int count = 1;
while(count == 1)
{
servo(servo1) = 46;
servo(servo2) = 210;
nxtDisplayTextLine(1, "bucket down");
if (joy1Btn(4) == 1)
{
count++;
}
}
}
else if (joy2Btn(1) == 1)
{
motor(motor_flag) = 60;
}
else
{
if (joy2Btn(3) == 1)
{
motor(motor_flag) = -60;
}
else //absolutely nothing
{
//servo(servo1) = 255;
//servo(servo2) = 1;
motor(motorA) = 0; //Every motor stopped, servos back to its position
motor(motorB) = 0;
motor(motor_lin2) = 0;
motor(motor_lin4) = 0;
motor(motor_flag) = 0;
nxtDisplayTextLine(1, "slide stop");
nxtDisplayTextLine(1, "Nothing, bucket up");
}
}
}
}
