C++ (Cpp) turnToAngle Examples

Example #1

File: AutoOp.c Project: STORM-FTC-4390-7433/FTC-7433-Main-2013

void placeBlockOnIr(DriveSys t, gyroSys g, ConvSys s, bool isRightPath, tSensors IR){
	if(isRightPath){
		if(HTIRS2readDCDir(IR) == 5){
			turnToAngle(t, g, 90.0, 50);
			//updateConvSys(s, 0, 100);
			turnToAngle(t, g, -90.0, 50);
		}
	} else {
		if(HTIRS2readDCDir(IR) == 5){
			turnToAngle(t, g, -90.0, 50);
			//conveyor system
			turnToAngle(t, g, 90.0, 50);
		}
	}
}

Example #2

File: c1b4.c Project: alecebrooks/860-robot-path-finding

task main(){
	calibrateLight();
	calibrateCompass();
	ExploreStack stack;
	Graph graph;

	initStack(stack);
	initGraph(graph);
	Stub stub;
	Edge lastEdge;

	int currentNode = -1;
	int canNode = -1;
	//go to node
	currentNode = exploreNewNode(graph, stack, currentNode, lastEdge);
	if(detectCan()) {
		canNode = currentNode;
		announceCan(currentNode);
	}
	//while not empty
	while(!empty(stack)){
		pop(stack, stub);
		motor[left] = 0;
		motor[right] = 0;
		goToNode(graph, currentNode, stub.node);
		currentNode = stub.node;
		turnToAngle(stub.angle, 15);
		turnToLine();
		nxtDisplayCenteredTextLine(4, "Exploring...");
		int stackSize = stack.top;
		FollowSegmentTilEnd(lastEdge);
		currentNode = exploreNewNode(graph, stack, currentNode, lastEdge);
		if(stackSize == stack.top && detectCan()) {
			canNode = currentNode;
			announceCan(currentNode);
		}
	}
	motor[left] = 0;
	motor[right] = 0;
	eraseDisplay();
	nxtDisplayCenteredTextLine(3, "%i nodes found", graph.nNodes);
	wait10Msec(500);


	if(canNode >= 0) {
		goToNode(graph, currentNode, canNode);
		motor[left] = 0;
		motor[right] = 0;
		eraseDisplay();
		nxtDisplayCenteredTextLine(3, "Can was here");
		wait10Msec(500);
	} else {
		motor[left] = 0;
		motor[right] = 0;
		eraseDisplay();
		PlaySound(soundLowBuzz);
		nxtDisplayCenteredTextLine(3, "There was no can");
		wait10Msec(500);
	}
}

Example #3

File: Qualifier Final Auto 2867.c Project: Pattonville-Robotics/Pre-2015-2016

task main()
{
	waitForStart();
//	raiseBucket();

	int IRState; //Gets value from the IR
	//StartTask (Play);
	StartTask (update_gyroSensor);

  goStraightForTime(4000, 40);
  goStraightForTime(500, 20);
  goStraightForTime(500, 10);

	driveUntilIRNot(5, 40);//Adjust later
	IRState = readIR();
	driveUntilIR(0, 40);

  driveUntilBumper(15);//Adjust later
	if(IRState == 4) // if IR is 4 then Turn
	{
		turnToAngle( -40, -60, -90);
	}
	else
	{
		turnToAngle( 40, -60, 90);
	}
	if(IRState > 5)
	{
		driveUntilIR(1, 40);
		turnToAngle(-40, -0, 0);
		driveUntilBumper(15);
	}
	else if(IRState < 5)
	{
		driveUntilIR(9, 40);
		turnToAngle(40, 0, 0);
		driveUntilBumper(15);
	}
	else
	{
		driveUntilIR(0,40);
	}
	//turnToAngle(40, 60, 90);
	//driveUntilIR();

}

Example #4

File: main.c Project: NsTremblay/real-time2016

/**
 * \brief Entering the robot into the "Attachement mode"
 * \detailed In this mode, the robot will look for a source of high heat and will follow it. If the robot doesn't find a source
 * after a period of time, it goes into a panic mode, which is turning around itself forever until it detects a heat source and follows it.
 * \param pvParameters - pointer to parameters
 */
static void attachmentMode(void * pvParameters) {
	TickType_t xLastWakeTime = xTaskGetTickCount();
	uint8_t distance = 0, panicModeCounter = 0, panicModeEnabledCount = 250;
	uint16_t currentPulseWidthTicks = INITIAL_PULSE_WIDTH_TICKS;

	// Initialize as search mode
	ATTACHMENT_MODE = SEARCH_MODE;

	// Head needs to be facing forward
	motion_servo_start(MOTION_SERVO_CENTER);
	rotateHead(&currentPulseWidthTicks);

	while (1) {
		// If we have new temperature data
		if(hasNewValues == 1){
			hasNewValues = 0;
			// the highest temperature sensed is greater than the trigger temperature
			if ((uint8_t) TRIGGER_TEMPERATURE < temperatures[indexOfHighestTemperature]) {
				// We are tracking a heat source!
				ATTACHMENT_MODE = TRACKING_MODE;

				// reset panic mode counter
				panicModeCounter = 0;

				// get the distance to the heat source
				distance = getDistance();

				// if the highest heat source is not in the center
				if (indexOfHighestTemperature != 4 || indexOfHighestTemperature != 5) {
					// turn towards the heat source
					turnToAngle(45 - 10 * indexOfHighestTemperature);
				}

				// get as close to target as possible
				moveToTarget(distance);

			// no temperature is higher than the trigger temperature
			// increment the panic mode counter and check if we have reached the panic mode enabled count
			} else if (panicModeEnabledCount <= panicModeCounter) {
				ATTACHMENT_MODE = PANIC_MODE;
				setMovementType(ROTATE_RIGHT_MOVEMENT);
			} else {
				++panicModeCounter;
				// Stop movement because we're no longer tracking a heat source
				setMovementType(STOP_MOVEMENT);

				// No longer in tracking or panic mode
				ATTACHMENT_MODE = SEARCH_MODE;
			}
		}

		vTaskDelayUntil(&xLastWakeTime, ((30000 / panicModeEnabledCount) / portTICK_PERIOD_MS));
	}
}

Example #5

File: follow_line.c Project: alecebrooks/860-robot-path-finding

/**
 * Turns around until black is found
 */
void turnToLine(void) {
  spinInDirection();

	while(!isDark()) {
		abortTimeslice();
	}
	int startDir = currentDirection();

	while(isDark()) {
	  abortTimeslice();
  }
	motor[left] = 0;
	motor[right] = 0;
	int centre = startDir + (angleDifference(currentDirection(), startDir)/2);
	nxtDisplayCenteredTextLine(6, "cen: %i", centre);
	turnToAngle(centre, 0);
}