void initializeRobot() {
bDisplayDiagnostics=false;
setGyroPos(false);
if (HTSMUXreadPowerStatus(MUX)) { // Multiplexer is off or dead?
nxtDisplayBigStringAt(0,45," MUX ");
nxtDisplayBigStringAt(0,25,"Dead Batt");
PlayImmediateTone(440, 50);
wait1Msec(1500);
eraseDisplay();
fallbackMode=true;
selection[SONAR_MENU]=3;
}
if (SensorRaw[gyro]>=640 || SensorRaw[gyro]<=590) { // Gyro moving or disconnected
nxtDisplayBigStringAt(21,45,"Gyro");
nxtDisplayBigStringAt(15,25,"Error");
nxtDisplayCenteredTextLine(7, "%i", SensorRaw[gyro]);
PlayImmediateTone(440, 50);
wait1Msec(1500);
setGyroEnabled(false);
} else {
wait1Msec(100);
nxtDisplayBigStringAt(15,45,"Wait...");
HTGYROstartCal(gyro);
PlaySound(soundBeepBeep);
setGyroEnabled(true);
}
eraseDisplay();
setClamp(true);
initMenus();
return;
}
task main()
{
nVolume = 4;
while (true)
{
while(SensorValue[touch] == 0 && SensorValue[distance] >=25)
{
motor[rMotor] = 75;
motor[lMotor] = 75;
}
while(SensorValue[distance] <25)
{
motor[rMotor] = -100;
motor[lMotor] = -100;
wait1Msec (750);
motor[rMotor] = 75;
motor[lMotor] = -75;
PlayImmediateTone(1500, 100);
wait1Msec (1300);
}
while(SensorValue[touch] == 1)
{
motor[rMotor] = 75;
motor[lMotor] = 75;
wait1Msec (750);
}
}
}
task main () {
int _x_tilt = 0;
int _y_tilt = 0;
int _z_tilt = 0;
int tone1;
int tone2;
int waitTime;
nxtDisplayCenteredTextLine(0, "Mindsensors");
nxtDisplayCenteredBigTextLine(1, "ACCEL-Nx");
nxtDisplayCenteredTextLine(3, "Test 2");
wait1Msec(2000);
// There are four ranges the ACCL-Nx can measure in
// up to 2.5G - MSAC_RANGE_2_5
// up to 3.3G - MSAC_RANGE_3_3
// up to 6.7G - MSAC_RANGE_6_7
// up to 10G - MSAC_RANGE_10
MSACsetRange(MSAC, MSAC_RANGE_10);
PlaySound(soundBeepBeep);
while(bSoundActive) EndTimeSlice();
while (true) {
// Read the tilt data from the sensor
if (!MSACreadTilt(MSAC, _x_tilt, _y_tilt, _z_tilt)) {
nxtDisplayTextLine(4, "ERROR!!");
wait1Msec(2000);
StopAllTasks();
}
// Tilt values seem to go from about -20 to +20.
// Adding 20 to them makes them go from 0 to 40.
// Make sure the tones are at least 0Hz
tone1 = max2(0, (_x_tilt + 20) * 20);
tone2 = max2(0, (_z_tilt + 20) * 25);
// Make sure the wait time is at least 10ms
waitTime = max2(10, (_y_tilt + 20));
PlayImmediateTone(tone1, 5);
wait1Msec(waitTime);
PlayImmediateTone(tone2, 1);
}
}
task main()
{
initializeRobot();
waitForStart(); // Wait for the beginning of autonomous phase.
servo[HookLeft] = 205;
servo[HookRight] = 20;
nMotorEncoderTarget[ArmMotor] = ArmTarget;
motor[ArmMotor] = 50;
nMotorEncoderTarget[RightDriveMotor] = rampLengthTicks;//
motor[RightDriveMotor]= 100; //drive off
nMotorEncoderTarget[LeftDriveMotor] = rampLengthTicks; //the ramp
motor[LeftDriveMotor] = 100; //
while(nMotorRunState[RightDriveMotor] != runStateIdle && nMotorRunState[LeftDriveMotor] != runStateIdle){
}
motor[RightDriveMotor] = 0;
motor[LeftDriveMotor] = 0;
motor[ArmMotor] = 0;
PlayImmediateTone(2500,100);
int count = 0;
while(count< 15000){
if(SensorValue[light] < 25){
//lightTemp = SensorValue[light];
motor[LeftDriveMotor] = fast;
motor[RightDriveMotor] = slow;
}
else{
motor[LeftDriveMotor] = slow;
motor[RightDriveMotor] = fast;
}
count++;
wait1Msec(1);
}
//All Stop
motor[LeftDriveMotor]=0;
motor[RightDriveMotor]=0;
PlayImmediateTone(2500,100);
wait1Msec(500);
PlayImmediateTone(2500,100);
//Lift bucket to dump balls
}
task main () {
nxtDisplayCenteredTextLine(0, "MindSensors");
nxtDisplayCenteredBigTextLine(1, "TouchPnl");
nxtDisplayCenteredTextLine(3, "Noise-a-Tron");
nxtDisplayCenteredTextLine(5, "Connect sensor");
nxtDisplayCenteredTextLine(6, "to S3");
wait1Msec(2000);
eraseDisplay();
int x, y = 0;
ubyte buttons = 0;
int multiplierX = 10;
int multiplierY = 10;
while (true) {
if (!MSTPgetTouch(MSTP, x, y, buttons))
PlaySound(soundBlip);
else if (isButtonTouched(buttons, BUTTON_L1))
multiplierX = 10;
else if (isButtonTouched(buttons, BUTTON_L2))
multiplierX = 15;
else if (isButtonTouched(buttons, BUTTON_L3))
multiplierX = 20;
else if (isButtonTouched(buttons, BUTTON_L4))
multiplierX = 25;
else if (isButtonTouched(buttons, BUTTON_R1))
multiplierY = 10;
else if (isButtonTouched(buttons, BUTTON_R2))
multiplierY = 15;
else if (isButtonTouched(buttons, BUTTON_R3))
multiplierY = 20;
else if (isButtonTouched(buttons, BUTTON_R4))
multiplierY = 25;
else if (x > 0 && y > 0)
{
PlayImmediateTone(x * multiplierX, 1);
wait1Msec(10);
PlayImmediateTone(y * multiplierY, 1);
nxtSetPixel(x, y);
}
EndTimeSlice();
}
}
task main()
{
if(getChoice("What pitch?", "Low", "High") == 0)
{
PlayImmediateTone(440, 100);
}else{
PlayTone(880, 100);
}
wait1Msec(1000);
}
void initializeRobot()
{
// Place code here to initialize servos to starting positions.
if (nAvgBatteryLevel < 8000) { // brick under 8.0 volts
PlayImmediateTone(1000, 100); // play a warning tone
PlaySoundFile("NxtBatteryLow.rso");
}
if (externalBatteryAvg < 12000) { // main battery under 12.0
int i;
// play a warning siren once for
// every tenth of a volt too low
for (i = 0; i < (14000 - externalBatteryAvg) / 100; i++) {
PlayImmediateTone(2000, 300); // play a warning tone
PlaySoundFile("ExternalBatteryLow.rso");
}
}
return;
}
task irBeep(){
if(th > (acS2a+acS4b/2)+10){
while(true){
int ir1 = acS1a+acS2a+acS3a+acS4a+acS5a;
int ir2 = acS1b+acS2b+acS3b+acS4b+acS5b;
ir1 = ir1/5;
ir2 = ir2/5;
PlayImmediateTone(ir1+ir2/2, 10);
}
}
}
void initializeRobot()
{
// Place code here to sinitialize servos to starting positions.
// Sensors are automatically configured and setup by ROBOTC. They may need a brief time to stabilize.
if (nAvgBatteryLevel < 8000) { // brick under 8.0 volts
PlayImmediateTone(1000, 100); // play a warning tone
PlaySoundFile("NxtBatteryLow.rso");
}
if (externalBatteryAvg < 14000) { // main battery under 14.0
int i;
// play a warning siren once for
// every tenth of a volt too low
for (i = 0; i < (14000 - externalBatteryAvg) / 100; i++) {
PlayImmediateTone(2000, 300); // play a warning tone
PlaySoundFile("ExternalBatteryLow.rso");
}
}
return;
}
task main()
{
//wait
servo[topServo] = 235;
initializeRobot();
StartTask(getHeading);
wait1Msec(400);
forwardInc(48, 30);
//Calculates the positioning of the center goal after 10 position tics at 200msec intervals
float a = 0;
float b = 0;
float c = 0;
wait1Msec(400);
StartTask(infared);
wait1Msec(200);
int ir2 = acS1a+ acS2a+acS3a+acS4a+acS5a;
int ir1 = acS1b+acS2b+acS3b+acS4b+acS5b;
for(float i = 0; i <= 10; i++){
if(ir1 < ir2-10){
PlayImmediateTone(440, 10);
a += 1;
}else if(abs(ir1-ir2) <= 18){ //ir1-4 > ir2 && ir1+4 < ir2
PlayImmediateTone(1024, 10);
b += 1;
}else if(ir1 > ir2+10){
PlayImmediateTone(2024, 10);
c += 1;
}else{
PlayImmediateTone(1024, 10);
//b += 1;
}
wait1Msec(200);
}
ClearSounds();
//code for each of the three center positions a=1 b=2 c=3
StopTask(getHeading);
wait10Msec(50);
StartTask(getHeading);
wait1Msec(200);
if(a > (b+c)/2){
//PlayImmediateTone(440, 10);
currHeading = 0;
turnDeg(-20, 25);
wait10Msec(50);
forwardInc(74, 30);
wait10Msec(50);
StopTask(getHeading);
currHeading = 0;
wait1Msec(200);
StartTask(getHeading);
wait1Msec(100);
turnDeg(-52, 20);//sdfhjkjhgftrhjkl;kjrtykl;kjgfdsghj.,mhfdsfghjkfdsgtgr5gtgr
wait10Msec(25);
forwardInc(-22, 25);
liftPos(3);
servo[topServo] = 210;//original 150
wait10Msec(25);
servo[topServo] = 235;
wait10Msec(500);
liftPos(0);
wait10Msec(500);
}else if(b > (a+c)/2){
PlayImmediateTone(1024, 10);
StopTask(getHeading);
currHeading = 0;
wait1Msec(200);
StartTask(getHeading);
wait1Msec(100);
turnDeg(180, 25); //do a one 80
StopTask(getHeading);
currHeading = 0;
wait1Msec(200);
StartTask(getHeading);
wait10Msec(70);
forwardInc(-36, 30);//go back __ inches
//StartTask(getHeading);
//wait1Msec(100);
//turnDeg(20, 25);
wait10Msec(200);
liftPos(3);
servo[topServo] = 210;//original 150
wait10Msec(25);
servo[topServo] = 235;
wait10Msec(100);
liftPos(0);
wait10Msec(500);
}else if(c > (a+b)/2){
PlayImmediateTone(2024, 10);
StopTask(getHeading);
currHeading = 0;
wait1Msec(200);
StartTask(getHeading);
wait1Msec(100);
turnDeg(-120, 25);
StopTask(getHeading);
currHeading = 0;
wait1Msec(200);
StartTask(getHeading);
wait10Msec(70);
forwardInc(-35, 30);
StartTask(getHeading);
wait1Msec(100);
turnDeg(-32, 25);
forwardInc(-4, 25);
wait10Msec(50);
liftPos(3);
servo[topServo] = 210;//original 150
wait10Msec(25);
servo[topServo] = 235;
wait10Msec(100);
liftPos(0);
wait10Msec(500);
}
}
/////////////////////////////////////////////////////////////////////////////////////////////////////
//
// Main Task
//
// The following is the main code for the autonomous robot operation. Customize as appropriate for
// your specific robot.
//
// The types of things you might do during the autonomous phase (for the 2008-9 FTC competition)
// are:
//
// 1. Have the robot follow a line on the game field until it reaches one of the puck storage
// areas.
// 2. Load pucks into the robot from the storage bin.
// 3. Stop the robot and wait for autonomous phase to end.
//
// This simple template does nothing except play a periodic tone every few seconds.
//
// At the end of the autonomous period, the FMS will autonmatically abort (stop) execution of the program.
//
/////////////////////////////////////////////////////////////////////////////////////////////////////
task main()
{
initializeRobot();
while(true)
{
bDisplayDiagnostics = false;// Wait for the beginning of autonomous phase.
eraseDisplay();
// WriteShort(hFileHandle, nIoResult, nParm);
if ( externalBatteryAvg < 0)
{
while (externalBatteryAvg < 0)
{
eraseDisplay();
nxtDisplayTextLine(1, "Ext Batt: OFF"); //External battery is off or not connected
nxtDisplayCenteredBigTextLine(1, "What!?");
nxtDisplayTextLine(3, "My name is,");
nxtDisplayTextLine(4, "Iego Montoya,");
nxtDisplayTextLine(5, "you did not turn");
nxtDisplayTextLine(6, "on the robot");
nxtDisplayTextLine(7, "NOW YOU DIE");
nxtDisplayTextLine(8, "YOU COCKROACH!!!!!!");
PlayImmediateTone(600, 20);
PlayImmediateTone(400, 20);
wait10Msec(20);
}
}
else
{
nxtDisplayTextLine(1, "Ext Batt:%4.1f V", externalBatteryAvg / (float) 1000);
}
while ( externalBatteryAvg / (float) 1000 < 13 && externalBatteryAvg / (float) 1000 > 0 || nAvgBatteryLevel / (float) 1000 < 7)
{
eraseDisplay();
nxtDisplayBigTextLine(6, "Battery");
nxtScrollText("poopheads");
nxtScrollText("You didnt change the battery you shmuts!");
nxtScrollText("the battery you");
nxtScrollText("shmuts!");
PlayImmediateTone(600, 70);
PlayImmediateTone(400, 70);
wait10Msec(20);
}
nxtDisplayTextLine(2, "NXT Batt:%4.1f V", nAvgBatteryLevel / (float) 1000); // Display NXT Battery Voltage
nxtDisplayTextLine(3, "R = %d L = %d", nMotorEncoder[R_Motor], nMotorEncoder[L_Motor]);
nxtDisplayTextLine(4, "SONAR_1 = %d", USreadDist(SONAR_1));
nxtDisplayTextLine(5, "IR = %d", SensorValue[IR]);
nxtDisplayTextLine(6, "Gyro = %d", SensorValue[Gyro]);
wait10Msec(20);
}//End of While
//Actuation tests.
motor[R_Motor] = 100;
wait10Msec(30);
motor[R_Motor] = 0;
wait10Msec(30);
motor[L_Motor] = 100;
wait10Msec(30);
motor[L_Motor] = 0;
wait10Msec(30);
motor[Flag_Raiser] = 100;
wait10Msec(30);
motor[Flag_Raiser] = 0;
wait10Msec(20);
motor[Winch] = 100;
wait10Msec(30);
motor[Winch] = 0;
wait10Msec(30);
motor[Arm] = 100;
wait10Msec(30);
motor[Arm] = 0;
wait10Msec(30);
motor[Block_Sweep] = 100;
wait10Msec(30);
motor[Block_Sweep] = 0;
wait10Msec(30);
servo[Bucket_Release] = 100;
wait10Msec(30);
servo[Bucket_Release] = 0;
wait10Msec(30);
servo[Spring_Release] = 100;
wait10Msec(30);
servo[Spring_Release] = 0;
wait10Msec(30);
servo[IRS_1] = 100;
wait10Msec(30);
servo[IRS_1] = 0;
wait10Msec(30);
}
// Put the main driver control loop in its own tasks so
// the drivers never loses control of the robot!
task drive()
{
// Initialize variables
int lastMessage = 0;
int leftMotorSpeed = 0;
int rightMotorSpeed = 0;
int armMotorSpeed = 0;
int totalMessages = 0;
int topSpeed = MOTOR_POWER_DOWN_MAX;
int armTopSpeed = 65;
int scoopTopSpeed = 40;
int scoopMotorSpeed = 0;
while (true)
{
getJoystickSettings(joystick);
if (lastMessage != ntotalMessageCount) {
if (true) {
ClearTimer(T2);
lastMessage = ntotalMessageCount;
// New joystick messages have been received!
// Set your drive motors based on user input
// here.
leftMotorSpeed = joystick.joy1_y1 / JOYSTICK_Y1_MAX * topSpeed; // Map the leftMotorSpeed variable to joystick 1_y1
if (abs(leftMotorSpeed) < JOYSTICK_DEAD_ZONE) leftMotorSpeed = 0; // Make sure that the joystick isn't inside dead zone
rightMotorSpeed = joystick.joy1_y2 / JOYSTICK_Y1_MAX * topSpeed; // Map the rightMotorSpeed variable to joystick 1_y2
if (abs(rightMotorSpeed) < JOYSTICK_DEAD_ZONE) rightMotorSpeed = 0; // Make sure that the joystick isn't inside dead zone
armMotorSpeed = joystick.joy2_y2 / JOYSTICK_Y1_MAX * armTopSpeed; // Map the armMotorSpeed variable to joystick 2_y2
if (abs(armMotorSpeed) < JOYSTICK_DEAD_ZONE) armMotorSpeed = 0; // Make sure that the joystick isn't inside dead zone
scoopMotorSpeed = joystick.joy2_y1 / JOYSTICK_Y1_MAX * scoopTopSpeed; // Map the teleMotorSpeed variable to joystick 2_y1
if (abs(scoopMotorSpeed) < JOYSTICK_DEAD_ZONE) scoopMotorSpeed = 0; // Make sure that the joystick isn't inside dead zone
motor[armMotor] = armMotorSpeed; // Set the motor armMotor speed as armMotorSpeed
motor[leftMotor] = leftMotorSpeed; // Set the motor leftMotor speed as leftMotorSpeed
motor[rightMotor] = rightMotorSpeed; // Set the motor rightMotor speed as rightMotorSpeed
motor[scoopMotor] = scoopMotorSpeed;
if (joy1Btn(5) == 1) {
// Power up
topSpeed = MOTOR_POWER_UP_MAX;
}
if (joy1Btn(7) == 1) {
// Power down
topSpeed = MOTOR_POWER_DOWN_MAX;
}
if (joy2Btn(2) == 1) {
// Power up
armTopSpeed = ARM_MOTOR_POWER_DOWN;
}
if (joy1Btn(4) == 1) {
// Power up
armTopSpeed = ARM_MOTOR_POWER_UP;
}
if (joy1Btn(4) == 1) {
// Nudge forward
motor[leftMotor] = NUDGE_POWER;
motor[rightMotor] = NUDGE_POWER;
wait1Msec(NUDGE_DURATION);
motor[leftMotor] = 0;
motor[rightMotor] = 0;
wait1Msec(NUDGE_DELAY);
}
if (joy1Btn(2) == 1) {
// Nudge backward
motor[leftMotor] = NUDGE_POWER * -1;
motor[rightMotor] = NUDGE_POWER * -1;
wait1Msec(NUDGE_DURATION);
motor[leftMotor] = 0;
motor[rightMotor] = 0;
wait1Msec(NUDGE_DELAY);
}
if (joy1Btn(0) == 1) {
// Nudge left
motor[leftMotor] = 0;
motor[rightMotor] = NUDGE_POWER;
wait1Msec(NUDGE_DURATION);
motor[leftMotor] = 0;
motor[rightMotor] = 0;
wait1Msec(NUDGE_DELAY);
}
if (joy1Btn(3) == 1) {
// Nudge right
motor[leftMotor] = NUDGE_POWER;
motor[rightMotor] = 0;
wait1Msec(NUDGE_DURATION);
motor[leftMotor] = 0;
motor[rightMotor] = 0;
wait1Msec(NUDGE_DELAY);
}
totalMessages = ntotalMessageCount;
} else if (time1[T2] > 200) {
// We have not received a packet in over two-tenths of a
// second, which probably means communications have been
// lost.
// Put code to stop all drive motors to avoid
// damaging the robot here.
PlayImmediateTone(3000, 1); // play a warning tone
// Stop motors
motor[leftMotor] = 0;
motor[rightMotor] = 0;
}
}
}
}
void AtWaypoint()
{
PlayImmediateTone(500, 30);
wait1Msec(1000);
}
/*
* Follows gap until gap found
*/
void ForwardWallFollow(bool sonarFacingLeft)
{
writeDebugStreamLine("WALL_FOLLOW");
int var;
float sensorDist;
float prevSensorDist = -1;
int count = 0, count2 = 0, switchCount = 0;
int lspeed, rspeed;
bool switchL, switchR;
//int timeLeft = (int) (distance*FORWARD_CONST/(float)FORWARD_SPEED);
while (true)
{
sensorDist = SensorValue[sonarSensor];
if (sensorDist > SENSOR_MAX) continue; // Ignore error readings
if (switchL || switchR)
switchCount++;
if (prevSensorDist == sensorDist) {
count2++;
if (count2 > 25) {
if (!sonarFacingLeft && abs(lspeed) > FORWARD_SPEED) {
switchR = true;
switchL = false;
}
else if (sonarFacingLeft && abs(rspeed) > FORWARD_SPEED) {
switchR = false;
switchL = true;
}
count2 = 0;
}
}
else count2 = 0;
// If difference between current and previous sensor reading is large, we have found a gap
if (abs(prevSensorDist - sensorDist) > 15 && prevSensorDist != -1)
count++; // Possibly found gap
else {
count = 0;
prevSensorDist = sensorDist;
}
if (count > 7) // && abs(var) < 10)
break; // Almost definately found gap
if (switchCount > 25)
switchR = false;
switchL = false;
}
var = WALL_FOLLOW_K*(sensorDist - DESIRED_WALL_DIST);
if (sonarFacingLeft || switchL) {
lspeed = FORWARD_SPEED-var;
rspeed = FORWARD_SPEED+var;
}
else if (!sonarFacingLeft || switchR) {
lspeed = FORWARD_SPEED+var;
rspeed = FORWARD_SPEED-var;
}
setMotorSpeeds(lspeed, rspeed);
writeDebugStreamLine("MOTORS: %d %f", var, sensorDist);
//wait1Msec(1);
setMotorSpeeds(0, 0);
PlayImmediateTone(500, 30);
}
task main() {
nVolume = 3;
waitForStart();
while(true) {
bFloatDuringInactiveMotorPWM = false;
getJoystickSettings(joystick);
if((joystick.joy1_y1 < 10 && joystick.joy1_y1 > -10) && (joystick.joy1_x1 < 10 && joystick.joy1_x1 > -10))
{
joystick.joy1_y1 = 0;
joystick.joy1_x1 = 0;
}
if((joystick.joy1_x2 < 10 && joystick.joy1_x2 > -10) && (joystick.joy1_y2 < 10 && joystick.joy1_y2 > -10))
{
joystick.joy1_x2 = 0;
joystick.joy1_y2 = 0;
}
/* uncomment following section for RC Car steering */
/*if (!revDir) {
if (!sloMo) {
motor[motorLeft] = (joystick.joy1_y1 + joystick.joy1_x2);
motor[motorRight] = (joystick.joy1_y1 - joystick.joy1_x2);
} else {
motor[motorLeft] = 0.16 * (joystick.joy1_y1 + joystick.joy1_x2);
motor[motorRight] = 0.16 * (joystick.joy1_y1 - joystick.joy1_x2);
}
} else {
if (!sloMo) {
motor[motorLeft] = -(joystick.joy1_y1 + joystick.joy1_x2);
motor[motorRight] = -(joystick.joy1_y1 - joystick.joy1_x2);
} else {
motor[motorLeft] = -0.16 * (joystick.joy1_y1 + joystick.joy1_x2);
motor[motorRight] = -0.16 * (joystick.joy1_y1 - joystick.joy1_x2);
}
}*/
/* uncomment following section for tank steering */
if (!sloMo) {
motor[motorLeft] = (joystick.joy1_y1);
motor[motorRight] = (joystick.joy1_y2);
} else {
motor[motorLeft] = 0.16 * (joystick.joy1_y1);
motor[motorRight] = 0.16 * (joystick.joy1_y2);
}
if (canSloMo) {
if (!sloMo) {
if (joy1Btn(12)) {
sloMo = true;
canSloMo = false;
PlayImmediateTone(400, 10);
}
} else {
if (joy1Btn(12)) {
sloMo = false;
canSloMo = false;
PlayImmediateTone(600, 10);
}
}
}
if (!canSloMo) {
if (!joy1Btn(12)) {
canSloMo = true;
}
}
if (canRevDir) {
if (!revDir) {
if (joy1Btn(11)) {
revDir = true;
canRevDir = false;
PlayImmediateTone(300, 10);
}
} else {
if (joy1Btn(11)) {
revDir = false;
canRevDir = false;
PlayImmediateTone(800, 10);
}
}
}
if (!canRevDir) {
if (!joy1Btn(11)) {
canRevDir = true;
}
}
// ------------------------------- End Drive Code -------------------------------
if (joy2Btn(5)) {
motor[motorLift] = 100;
} else if (joy2Btn(7)) {
motor[motorLift] = -100;
} else {
motor[motorLift] = 0;
}
if (joystick.joy2_TopHat == 0)
motor[motorBelt] = 100;
else if (joystick.joy2_TopHat == 4)
motor[motorBelt] = -100;
else
motor[motorBelt] = 0;
if (joy2Btn(6)) {
servo[srvoF1] = srvoVal[0];
servo[srvoF2] = srvoVal[0];
}
if (joy2Btn(8)) {
servo[srvoF1] = srvoVal[1];
servo[srvoF2] = srvoVal[1];
}
} //while true
} //task main
/*
void chamberAdjust()
{
//Sonar look left + take reading.
nMotorEncoder[motorC] = 0;
motor[motorC] = 5;
while(nMotorEncoder[motorC] < 90)
;
motor[motorC] = 0;
int leftReading = SensorValue(sonar);
nxtDisplayCenteredTextLine(1, "LR: %d", leftReading);
//Sonar look right + take reading.
motor[motorC] = -5;
while(nMotorEncoder[motorC] > -90)
;
motor[motorC] = 0;
int rightReading = SensorValue(sonar);
nxtDisplayCenteredTextLine(2, "RR: %d", rightReading);
wait1Msec(5000);
//Recenter sonar
motor[motorC] = 5;
while(nMotorEncoder[motorC] < 0)
;
motor[motorC] = 0;
//Adjust the position of the robot so that the left reading = right reading.
if(leftReading > rightReading)
{
turnRadiansClockwise(PI/2);
nSyncedMotors = synchAB;
nSyncedTurnRatio = 100;
motor[motorA] = 5;
while( SensorValue(sonar) > 21 - sonarOffset)
;
motor[motorA] = 0;
turnRadiansClockwise(-PI/2);
}
else if(leftReading < rightReading)
{
turnRadiansClockwise(-PI/2);
nSyncedMotors = synchAB;
nSyncedTurnRatio = 100;
motor[motorA] = 5;
while( SensorValue(sonar) > 21 - sonarOffset)
;
motor[motorA] = 0;
turnRadiansClockwise(PI/2);
}
}
*/
void beep()
{
//beep when done.
PlayImmediateTone(780, 100);
}
task main() {
waitForStart();
while(true) {
bFloatDuringInactiveMotorPWM = false;
getJoystickSettings(joystick);
if((joystick.joy1_y1 < 10 && joystick.joy1_y1 > -10) && (joystick.joy1_x1 < 10 && joystick.joy1_x1 > -10))
{
joystick.joy1_y1 = 0;
joystick.joy1_x1 = 0;
}
if((joystick.joy1_x2 < 10 && joystick.joy1_x2 > -10) && (joystick.joy1_y2 < 10 && joystick.joy1_y2 > -10))
{
joystick.joy1_x2 = 0;
joystick.joy1_y2 = 0;
}
if (!revDir) {
if (!sloMo) {
motor[motorLeft] = (joystick.joy1_y1 + joystick.joy1_x2);
motor[motorRight] = (joystick.joy1_y1 - joystick.joy1_x2);
} else {
motor[motorLeft] = 0.16 * (joystick.joy1_y1 + joystick.joy1_x2);
motor[motorRight] = 0.16 * (joystick.joy1_y1 - joystick.joy1_x2);
}
} else {
if (!sloMo) {
motor[motorLeft] = -(joystick.joy1_y1 + joystick.joy1_x2);
motor[motorRight] = -(joystick.joy1_y1 - joystick.joy1_x2);
} else {
motor[motorLeft] = -0.16 * (joystick.joy1_y1 + joystick.joy1_x2);
motor[motorRight] = -0.16 * (joystick.joy1_y1 - joystick.joy1_x2);
}
}
if (canSloMo) {
if (!sloMo) {
if (joy1Btn(12)) {
sloMo = true;
canSloMo = false;
PlayImmediateTone(400, 10);
}
} else {
if (joy1Btn(12)) {
sloMo = false;
canSloMo = false;
PlayImmediateTone(600, 10);
}
}
}
if (!canSloMo) {
if (!joy1Btn(12)) {
canSloMo = true;
}
}
if (canRevDir) {
if (!revDir) {
if (joy1Btn(11)) {
revDir = true;
canRevDir = false;
PlayImmediateTone(300, 10);
}
} else {
if (joy1Btn(11)) {
revDir = false;
canRevDir = false;
PlayImmediateTone(800, 10);
}
}
}
if (!canRevDir) {
if (!joy1Btn(11)) {
canRevDir = true;
}
}
// ------------------------------- End Drive Code -------------------------------
if (joy2Btn(1)) {
motor[motorLift] = -100;
} else if (joy2Btn(4)) {
motor[motorLift] = 100;
} else {
motor[motorLift] = 0;
}
if (joy2Btn(2))
motor[motorBelt] = -100;
else if (joy2Btn(3))
motor[motorBelt] = 100;
else
motor[motorBelt] = 0;
if (joy2Btn(6)) {
servo[srvoF1] = srvoVal[0];
servo[srvoF2] = srvoVal[0];
}
if (joy2Btn(8)) {
servo[srvoF1] = srvoVal[1];
servo[srvoF2] = srvoVal[1];
}
} //while true
} //task main
task main()
{
initializeRobot();
waitForStart(); // wait for start of tele-op phase
while (true)
{
updateControllers();
//========== handle drive ==========
calculateDrive(joystick.joy1_x1, joystick.joy1_y1);
if (ControllerButtonPressed(BUTTON_LB, CONTROLLER_1)) {
driveSetPower(30);
} else if (ControllerButtonReleased(BUTTON_LB, CONTROLLER_1)) {
driveSetPower(100);
}
//========== handle lift ==========
if (ControllerButtonDown(BUTTON_X, CONTROLLER_1)) {
liftBottomPositionSafety();
} else if (ControllerButtonDown(BUTTON_Y, CONTROLLER_1)) {
liftDumpPositionSafety();
} else if (ControllerButtonDown(BUTTON_RSTICK, CONTROLLER_1)) {
liftHangPositionSafety();
} else {
liftUpSpeed(joystick.joy1_y2);
}
//========== handle bucket ==========
if (ControllerButtonPressed(BUTTON_RB, CONTROLLER_1)) {
bucketIncrementState();
} else if (ControllerButtonPressed(BUTTON_RT, CONTROLLER_1)) {
bucketDecrementState();
}
//========== handle spinner ==========
if (ControllerButtonPressed(BUTTON_START, CONTROLLER_1)) {
spinnerOut();
} else if (ControllerButtonPressed(BUTTON_BACK, CONTROLLER_1)) {
spinnerIn();
}
if (ControllerButtonPressed(BUTTON_A, CONTROLLER_1)) {
spinnerSpinCClockwise();
} else if (ControllerButtonReleased(BUTTON_A, CONTROLLER_1)) {
spinnerStop();
}
if (ControllerButtonPressed(BUTTON_B, CONTROLLER_1)) {
spinnerAlignCClockwise();
} else if (ControllerButtonReleased(BUTTON_B, CONTROLLER_1)) {
spinnerStop();
}
//========== handle PTO ==========
if (ControllerHatPressed(BUTTON_HAT_LEFT, CONTROLLER_1)) {
hangingPTOEngage();
}
if (ControllerHatPressed(BUTTON_HAT_RIGHT, CONTROLLER_1)) {
hangingPTODisengage();
}
if (ControllerButtonPressed(BUTTON_LT, CONTROLLER_1)) {
hangingPTOEngage();
} else if (ControllerButtonReleased(BUTTON_LT, CONTROLLER_1)) {
hangingPTODisengage();
}
//========= handle Ratchet ==========
if (ControllerHatPressed(BUTTON_HAT_UP, CONTROLLER_1)) {
hangingRatchetLock();
}
if (ControllerHatPressed(BUTTON_HAT_DOWN, CONTROLLER_1)) {
hangingRatchetUnlock();
}
//========== debug beep ==========
if (ControllerButtonDown(BUTTON_LSTICK, CONTROLLER_1)) {
PlayImmediateTone(4186, 1);
}
//========== EMERGENCY BACKUP FUNCTIONS!!! ==========
//dump block in flipper
if (ControllerButtonPressed(BUTTON_B, CONTROLLER_2)) {
flipperDump();
} else if (ControllerButtonReleased(BUTTON_B, CONTROLLER_2)) {
flipperStorage();
}
//trim bucket positions
if (ControllerButtonPressed(BUTTON_RB, CONTROLLER_2)) {
bucketTrimUp();
} else if (ControllerButtonPressed(BUTTON_RT, CONTROLLER_2)) {
bucketTrimDown();
}
}
}
