void autoRamp(bool useLift) { travelDistance(200, dBackward); gyroTurn(30, 15, dRight); goalRelease(); wait1Msec(100); backward(15); wait1Msec(1500); stopMotors(); int iCRate = servoChangeRate[goalCapture]; // Save change rate servoChangeRate[goalCapture] = 0; // Max Speed servo[goalCapture] = CATCHDOWN; // Set servo position wait1Msec(50); servoChangeRate[goalCapture] = iCRate; // Reset the servo wait1Msec(100); travelDistance(10, dBackward); strafe(75); wait1Msec(500); stopMotors(); travelDistance(100, dForward); gyroTurn(30, 2, dLeft); travelDistance(150, dForward); gyroTurn(20, 100, dRight); goalMed(useLift); }
void forward( int rotations, int power ){ resetEncoders(); //Resets the motor encoder readings. while( (nMotorEncoder[leftWheel1] < rotations) && (nMotorEncoder[rightWheel1] < rotations) && (nMotorEncoder[leftWheel2]) < rotations && (nMotorEncoder[rightWheel2] < rotations)){ showStatus(rotations, power); motor[leftWheel1] = power; motor[rightWheel1] = power; motor[leftWheel2] = power; motor[rightWheel2] = power; }//goes forward until one of the two sides has rotated enough stopMotors(); if( ( nMotorEncoder[leftWheel1] + nMotorEncoder[leftWheel2] ) - ( nMotorEncoder[rightWheel1] + nMotorEncoder[rightWheel2] ) > 100){ //if shifted to face left while( nMotorEncoder[leftWheel1] - nMotorEncoder[rightWheel1] > 0 ){ motor[rightWheel1] = power / 5;//turns to be straight } stopMotors(); resetEncoders(); } if( ( nMotorEncoder[rightWheel1] + nMotorEncoder[rightWheel2] ) - ( nMotorEncoder[leftWheel1] + nMotorEncoder[leftWheel2] ) > 100 ){ //if shifted to face right while( nMotorEncoder[rightWheel1] - nMotorEncoder[leftWheel1] > 0 ){ motor[leftWheel1] = power/5;//turns to be straight } } stopMotors(); resetEncoders(); }//forward function
void followLeftWall() { /* Invoked when robot is driven and limit switches are activated. ** If the limit switches are activated then the robot is traveling ** in a direction that is somewhat close to being parrallel of the wall. ** We should correct our direction (to avoid a crash) and oscillate on the wall. */ writeDebugStreamLine("In followLeftWall"); while(SensorValue(frontRightBumper) == 0) { driveMotors(-127,127); wait1Msec(1000); writeDebugStreamLine("In the first while loop of followLeftWall"); while ((SensorValue(leftLimitSensor) == 0) && (SensorValue(frontRightBumper) == 0)) { writeDebugStreamLine("LEFT LIMIT SENSOR IS 0"); driveMotors(-127,127); wait1Msec(200); } while ((SensorValue(leftLimitSensor) == 1) && (SensorValue(frontRightBumper) == 0) ) { writeDebugStreamLine("LEFT LIMIT SENSOR IS 1"); stopMotors(300); turnRight(1200); driveMotors(-127,127); wait1Msec(2000); stopMotors(300); turnLeft(1000); } driveMotors(-70,70); wait1Msec(100); } }
task main() { gyroLeftTurn(90, 50); stopMotors(); wait1Msec(1000); gyroRightTurn(90, 50); stopMotors(); }
void devicePwmMotorHandleRawData(char commandHeader, InputStream* inputStream, OutputStream* outputStream) { if (commandHeader == COMMAND_MOVE_MOTOR) { signed int left = readSignedHex2(inputStream); signed int right = readSignedHex2(inputStream); ackCommand(outputStream, MOTOR_DEVICE_HEADER, COMMAND_MOVE_MOTOR); setMotorSpeeds(left * 2, right * 2); } else if (commandHeader == COMMAND_READ_MOTOR_VALUE) { ackCommand(outputStream, MOTOR_DEVICE_HEADER, COMMAND_READ_MOTOR_VALUE); signed int left = getLeftSpeed(); signed int right = getRightSpeed(); appendHex2(outputStream, left / 2); appendHex2(outputStream, right / 2); } else if (commandHeader == COMMAND_STOP_MOTOR) { ackCommand(outputStream, MOTOR_DEVICE_HEADER, COMMAND_STOP_MOTOR); stopMotors(); } else if (commandHeader == COMMAND_TEST_MOTOR) { ackCommand(outputStream, MOTOR_DEVICE_HEADER, COMMAND_TEST_MOTOR); appendString(getAlwaysOutputStreamLogger(), "Left Forward\n"); // Left forward setMotorSpeeds(50, 0); delaymSec(2000); appendString(getAlwaysOutputStreamLogger(), "Right Forward\n"); // Right forward setMotorSpeeds(0, 50); delaymSec(2000); appendString(getAlwaysOutputStreamLogger(), "Left Backward\n"); // Left backward setMotorSpeeds(-50, 0); delaymSec(2000); appendString(getAlwaysOutputStreamLogger(), "Right Forward\n"); // Right backward setMotorSpeeds(0, -50); delaymSec(2000); appendString(getAlwaysOutputStreamLogger(), "Both Forward\n"); // Both forward setMotorSpeeds(50, 50); delaymSec(2000); appendString(getAlwaysOutputStreamLogger(), "Both Backward\n"); // Both backward setMotorSpeeds(-50, -50); delaymSec(2000); stopMotors(); } }
task main() { waitForStart(); stopMotors(); wait10Msec(500); moveForward(2, 80); wait10Msec(50); rightTwoWheelTurn(10, 50); wait10Msec(40); //robot stops at first bucket from the right side of the pendulum stopMotors(); wait10Msec(100); /* motor[tiltingMotor] = 75; wait10Msec(105); motor[tiltingMotor] = 25; wait10Msec(5); motor[tiltingMotor] = 0; wait10Msec(10); motor[conveyorMotor] = 100; wait10Msec(200); motor[conveyorMotor] = 0; wait10Msec(50); motor[tiltingMotor] = -45; wait10Msec(110); motor[tiltingMotor] = -25; wait10Msec(5); motor[tiltingMotor] = 0; wait10Msec(10);*/ armUp(); wait10Msec(200); conveyorForward(); wait10Msec(200); conveyorStop(); wait10Msec(100); armDown(); wait10Msec(200); moveForward(3, 80); wait10Msec(50); rightTwoWheelTurn(50, 50); wait10Msec(83); moveForward(25, 80); wait10Msec(50); leftTwoWheelTurn(48, 50); wait10Msec(58); moveForward(29, 80); wait10Msec(50); rightTwoWheelTurn(53, 50); wait10Msec(150); moveBackward(43, 80); wait10Msec(60); //robot parked in the middle of the ramp }
task main() { waitForStart(); stopMotors(); wait10Msec(500); moveForward(0.25, 80); wait10Msec(50); leftTwoWheelTurn(45, 50); wait10Msec(56); moveForward(41.5, 80); wait10Msec(50); rightTwoWheelTurn(45, 50); wait10Msec(135); //robot stopped in the third bucket from the right side of the pendulum stopMotors(); wait10Msec(100); motor[tiltingMotor] = 75; wait10Msec(98); motor[tiltingMotor] = 25; wait10Msec(5); motor[tiltingMotor] = 0; wait10Msec(10); motor[conveyorMotor] = 100; wait10Msec(200); motor[conveyorMotor] = 0; wait10Msec(50); motor[tiltingMotor] = -35; wait10Msec(105); motor[tiltingMotor] = -25; wait10Msec(5); motor[tiltingMotor] = 0; wait10Msec(10); leftTwoWheelTurn(42, 50); wait10Msec(127); moveForward(16, 80); wait10Msec(50); rightTwoWheelTurn(48, 50); wait10Msec(53.5); moveForward(25, 80); wait10Msec(50); rightTwoWheelTurn(48, 50); wait10Msec(58); moveForward(36, 80); wait10Msec(50); leftTwoWheelTurn(53, 50); wait10Msec(115); moveBackward(49, 80); wait10Msec(50); //robot parked in the middle of the ramp }
void strafeRight(float tileDist) { /* Moves Left X tiles 1 tile = 24in 15.7 = 1 Rotation (inches traveled) 24 / 15.7 = 1.528 = Rotation per tile 1 rotation = 240.448 counts 367.563 = Counts per tile */ float countGoal = tileDist * 367.563; float currentCount = 0; //Motors at full speed writeDebugStreamLine("Right Strafe Started"); nMotorEncoder[backLeft] = 0; //Reset encoder count currentCount = abs(nMotorEncoder[backLeft]); motor[frontRight] = -127; motor[frontLeft] = -127; motor[backRight] = 127; motor[backLeft] = 127; while(currentCount < countGoal){ currentCount = abs(nMotorEncoder[backLeft]); } stopMotors(); writeDebugStreamLine("Strafe Right Ended"); }
task main() { bNxtLCDStatusDisplay = true; HTMCsetTarget(HTMC); while(HTMCreadRelativeHeading(HTMC)>-90){ motor[motorD] = 15; motor[motorE] = 15; motor[motorF] = 15; motor[motorG] = 15; } stopMotors(); HTMCsetTarget(HTMC); wait10Msec(10); while(true){ checkBTLinkConnected(); readMultipleDataMsgs(); wait1Msec(1); } return; }
task main() { disableDiagnosticsDisplay(); /* motor[leftFront] = 50; wait1Msec(1000); motor[leftFront] = 0; motor[rightFront] = 50; wait1Msec(1000); motor[rightFront] = 0; motor[leftBack] = 50; wait1Msec(1000); motor[leftBack] = 0; motor[rightBack] = 50; wait1Msec(1000); motor[rightBack] = 0; */ //travelDistance(45, dForward); //gyroTurn(50, 35, dLeft); forward(75); wait1Msec(1000); stopMotors(); displayCenteredTextLine(1, "lf = %i", nMotorEncoder[leftFront]); displayCenteredTextLine(2, "lb = %i", nMotorEncoder[leftBack]); displayCenteredTextLine(3, "rf = %i", nMotorEncoder[rightFront]); displayCenteredTextLine(4, "rb = %i", nMotorEncoder[rightBack]); wait1Msec(30000); }
long cmsMenuExit(displayPort_t *pDisplay, const void *ptr) { if (ptr) { displayClear(pDisplay); displayWrite(pDisplay, 5, 3, "REBOOTING..."); displayResync(pDisplay); // Was max7456RefreshAll(); why at this timing? stopMotors(); stopPwmAllMotors(); delay(200); cmsTraverseGlobalExit(&menuMain); if (currentMenu->onExit) currentMenu->onExit((OSD_Entry *)NULL); // Forced exit saveConfigAndNotify(); } cmsInMenu = false; displayRelease(pDisplay); currentMenu = NULL; if (ptr) systemReset(); ENABLE_ARMING_FLAG(OK_TO_ARM); return 0; }
void gyroTurn(float power, float fDegrees, eDirection direct) {//Robot turns at a specific power, a certain angle, either right or left HTGYROstartCal(gyro); //Activate gyroscope wait1Msec(100); //Let it adjust float fCurrent = 0.0; //Set heading to 0 degrees float fRotSpeed = 0.0; //Current rotational speed is 0 if (direct == dLeft) //If told to turn left { leftTurn(power); //Set motors to turn left } else //If tol to turn right { rightTurn(power); //Set motors to turn right } do //Loop through following until conditions are met { wait1Msec(MEASUREMENT_MS); //Give a moment to recalibrate fRotSpeed = HTGYROreadRot(gyro); //Rotation speed is now the gyro's input fCurrent += fRotSpeed * (MEASUREMENT_MS / 1000.0); //Current heading is what is was before //plus the value of the rate of turn times the amount of time that rate value was taken } while (abs(fCurrent) < fDegrees); //Repeat until the heading is the amount told to turn stopMotors(); //You got there, now stop }
void driveTime (int waitTime, int l, int r) //drive infinitely function, order: wait time, power s, power right { motor[motorD] = l; motor[motorE] = r; wait1Msec(waitTime); stopMotors(); }
void setRobotPhysLimit( int distance, char direction, int height, int roller, int tray, int platform, int timer ) { ClearTimer( T1 ); resetEncoders(); //artificiallyresetGyro(); while(time1[T1] < timer && !SensorValue[TowerLimitL] && !SensorValue[TowerLimitR] ) { if( direction == 'S' ) setDrive( distance ); else if( direction == 'T' ) spinDrive( distance ); else wheelDrive( distance, direction ); //setArm( height ); if( height == 0 ) moveArm( -10, -10 ); else { if( SensorValue[ButtonBlockL] == 1 && SensorValue[ButtonBlockR] == 1 ) moveArm( 7, 7 ); else if( SensorValue[ButtonBlockL] == 1 && SensorValue[ButtonBlockR] == 0 ) moveArm( 7, 20 ); else if( SensorValue[ButtonBlockL] == 0 && SensorValue[ButtonBlockR] == 1 ) moveArm( 20, 7 ); else setArm( height ); } moveRollers( roller, roller ); movePiston( tray, platform ); } stopMotors(); }
void HardFault_Handler(void) { LED2_ON; // fall out of the sky uint8_t requiredStateForMotors = SYSTEM_STATE_CONFIG_LOADED | SYSTEM_STATE_MOTORS_READY; if ((systemState & requiredStateForMotors) == requiredStateForMotors) { stopMotors(); } #ifdef TRANSPONDER // prevent IR LEDs from burning out. uint8_t requiredStateForTransponder = SYSTEM_STATE_CONFIG_LOADED | SYSTEM_STATE_TRANSPONDER_ENABLED; if ((systemState & requiredStateForTransponder) == requiredStateForTransponder) { transponderIrDisable(); } #endif LED1_OFF; LED0_OFF; while (1) { #ifdef LED2 delay(50); LED2_TOGGLE; #endif } }
task main() { waitForStart(); moveForward(0.15, 70); wait10Msec(50); stopMotors(); wait10Msec(30); rightTwoWheelTurn(45, 40); wait10Msec(70); stopMotors(); wait10Msec(30); //robot moves backward, so it will be able to sense the ir beacon underneath first bucket /*moveBackward(0.25, 80); wait10Msec(50);*/ moveForward(6, 100); wait10Msec(50); stopMotors(); wait10Msec(30); //keep going forward until ir sensor senses ir beacon StartTask(irRightTesting); wait10Msec(1400); //wall follow the wall until the ultrasonic sensor stops sensing the black base underneath the pendulum while (SensorValue[sonarSensor] < 80) { motor[frontLeftMotor] = 100; motor[frontRightMotor] = -100; motor[backRightMotor] = 100; motor[backLeftMotor] = 100; } stopMotors(); wait10Msec(10); moveForward(1.5, 90); wait10Msec(50); leftTwoWheelTurn(48, 85); wait10Msec(40); moveForward(30, 90); wait10Msec(50); leftTwoWheelTurn(42, 85); wait10Msec(30); moveForward(32, 90); wait10Msec(50); leftTwoWheelTurn(59, 85); wait10Msec(70); moveForward(44, 80); wait10Msec(50); //robot is parked in the middle of the ramp }
// task that turns left when an IR seeker finds the IR beacon underneath a box to put the autonomous block task irRightTesting() { while(true) { if(SensorValue(irSensor) == 5) { stopMotors(); wait10Msec(40); moveForward(0.5, 80); wait10Msec(100); leftTwoWheelTurn(90, 80); wait10Msec(98.5); stopMotors(); wait10Msec(30); motor[tiltingMotor] = 55; wait10Msec(144); motor[tiltingMotor] = 25; wait10Msec(5); motor[tiltingMotor] = 0; wait10Msec(10); motor[conveyorMotor] = 100; wait10Msec(250); motor[conveyorMotor] = 0; wait10Msec(50); motor[tiltingMotor] = -35; wait10Msec(140); motor[tiltingMotor] = -25; wait10Msec(5); motor[tiltingMotor] = 0; wait10Msec(10); stopMotors(); wait10Msec(100); rightTwoWheelTurn(90, 40); wait10Msec(186); stopMotors(); wait10Msec(100); break; } else { motor[frontLeftMotor] = 100; motor[frontRightMotor] = -100; motor[backRightMotor] = 100; motor[backLeftMotor] = 100; } } }
void autoFloor(bool useLift) { int irValue = getIRReading(); strafeDist(40, 100, dRight); displayCenteredTextLine(1, "IR = %i", irValue); if (irValue == 2) { strategyA(useLift); } else { irValue = getIRReading(); if (irValue == 2) { strategyA(useLift); } else { gyroTurn(10, 5, dRight); int irValue1 = getIRReading(); gyroTurn(10, 10, dLeft); int irValue2 = getIRReading(); eraseDisplay(); displayCenteredTextLine(2, "IR1 = %i", irValue1); displayCenteredTextLine(3, "IR2 = %i", irValue2); if (irValue1 == 2 || irValue2 == 2) { gyroTurn(10, 5, dRight); strategyA(useLift); } else { strafeDist(40, 75, dRight); gyroTurn(50, 30, dLeft); resetEncoders(); while (irValue != 6) { irValue = getIRReading(); strafe(100); int enc = abs(nMotorEncoder[leftBack]); displayCenteredTextLine(3, "distance=%i", enc); wait1Msec(1); } stopMotors(); int enc = abs(nMotorEncoder[leftBack]); displayCenteredTextLine(1, "enc = %i", enc); if (enc < 1500) { strategyB(useLift); } else { strategyC(useLift); } } } } }
void HardFault_Handler(void) { // fall out of the sky uint8_t requiredState = SYSTEM_STATE_CONFIG_LOADED | SYSTEM_STATE_MOTORS_READY; if ((systemState & requiredState) == requiredState) { stopMotors(); } while (1); }
//Start of Auto task main() { waitForStart(); //waits for Starts initializeRobot(); //Servos armUp(); forward(6950, 50); stopMotors(); forward(650, 20); // drives down the ramp armUp(); frontServoDown(); wait1Msec(500); turnLeft(4700, 20); wait1Msec(500); //turns to face the other tube armUp(); backServoUp(); wait1Msec(500); backwards(2375, 15); //backwards for half the distance as we want stopMotors(); backServoDown(); armDown(); PlaySound(soundBeepBeep); wait1Msec(500); armUp(); //scores balls frontServoDown(); backServoDown(); wait1Msec(500); turnRight(300, 30); //backs into the second tube and grabs it wait1Msec(500); PlaySound(soundBeepBeep); forward(9000, 50); turnRight(300, 30); forward(450, 50); turnLeft(500, 30); wait1Msec(500); stopMotors(); PlaySound(soundBeepBeep); forward(5000, 7.5); //forward(5000, 20); //makes sure we dont fall short. }
void testLimitSwitch() { wait1Msec(2000); // Loop while robot's sensor is not pressed while (SensorValue(leftLimitSensor) == 0 ) { motor[leftMotor] = 23; motor[rightMotor] = 23; writeDebugStreamLine("Right Limit Sensor: %d", SensorValue(rightLimitSensor)); } stopMotors(2000); }
void strategyX() //use if in position 3, from ramp { displayCenteredTextLine(1, "strategy X"); //display whch strategy it chose strafe(50); //strafe right at half power wait1Msec(timeX1); //keep going for a certain amount of time stopMotors(); //stop travelDistance(distanceX1, dForward); //travel forward a certain distance gyroTurn(30, 80, dLeft); //turn left 80 degrees travelDistance(distanceX2, dForward); //travel forward a certain distance }
task main(){ waitForStart(); servo[servo2] = 180; motor[motorG] = 50; motor[motorF] = 50; sleep(850); stopMotors(1); straight(2, 50); stopMotors(1); goRight(95); motor[motorG] = -50; motor[motorF] = -50; sleep(850); straight(2.9, 50); back(1.7, 50); goRight(15); straight(2.7, 50); //Sandstorm(); }
void find_line()//finds white line on dark surface { int BLACK = SensorValue[S2]; while(SensorValue[S2] < (BLACK + 25)) { nxtDisplayCenteredTextLine(3, "%i, %i", BLACK, SensorValue[S2]); motor[left] = 45; motor[right] = 45; } stopMotors(); }
void turnRight(int rotations, int power){ resetEncoders();//resets encoders while( (nMotorEncoder[leftWheel2] < rotations) && abs(nMotorEncoder[rightWheel2]) < rotations ){ showStatus(rotations, power); motor[rightWheel2] = 0; motor[rightWheel1] = 0; motor[leftWheel2] = power; motor[leftWheel1] = power; } stopMotors(); }//turns right
/** * Stop the robot. */ void stopPosition(PidMotion* pidMotion, bool maintainPositionValue, OutputStream* notificationOutputStream) { updateTrajectoryAndClearCoders(); if (maintainPositionValue) { maintainPosition(pidMotion, notificationOutputStream); } // Avoid that the robot considered he will remain the initial speed for next move (it is stopped). clearInitialSpeeds(pidMotion); DualHBridgeMotor* dualHBridgeMotor = pidMotion->dualHBridgeMotor; stopMotors(dualHBridgeMotor); }
task main() { int mSD, mSE, mSF, mSG; runMotorSpeeds(mSD, mSE, mSF, mSG, 213, 25); wait1Msec(4000); // The program waits 4000 milliseconds (4 seconds) before running further code runMotorSpeeds(mSD, mSE, mSF, mSG, 270, 25); wait1Msec(4000); // The program waits 4000 milliseconds (4 seconds) before running further code runMotorSpeeds(mSD, mSE, mSF, mSG, 323, 25); wait1Msec(4000); // The program waits 4000 milliseconds (4 seconds) before running further code runMotorSpeeds(mSD, mSE, mSF, mSG, 270, 25); wait1Msec(4000); // The program waits 4000 milliseconds (4 seconds) before running further code stopMotors(); }
/** * Stop the robot. */ void stopPosition(bool maintainPositionValue) { updateTrajectoryAndClearCoders(); if (maintainPositionValue) { maintainPosition(); } else { // Avoid that robot reachs his position, and stops the motors setMustReachPosition(false); } // Avoid that the robot considered he will remain the initial speed for next move (it is stopped). clearInitialSpeeds(); stopMotors(); }
void strategyA(bool useLift) { displayCenteredTextLine(6, "Strategy A"); gyroTurn(30, 90, dLeft); stopMotors(); wait1Msec(100); travelDistance(40, dBackward); wait1Msec(100); strafeDist(27, 100, dLeft); wait1Msec(100); goalCenter(useLift); strafeDist(45, 50, dLeft); gyroTurn(30, 10, dLeft); travelDistance(125, dBackward); }
void examineID(msg_pointer mp){ printf("ID is %d\n", mp->ID); if (mp->ID == START_ID) startMotors(); if (mp->ID == STOP_ID) stopMotors(); if (mp-> ID == CONTROL_ID) controlMotors(mp); if (mp-> ID == SPECIAL_COMMAND_ID) specialMotorCommand(mp); return; }