Robotc IR Autonomous Code

#pragma config(Hubs, S1, HTMotor, HTMotor, HTServo, none)

#pragma config(Hubs, S2, HTMotor, HTMotor, none, none)

#pragma config(Sensor, S3, IRSeeker2, sensorHiTechnicIRSeeker1200)

#pragma config(Motor, mtr_S1_C1_1, motor_lin1, 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, motor_lin3, 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, tServoNone)

#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 "hitechnic-irseeker-v2.h" //required driver for ir sensor

#include "JoystickDriver.c" //required driver for joystick

void initializeRobot() //place for sensor resets and pre-game preparation.

{

nMotorEncoder(motor_left1) =0;//D //Set all motor rotation counts to 0

nMotorEncoder(motor_right1) =0;//E

nMotorEncoder(motor_left2) =0;//F

nMotorEncoder(motor_right2) =0;//G

nMotorEncoder(motor_lin1) =0;

return;

}

task main() //main instruction for the robot starts

{

initializeRobot(); //Prepares robot, communicates with Field control system.

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

while(true) // Repeat the below code for ever until specified otherwise.

{

int sen1 = SensorValue[IRSeeker2]; //Obtain ir seeker sensor value each time loop is run.

nxtDisplayTextLine(3, "%d", SensorValue[IRSeeker2]); //Display on the third line of the NXT brick display the ir seeker value.

//Value ranges from 1 to 9: 1 being left and 9 being right.

if(sen1 != 9) //If the ir sensor does not receive an ir signal from the 9th sector, which is the right side, run the following code.

{

motor(motor_left1)= -60; //move forward

motor(motor_left2)= 60;

motor(motor_right1)= -60;

motor(motor_right2)= 60;

nxtDisplayTextLine(1, "%d", nMotorEncoder(motor_left1)); //Display rotation values of the motors on the 1st, 2nd, 3rd, and 4th lines of the NXT display.

nxtDisplayTextLine(2, "%d", nMotorEncoder(motor_left2));

nxtDisplayTextLine(3, "%d", nMotorEncoder(motor_right1));

nxtDisplayTextLine(4, "%d", nMotorEncoder(motor_right2));

}

else

{

if(sen1 == 9) //If the ir sensor receives a signal from the 9th sector, run the following code.

{

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);

nxtDisplayTextLine(5, "%d", Dm); //Display the rotations taken to reach the ir beacon.

motor(motor_left1)= -60; // goes past bucket a little

motor(motor_left2)= 60;

motor(motor_right1)= -60;

motor(motor_right2)= 60;

wait1Msec(400);

motor(motor_left1)= 0; // stops

motor(motor_left2)= 0;

motor(motor_right1)= 0;

motor(motor_right2)= 0;

wait1Msec(300);

motor(motor_left1)= -60; // turn towards boxes

motor(motor_left2)= -60;

motor(motor_right1)= -60;

motor(motor_right2)= -60;

wait1Msec(700);

motor(motor_left1)= 0; // stops

motor(motor_left2)= 0;

motor(motor_right1)= 0;

motor(motor_right2)= 0;

wait1Msec(300);

//change linear slide raise to here

//raise it, then dump

while(nMotorEncoder(motor_lin1 < x) // x = # rotations it takes to raise the slide)

{

motor(motor_lin1)= 60; //move linear slide up

motor(motor_lin2)= 60;

motor(motor_lin3)= -60;

motor(motor_lin4)= -60;

}

// need dump

//lower slide a tad

motor(motor_left1)= 60; // turn away from boxes

motor(motor_left2)= 60;

motor(motor_right1)= 60;

motor(motor_right2)= 60;

wait1Msec(700);

motor(motor_left1)= 0; // stop

motor(motor_left2)= 0;

motor(motor_right1)= 0;

motor(motor_right2)= 0;

wait1Msec(300);

int DDm = nMotorEncoder(motor_left1);

int newdm = Dm;

while(nMotorEncoder(motor_left1) < newdm) //Run code until rotation values reach Dm. (Go back the same distance that it took to come to the beacon(?))

{

motor(motor_left1)= 60; //goes forward

motor(motor_left2)= -60;

motor(motor_right1)= 60;

motor(motor_right2)= -60;

}

motor(motor_left1)= 0; //wait

motor(motor_left2)= 0;

motor(motor_right1)= 0;

motor(motor_right2)= 0;

wait1Msec(300);

motor(motor_left1)= 60; //turn

motor(motor_left2)= 60;

motor(motor_right1)= 60;

motor(motor_right2)= 60;

wait1Msec(340);

motor(motor_left1)= 60; //back

motor(motor_left2)= -60;

motor(motor_right1)= 60;

motor(motor_right2)= -60;

wait1Msec(1400);

motor(motor_left1)= 60; //turn

motor(motor_left2)= 60;

motor(motor_right1)= 60;

motor(motor_right2)= 60;

wait1Msec(340);

motor(motor_left1)= 60; //back

motor(motor_left2)= -60;

motor(motor_right1)= 60;

motor(motor_right2)= -60;

wait1Msec(900);

motor(motor_left1)= 0; //wait

motor(motor_left2)= 0;

motor(motor_right1)= 0;

motor(motor_right2)= 0;

wait1Msec(350);

motor(motor_left1)= 60; //turn

motor(motor_left2)= 60;

motor(motor_right1)= 60;

motor(motor_right2)= 60;

wait1Msec(640);

motor(motor_left1)= 0; //wait

motor(motor_left2)= 0;

motor(motor_right1)= 0;

motor(motor_right2)= 0;

wait1Msec(300);

motor(motor_left1)= 60; // bridge

motor(motor_left2)= -60;

motor(motor_right2)= 60;

motor(motor_right2)= -60;

wait1Msec(2000);

//end

break;

}

}

}

}