task autonomous()
{
StartTask(descore);
StartTask(velocitycalculator);
if(SensorValue[J1]) // blue
{
if(SensorValue[J2]) //normal
{
// RAM
ArmWall();
ForwardTillStop(127);
}
else // oppo
{
Gamma();
}
}
else // red
{
if(SensorValue[J2]) // normal
{
Beta();
}
else // oppo
{
Delta();
}
}
StopTask(velocitycalculator);
}
void RedIsolationPickupAutonomous()
{
SensorValue[Gyroscope] = 0;
StartTask(KeepArmInPosition);
armPosition = minArm;
Intake(127);
Forward(45, 125, "none");
StopMoving();
Backward(127, 90, "none");
Intake(0);
StopMoving();
TurnLeftDegrees(40);
armPosition = midGoalHeight;
wait10Msec(70);
Intake(127);
Forward(100, 60, "none");
StopMoving();
armPosition = midGoalHeight - 100;
wait10Msec(50);
StopTask(KeepArmInPosition);
StopArm();
StartTask(StayInPosition);
wait10Msec(500);
Intake(0);
wait10Msec(1000);
StopTask(StayInPosition);
}
//THIS IS THE BEST CODE THAT HAS BEEN WRITTEN
task main()
{
bool isInMenu = true;
float distance = 0;
while(isInMenu){
nxtDisplayCenteredTextLine(3, "Distance: %.1f", distance);
if(nNxtButtonPressed == 1){
distance+=0.5;
}
else if(nNxtButtonPressed == 2){
distance-=0.5;
}
else if(nNxtButtonPressed == 3){
nxtDisplayCenteredBigTextLine(3, "LAUNCH");
isInMenu = false;
}
wait1Msec(250);
}
nMotorEncoder[drive] = 0;
StartTask(Display);
target = distance * conv;
wait1Msec(100);
StopTask(P);
wait10Msec(100);
motor[drive] = 0;
StopTask(Display);
}
task main()
{
waitForStart();
init();
StartTask(moveElevatorUpFull);
wait1Msec(time_elevator1_up);
turn(-TURN_1);
move(MOVE_1);
turn(-TURN_2);
move(MOVE_2);
gotoBox();
//start going to the ramp
turn(-TURN_3);
int startTime = nPgmTime;
StartTask(moveElevatorDownFull);
move(MOVE_3);
turn(-TURN_4);
move(MOVE_4 + MOVE_OVER_5);
wait1Msec(time_elevator1_down - (nPgmTime - startTime));
StopTask(moveElevatorDownFull);
StopTask(moveElevatorUpFull);
}
task main()
{
//waitForStart(); // Wait for the beginning of autonomous phase.
min = SensorRaw[frontLight];
StartTask(UpdateMax);
while(SensorRaw[frontLight] <= min + 50){
motor[fr] = 10;
motor[fl] = 15;
motor[br] = 10;
motor[bl] = 15;
}
motor[fr] = 0;
motor[fl] = 0;
motor[br] = 0;
motor[bl] = 0;
StopTask(UpdateMax);
StartTask(followline);
while(HTIRS2readACDir(frontRightIR) != 5 && HTIRS2readACDir(backRightIR) != 9){
}
StopTask(followline);
irturn();
liftUpAndDownAndTakeDumpAndForward();
getbackontape();
while (true)
{}
}
void doLineFollow() {
//int LightSensors[6]; 0 is far left, 4 is far right, 5 is middle
while(!hitTarget())
{
if(OnLine(LightSensors[2])
{
if(!goingForward)
{
if(correcting)
{
StopTask(correctLine);
correcting = false;
}
StartTask(MoveForward);
goingForward = true;
}
}
else
{
if(!correcting)
{
if(goingForward)
{
StopTask(MoveForward);
goingForward = false;
}
StartTask(correctLine);
correcting = true;
}
}
}
void Scoremiddlegoal()
{ waitForButton();
starttask(CollectD);
{
drive_Enc(63,7,4);
}
stopdrivemotors();
StopTask(CollectD);
StartTask(medgoal);
{
stopTime(1.75);//wait for arm
drive_Enc(63,8,5);//drive foward
}
StopTask(medgoal);
spit(0.75);
drive_Enc(-63,4,2);
spit(0.75);
StartTask(armDown);
{
drive_Enc(-63,15,5);
}
StopTask(armDown);
waitForButton();
StartTask(CollectD);
{
drive_Enc(70,15,5);
}
StopTask(CollectD);
}
void arrival()
{
int now=sec;
StopTask(read);
StopTask(seek);
int ang1=compass[now];
int ir1=ir[now];
int sonar1=sonar[now];
}
task main()
{
initializeRobot();
bFloatDuringInactiveMotorPWM = false;
nxtDisplayTextLine(2, "Waiting for start");
waitForStart(); // wait for start of tele-op phase
nxtDisplayTextLine(2, "starting");
StartTask( drive );
StartTask( Arm_task );
StartTask( Score_task );
float watchedmessage;
//int watch = 1;
while(true)
{
watchedmessage = ntotalMessageCount; // grabs the number of Messages and names it watchedMessage
wait10Msec(25); // give the Messagecount time to change it changes every 50ms
if(watchedmessage == ntotalMessageCount) // checks to see if the Message change if has not it is Disconnected
{ // if Disconnected it runs this
motor[mtr_S1_C1_1] = 0; // stops motor
motor[mtr_S1_C1_2] = 0;
motor[mtr_S1_C2_1] = 0;
motor[mtr_S1_C2_2] = 0;
motor[mtr_S1_C3_1] = 0;
motor[mtr_S1_C3_2] = 0;
motor[mtr_S1_C4_1] = 0;
motor[mtr_S1_C4_2] = 0;
StopTask(Arm_task); //stops task robot control task
StopTask(drive);
StopTask(Score_task);
wait1Msec(10); //gives time to spot
StartTask(Arm_task); //restart task incase of reconnection
StartTask(drive);
StartTask(Score_task);
nxtDisplayTextLine(2, "DISCONNECTED"); //displays DISCONNECTED to tell you if you are Disconnected
}
else//if the Message change you are connected
{
nxtDisplayTextLine(2, "CONNECTED");//displays CONNECTED to tell you if everything is working
}
}
}
void stopCurrentLightTask(int oldT)
{
switch(oldT){
case 1:
StopTask(approach_light);
break;
case 0:
StopTask(seek);
break;
}
}
/* Map shooter to secondary joystick */
void joyMapPartner() {
//Manually edit shooter speed with auditory feedback
if(joyPartner.rx > 120) {
shooter+=2;
StopTask(soundSpeedUp);
StartTask(soundSpeedUp);
wait10Msec(10);
} else if(joyPartner.rx < -120) {
shooter-=2;
StopTask(soundSpeedDown);
startTask(soundSpeedDown);
wait10Msec(10);
} //pac-man af
}
task main()
{
// waitForStart(); // wait for start of tele-op phase
// StartTask( drive );
StartTask( Arm );
//Allow display control
StopTask(displayDiagnostics);
eraseDisplay();
float watchedmessage;
//int watch = 1;
while(true)
{
watchedmessage = ntotalMessageCount; // grabs the number of Messages and names it watchedMessage
wait10Msec(25); // give the Messagecount time to change it changes every 50ms
if(watchedmessage == ntotalMessageCount) // checks to see if the Message change if has not it is Disconnected
{ // if Disconnected it runs this
motor[mtr_S1_C1_1] = 0; // stops motor
motor[mtr_S1_C1_2] = 0;
motor[mtr_S1_C2_1] = 0;
motor[mtr_S1_C2_2] = 0;
motor[mtr_S1_C3_1] = 0;
motor[mtr_S1_C3_2] = 0;
//StopTask(main); //stops task robot control task
//StopTask(drive);
StopTask(Arm);
wait1Msec(10); //gives time to spot
//StartTask(main); //restart task incase of reconnection
StartTask(drive);
StartTask(Arm);
nxtDisplayTextLine(2, "DISCONNECTED"); //displays DISCONNECTED to tell you if you are Disconnected
}
else//if the Message change you are connected
{
nxtDisplayTextLine(2, "CONNECTED"); //displays CONNECTED to tell you if everything is working
}
}
}
int FollowSegmentTilEnd(Edge &edge) {
StartTask(FollowEdge);
int forwards = gatherForwardsData();
edge.angle = forwards;
int found = FOUND_ERROR;
while(found == FOUND_ERROR) {
if(detectCan()) {
found = FOUND_CAN;
}
if(detectNode(forwards)) {
found = FOUND_NODE;
}
//abortTimeslice();
}
followingEdge = false;
StopTask(FollowEdge);
edge.length = sweeps;
faceForward(forwards);
moveToSensor();
return found;
}
void CDialogCut::TimeGetProgress()
{
if (m_nPort != -1)
{
int progress = Player_FileConvertProcess(m_nPort);
ui.progressBar->setValue(progress);
if (progress == 100)
{
StopTask();
bool bRet = m_PlayCtrlTest->OpenAndPlayFile(m_strDstPathName, NULL);
if (bRet)
{
m_PlayCtrlTest->StopAudio();
ui.labelState->setText(QStringLiteral("成功"));
}
else
{
ui.progressBar->setValue(0);
ui.labelState->setText(QStringLiteral("失败(此文件不支持)"));
}
m_PlayCtrlTest->Stop();
}
/*
else if (progress == -1)//出错
{
StopTask();
ui.labelState->setText(QStringLiteral("出错！"));
}*/
}
}
task JohnCena(){
// 130 = Tempo
// 6 = Default octave
// Quarter = Default note length
// 10% = Break between notes
//
PlayTone( 0, 21); wait1Msec( 231); // Note(Rest, Duration(Eighth))
PlayTone( 1175, 42); wait1Msec( 462); // Note(G)
PlayTone( 1320, 21); wait1Msec( 231); // Note(A, Duration(Eighth))
PlayTone( 1047, 42); wait1Msec( 462); // Note(F)
PlayTone( 1175, 83); wait1Msec( 923); // Note(G, Duration(Half))
PlayTone( 0, 21); wait1Msec( 231); // Note(Rest, Duration(Eighth))
PlayTone( 1398, 42); wait1Msec( 462); // Note(A#)
PlayTone( 1320, 21); wait1Msec( 231); // Note(A, Duration(Eighth))
PlayTone( 1047, 42); wait1Msec( 462); // Note(F)
PlayTone( 1175, 83); wait1Msec( 923); // Note(G, Duration(Half))
PlayTone( 0, 21); wait1Msec( 231); // Note(Rest, Duration(Eighth))
PlayTone( 1175, 42); wait1Msec( 462); // Note(G)
PlayTone( 1320, 21); wait1Msec( 231); // Note(A, Duration(Eighth))
PlayTone( 1047, 42); wait1Msec( 462); // Note(F)
PlayTone( 1175, 83); wait1Msec( 923); // Note(G, Duration(Half))
PlayTone( 0, 21); wait1Msec( 231); // Note(Rest, Duration(Eighth))
PlayTone( 1398, 42); wait1Msec( 462); // Note(A#)
PlayTone( 1320, 21); wait1Msec( 231); // Note(A, Duration(Eighth))
PlayTone( 1047, 42); wait1Msec( 462); // Note(F)
PlayTone( 1175, 83); wait1Msec( 923); // Note(G, Duration(Half))
StopTask(JohnCena);
}
task PinkPanther()
{
// 160 = Tempo
// 5 = Default octave
// Quarter = Default note length
// 10% = Break between notes
//
PlayTone( 622, 17); wait1Msec( 188); // Note(D#, Duration(Eighth))
PlayTone( 659, 17); wait1Msec( 188); // Note(E, Duration(Eighth))
PlayTone( 0, 68); wait1Msec( 750); // Note(Rest, Duration(Half))
PlayTone( 739, 17); wait1Msec( 188); // Note(F#, Duration(Eighth))
PlayTone( 783, 17); wait1Msec( 188); // Note(G, Duration(Eighth))
PlayTone( 0, 68); wait1Msec( 750); // Note(Rest, Duration(Half))
PlayTone( 622, 17); wait1Msec( 188); // Note(D#, Duration(Eighth))
PlayTone( 659, 17); wait1Msec( 188); // Note(E, Duration(Eighth))
PlayTone( 0, 8); wait1Msec( 94); // Note(Rest, Duration(16th))
PlayTone( 739, 17); wait1Msec( 188); // Note(F#, Duration(Eighth))
PlayTone( 783, 17); wait1Msec( 188); // Note(G, Duration(Eighth))
PlayTone( 0, 8); wait1Msec( 94); // Note(Rest, Duration(16th))
PlayTone( 1046, 17); wait1Msec( 188); // Note(C7, Duration(Eighth))
PlayTone( 987, 17); wait1Msec( 188); // Note(B, Duration(Eighth))
PlayTone( 0, 8); wait1Msec( 94); // Note(Rest, Duration(16th))
PlayTone( 622, 17); wait1Msec( 188); // Note(D#, Duration(Eighth))
PlayTone( 659, 17); wait1Msec( 188); // Note(E, Duration(Eighth))
PlayTone( 0, 8); wait1Msec( 94); // Note(Rest, Duration(16th))
PlayTone( 987, 17); wait1Msec( 188); // Note(B, Duration(Eighth))
PlayTone( 932, 68); wait1Msec( 750); // Note(A#, Duration(Half))
PlayTone( 0, 68); wait1Msec( 750); // Note(Rest, Duration(Half))
PlayTone( 880, 8); wait1Msec( 94); // Note(A, Duration(16th))
PlayTone( 783, 8); wait1Msec( 94); // Note(G, Duration(16th))
PlayTone( 659, 8); wait1Msec( 94); // Note(E, Duration(16th))
PlayTone( 587, 8); wait1Msec( 94); // Note(D, Duration(16th))
PlayTone( 659, 68); wait1Msec( 750); // Note(E, Duration(Half))
StopTask(PinkPanther);
}
void initSweg() {
//Stop moving.
motor[left] = 0;
motor[right] = 0;
//Stand up.
servo[rearFlipper] = REAR_LIFT_DEPRESSED;
servo[frontFlipper] = FRONT_LIFT_DEPRESSED;
//Wait for it to stand all the way up.
wait1Msec(2700);
//Recalibrate the gyro.
StopTask(gyros);
PlaySound(soundBeepBeep);//"Starting calibration!"
HTGYROstartCal(forwardsTilt);
wait1Msec(1000);//Give it time to calibrate
PlaySound(soundBeepBeep);//"Done calibrating!"
//K, finished with gyro calibration. Start measuring angle now.
StartTask(gyros);
//Not sure what these two lines are for.
nMotorEncoder[left] = 0;
nMotorEncoder[right] = 0;
}
void CPingTestDoc::OnCloseDocument()
{
// TODO: 在此添加专用代码和/或调用基类
StopTask();
CDocument::OnCloseDocument();
}
task main()
{
initializeRobot(); //Executes all code in the initializeRobot() function
// waitForStart(); //Wait for the beginning of autonomous phase
while(true)
{
nxtDisplayBigTextLine(3, "IR_position: %4d", IR_position);
if (abs(IR_position - pos1) < 4)
{
IR_Beacon = 1;
}
else if (abs(IR_position - pos2) < 4)
{
IR_Beacon = 2;
}
else if (abs(IR_position - pos3) < 4)
{
IR_Beacon = 3;
}
else if (abs(IR_position - pos4) < 4)
{
IR_Beacon = 4;
}
nxtDisplayBigTextLine(4, "IR_Beacon: %4d", IR_Beacon);
}
auto_waiting = false;
StopTask(Find_IR);
}
task usercontrol()
{
//Reset the encoders
StopTask(autonomous);
nMotorEncoder[btlftarm]=0;
bLCDBacklight = true;
while (true)
{
gyroo = gyroValue();
driveOpControlComp();
armControlOpComp();
intakeControlOpComp();
//Try to auto-descore height with roller already in
if (vexRT[Btn8D]==1&&vexRT[Btn6D]==0&&vexRT[Btn6U]==0)
{
savedPosition = true;
position = 250;
SensorValue[lftSolenoid]=1;
SensorValue[rtSolenoid]=1;
motor[intk]=127;
motor[intk2]=127;
}
//User-reset the motor encoders
if (vexRT[Btn8R]==1){
nMotorEncoder[btlftarm]=0;
}
}
}
//////////////////////////////////////////////////////////////////////////////////////
// //
// Autony Mouses //
// //
//////////////////////////////////////////////////////////////////////////////////////
void InteractionInterceptionAutonomous()
{
SensorValue[Gyroscope] = 0;
Intake(127);
Backward(127, 90, "none");
StopMoving();
Intake(0);
StartTask(StayInPosition);
wait10Msec(1500);
StopTask(StayInPosition);
Backward(127, 25, "none");
StopMoving();
StartTask(StayInPosition);
wait10Msec(500);
StopTask(StayInPosition);
}
task shoot(){
/*
PlaySound(soundBeepBeep);
wait1Msec(200);
StopTask(shoot);
*/
int r=motor[motorB];
int l=motor[motorC];
motor[motorB]=0;
motor[motorC]=0;
////
nMotorEncoder[motorA] = 0;
nMotorEncoderTarget[motorA] = 360;
motor[motorA] = 100;
while (nMotorRunState[motorA] != runStateIdle) //while the encoder wheel turns one revolution
{
// This condition waits for motors B + C to come to an idle position. Both motors stop
// and then jumps out of the loop
}
motor[motorA] = 0;
//////
motor[motorB]=r;
motor[motorC]=l;
StopTask(shoot);
}
CDTManager::~CDTManager(void)
{
POSITION p=_tks.GetStartPosition();
while(p)
{
StopTask(_tks.GetNextKey(p));
}
}
task main() {
pidSetup();
StartTask(checkObstacles);
wait1Msec(500);
while(true){
wait1Msec(400);
if(handleObstacleLeft){
StopTask(obstacleHandler);
StartTask(obstacleHandler);
} else if(handleObstacleRight){
StopTask(obstacleHandler);
StartTask(obstacleHandler);
} else if(!passingByObstacle){
moveToTarget();
}
}
}
task main() {
clearDebugStream();
nMotorEncoder[LEFT_WHEEL] = 0;
nMotorEncoder[RIGHT_WHEEL] = 0;
bPlaySounds = true;
set_starting_position(84.0, 30.0, 0.0);
drawMap();
StartTask(vehicle_compute_position);
//StartTask(vehicle_draw_position);
navigate_to_waypoint(104, 30);
drawParticles();
navigate_to_waypoint(124, 30);
drawParticles();
navigate_to_waypoint(144, 30);
drawParticles();
navigate_to_waypoint(180, 30);
drawParticles();
navigate_to_waypoint(180, 54);
drawParticles();
navigate_to_waypoint(164, 54);
drawParticles();
navigate_to_waypoint(126, 54);
drawParticles();
// Moved left
// Going up
navigate_to_waypoint(126, 74);
drawParticles();
navigate_to_waypoint(126, 94);
drawParticles();
navigate_to_waypoint(126, 104);
drawParticles();
navigate_to_waypoint(126, 124);
drawParticles();
navigate_to_waypoint(126, 144);
drawParticles();
navigate_to_waypoint(126, 168);
drawParticles();
navigate_to_waypoint(126, 148);
drawParticles();
navigate_to_waypoint(126, 126);
drawParticles();
// From here, move in 20 cm ranges
navigate_to_waypoint(30, 54);
drawParticles();
navigate_to_waypoint(84, 54);
drawParticles();
navigate_to_waypoint(84, 30);
drawParticles();
StopTask(vehicle_compute_position);
//StopTask(vehicle_draw_position);
wait10Msec(60000); // wait 1MIN
}
/* Runs a motor 0.5sec in each direction to make sure it's working */
void pulseMotor(tMotor motor) {
StopTask(usercontrol);
motorSet(motor,127);
wait1Msec(250);
motorSet(motor,-127);
wait1Msec(250);
motorSet(motor);
StartTask(usercontrol);
}
void InteractionAntiWallBot1Autonomous()
{
SensorValue[Gyroscope] = 0;
Forward(127, 140, "none");
StopMoving();
StartTask(StayInPosition);
wait10Msec(2000);
StopTask(StayInPosition);
}
void BlockMiddleGoal()
{
StartTask(medgoal);
{
drivetime(80,1.2)
}
StopTask(medgoal);
spit(20);
}
void
CLogSvcClient::Close( void )
{GUCEF_TRACE;
if ( m_tcpClient.IsActive() )
{
StopTask();
m_connectionInitialized = false;
m_tcpClient.Close();
}
}
void InteractionGoalAutonomous()
{
SensorValue[Gyroscope] = 0;
StartTask(KeepArmInPosition);
Intake(127);
armPosition = maxArm;
wait10Msec(50);
Intake(0);
Forward(65, 70, "none");
StopTask(KeepArmInPosition);
}
