Beispiel #1
0
/*
 * main.c
 * Description: Makes the robot move in the corresponding directions.
 * The length of delay cycles after each function call determines how
 * long/far the robot moves in that particular direction.
 */
int main(void) {
    WDTCTL = WDTPW | WDTHOLD;	// Stop watchdog timer

    InitPinsOut();
    InitTimer();


	while (1) {
		//move forward
		moveForward();
		_delay_cycles(1000000);

		//move backward
		moveBackward();
		_delay_cycles(1000000);

		//complete a small (<45 degree) right turn
		rightTurn();
		_delay_cycles(500000);

		//complete a small (<45 degree) left turn
		leftTurn();
		_delay_cycles(500000);

		//complete a large (>45 degree) right turn
		rightTurn();
		_delay_cycles(1000000);

		//complete a large (>45 degree) left turn
		leftTurn();
		_delay_cycles(1000000);
	}

	return 0;
}
Beispiel #2
0
int main(void) {
	WDTCTL = WDTPW | WDTHOLD;	// Stop watchdog timer

	initPinOuts();
	timersConfig();

	while (1) {

		leftTurn();
		__delay_cycles(500000);
		rightTurn();
		__delay_cycles(500000);
		moveForward();
		__delay_cycles(2500000);
		moveBackward();
		__delay_cycles(2500000);
		leftTurn();
		__delay_cycles(1000000);
		rightTurn();
		__delay_cycles(1000000);

	}

	return 0;
}
void main (void)
{
  initialize();
  wait1Msec(200);
  
  //initialize values
  int pwr = 15;
  int sensorState = -1;
  //determine these values by running the calibration program first
  int threshold1 = 150;
  int threshold2 = 150;
  int delay = 10;
  
  //initialize motors and LED indicators
  setMotor (MOTOR_A, 0);
  setMotor (MOTOR_B, 0);
  LEDnum(0);
  LEDoff(RED_LED);
  LEDoff(GREEN_LED);

  //waits for a button press
  while(getSensor(BUTTON) == 0);
  wait1Msec(100);
  while(getSensor(BUTTON) == 1);
  
  resetTimer(); 
  while(getSensor(BUTTON)== 0 && time100() < 64) { 
  //line follows for 6.4 seconds
    if (sensorState != getSensorState(threshold1, threshold2)) { 
    //only change direction if conditions are different than before. 
      sensorState = getSensorState(threshold1,threshold2);
      if (sensorState == 0) { //go straight
        drive(15, delay);
      } else if (sensorState == 1){
        rightTurn(15,delay);
      } else if (sensorState == 2){
        leftTurn(15, delay);
      } else if (sensorState == 3){
        rightTurn(15, delay);
      }  
    }
  } 
  //hard code exit 2
  rightTurn(pwr,2400);
  drive(pwr,9400);
  leftTurn(pwr,1500);
  drive(pwr,20000);
}
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
}
task main()
{
  forward(1000);   //change the value of 1000 to change how long the
  backward(1000);  //robot moves forward, backward, or turns
  rightTurn(1000);
  leftTurn(1000);
}
Beispiel #6
0
__interrupt void interrupt1() {

	if (P1IFG & I1_LEFT_BUTTON) {
		switch (turnState) {
		case NO_TURN:
			leftTurn();
			autoCancelState = AUTO_CANCEL_DISABLED;
			break;
		case LEFT_TURN:
			cancelTurn();
			break;
		case RIGHT_TURN:
			cancelTurn();
			break;
		}

		P1IFG &= ~I1_LEFT_BUTTON;				// Clear interrupt flag
	}


	if (P1IFG & I1_LEFT_BRAKE || P1IFG & I1_RIGHT_BRAKE) {
		switch (brakeState) {
		case NO_BRAKES:
			setBrakes();
			break;
		}

		P1IFG &= ~(I1_LEFT_BRAKE + I1_RIGHT_BRAKE);				// Clear interrupt flag
	}


	LPM4_EXIT;
}
task main()
{

	wait1Msec(2000);						// Robot waits for 2000 milliseconds before executing program


	rightTurn();

	fowardTime (3000);

	leftTurn();

	fowardTime(5000);

	leftTurn();

	fowardTime(6000);

	rightTurn();

	fowardTime(4000);

	rightTurn ();

	fowardTime(6000);

	leftTurn ();

	fowardTime (4000);

	leftTurn ();

	fowardTime (6000);

	rightTurn();

	fowardTime (4000);





	}												        // Program ends, and the robot stops
Beispiel #8
0
int main(int argc, char *argv[])
{
//    set_conio_terminal_mode();

    fdFifo = open("./RobotFifo", O_WRONLY);

    for (int n = 0; n < 4; n++)
    {
        forward(40, 5);
        leftTurn(4);
    }
    stop();
}
task main()
{
    while(true)
    {
        if(VexRT(Btn5U)) {
            leftTurn(1);
            wait10Msec(100);
        }
        if(VexRT(Btn6U)) {
            rightTurn(1);
            wait10Msec(100);
        }
        if(VexRT(Btn8U)) {
            move(2);
            wait10Msec(100);
        }
        if(VexRT(Btn8D)) {
            move(-2);
            wait10Msec(100);
        }

        if(VexRT(Btn7U)) {
            rightTurn(360);
        }


        if(vexRT(Btn8R)) {
            rightTurn(2);
            move(20);
            leftTurn(10);
            move(30);
            intake(true);
            fly(3);
            wait10Msec(1000);
            rest();
        }
    }
}
task main()
{
	initializeRobot();
	waitForStart();
	raiseArm(ArmUpRotations);
	intializeClaw();

  int _dirAC = 0;

  wait1Msec(200);

  int sensorDirection = HTIRS2readDCDir(irSeeker);

  if(sensorDirection < 5){
  	raiseArm(LeftArmUpRotations);
  	leftTurn(leftTurnRotations);
  	attackRack(leftFinalInches);
	}
	else{
		firstMove(middleStartInches);

		wait1Msec(300);
		sensorDirection = HTIRS2readDCDir(irSeeker);
		if(sensorDirection < 7){
			leftTurn(middleTurnRotations);
			attackRack(middleFinalInches);
		}
		else{
			raiseArm(RightArmUpRotations);
			firstMove(rightStartInches - middleStartInches);
			leftTurn(middleTurnRotations);
			attackRack(rightFinalInches);
		}
	}
	retreat();
}
Beispiel #11
0
  ConvexHull(const Polygon &scatteredPoints) {
    const int numPoints = scatteredPoints.size();
    Polygon copy(scatteredPoints);
    // Sort input by x (lex):
    std::sort(&copy[0], &copy[numPoints]);

    // Find left and rightmost points:
    Point leftMost = copy[0];
    Point rightMost = copy[numPoints-1];

    //Split points in upper and lower:
    Polygon upperPoints;
    Polygon lowerPoints;
    for(int i = 1; i < numPoints-1; ++i) {			
      Point p = copy[i];
      if(colinear(leftMost, rightMost, p)) {
	continue;
      }
      if(leftTurn(leftMost, rightMost, p)) {
	upperPoints.push_back(p);
      }
      else {
	lowerPoints.push_back(flipY(p));
      }
    }

    upperPoints = upperHull(leftMost, rightMost, upperPoints);
    lowerPoints = upperHull(flipY(leftMost), flipY(rightMost), lowerPoints);
    
    //Make points of hull:
    int actualNumPoints = size = upperPoints.size() + lowerPoints.size();
    points = new Point[actualNumPoints];
    int i = 0;
    for(Polygon::const_iterator it = upperPoints.begin(); it != upperPoints.end(); ++it) {
      points[i++] = *it;
    }		
    points[i] = rightMost;
    i = actualNumPoints;
    for(Polygon::const_iterator it = lowerPoints.begin(); it != lowerPoints.end(); ++it) {
      i--;
      if(i != actualNumPoints-1)
	points[i+1] = flipY(*it);
    }
    std::cout << actualNumPoints;
    for(i = 0; i < actualNumPoints; ++i)
      std::cout << " " << points[i].first << " " << points[i].second;
    std::cout << std::endl;
  }
Beispiel #12
0
void faceIR(tSensors ir_seeker)
{
	int	ir_value = getIRReading(/*ir_seeker*/);
	do
	{
		if(ir_value < 2)
		{
			leftTurn(50);
		}
		else if(ir_value > 2)
		{
			rightTurn(50);
		}
		wait1Msec(1);
		ir_value = getIRReading(/*ir_seeker*/);
	}
	while(ir_value != 2);
}
//Realiza el algoritmo de Graham de las 3 monedas para hallar el Convex Hull S de una lista de Puntos Q.
//Devuelve una Pila con el resultado clockwise.
void grahamScan(std::list<Point2D> & Q, std::stack<Point2D> & S){
  minimal = encontrarMinimal(Q);                            //Encuentra el minimal izquierda abajo
 // std::cout<<"Minimal: "; minimal.print();
  
  borrarMinimal(Q, minimal);                                //Borra el minimal de la cola 

  Q.sort(comparePoint2DPolar);                              //Ordena en forma polar
  std::cout<<"Lista ordenada\n"; printList(Q);
  
  eliminarColineales(Q);                                    //Hace limpieza de los puntos colineales, dejando el mas lejano
  std::cout<<"Lista ordenada\n"; printList(Q);
  
  
  //Ubica las 3 primeras monedas
  S.push(minimal);                                          //Agrega el primero que es el minimal
  
  //Agrega la segunda y tercera
  std::list<Point2D>::iterator it = Q.begin();              //Iterador para recorrer la Q
  for(unsigned int i = 0; i < 2 and it != Q.end(); i++, it++){
    S.push(*it);
  }
  
  //tamanio de Q
  unsigned int n = Q.size();

  //Loop de Graham Scan
  for(unsigned int i = 2; i < n and it != Q.end(); i++, it++){
    Point2D ntt = nextToTop(S);
    Point2D nt = top(S);
    Point2D p = *it;
    while(!leftTurn(ntt, nt, p) and (S.size() > 1)){        //Si no froman un giro a la izquierda y queda mas de un elemento en S
      // printStack(S);
      S.pop();                                              //Saco el tope de S
      ntt = nextToTop(S);                                   //Renuevo los valores y vuelvo a probar
      nt = top(S);
    }
    S.push(p);                                              //Agrego el elemento a S
  }
}
Beispiel #14
0
task FigureEight()		//Starts first task, task main()
{

	while(1==1)
		{
			for(int i = 0; i <= 3; i++)  // initialize int 'i' to 0, and run the loop as long as 'i' is less than 3,
                             // incrementing 'i' by 1 after each iteration of the loop

			{
				moveForward(15);
				rightTurn();
			}
			for(int i = 0; i <= 3; i++)  // initialize int 'i' to 0, and run the loop as long as 'i' is less than 3,
                             // incrementing 'i' by 1 after each iteration of the loop

			{
				moveForward(15);
				leftTurn();
			}

	}
}
Beispiel #15
0
bool Util::leftTurn(const QVector2D& a, const QVector2D& b, const QVector2D& c) {
	return leftTurn(b - a, c - b);
}
Beispiel #16
0
void
play(void)
{
	MWEvent		event;
	MW244BPacket	incoming;

	event.eventDetail = &incoming;

	while (TRUE) {
		NextEvent(&event, M->theSocket());
		if (!M->peeking())
			switch(event.eventType) {
			case EVENT_A:
				aboutFace();
				break;

			case EVENT_S:
				leftTurn();
				break;

			case EVENT_D:
				forward();
				break;

			case EVENT_F:
				rightTurn();
				break;

			case EVENT_BAR:
				backward();
				break;

			case EVENT_LEFT_D:
				peekLeft();
				break;

			case EVENT_MIDDLE_D:
				shoot();
				break;

			case EVENT_RIGHT_D:
				peekRight();
				break;

			case EVENT_NETWORK:
				processPacket(&event);
				break;

			case EVENT_INT:
				quit(0);
				break;

			}
		else
			switch (event.eventType) {
			case EVENT_RIGHT_U:
			case EVENT_LEFT_U:
				peekStop();
				break;

			case EVENT_NETWORK:
				processPacket(&event);
				break;
			}

		ratStates();		/* clean house */

		manageMissiles();

		DoViewUpdate();

		mws_update(M->state);

		/* Any info to send over network? */

	}
}
Beispiel #17
0
void performLogic(uint8_t buffer [] , uint8_t length, MoveTaskStruct *moveTPtr, SensorAStruct *sensorT)
{
    webServerTaskStruct *webServerData = sensorT->webServerData;
    
    uint16_t frontDistanceInches = getFrontSensorDistanceInInches(buffer, length);
    uint16_t topRightDistanceInches = getTopRightSensorDistanceInInches(buffer, length);
    uint16_t bottomRightDistanceInches = getBottomRightSensorDistanceInInches(buffer, length);

	statePrintedToWebServer = current_state;
    
    if (current_state == GLOBAL_READ) {
        printf("GLOBAL READ \n");
    }
    
    else if (current_state == GETTING_IN_3_FEET){
        printf("Get In 3 Feet \n");
    }
    
    else if (current_state == AVOID_OBSTACLE_1){
        printf("AVOID_OBSTACLE_1 \n");
    }
    
    else
        printf("Invalid State \n");
    
    switch (current_state) {
            
        case GLOBAL_READ:
        {
            
            //Check whether both sensor values out of threshold
            
            uint8_t outOfRange = sideSensorOutOfRange(topRightDistanceInches, bottomRightDistanceInches);
            
			//printf("OutOfRange %d\n" , outOfRange);
            
            if (outOfRange == 1) {
                // Turn right and go to the state where u wait for the side
                // sensor values to be in range or the front is not clear
                
                if (count == 0) {
                    moveForward(moveTPtr, sensorT->mapT,webServerData, 8);
                    count++;
                    return;
                }
                
                if (prev_state == AVOID_OBSTACLE_1) {
                    prev_state = GLOBAL_READ;
                    current_state = GETTING_IN_3_FEET;
                }
                
                else {
                    
                    prev_state = GLOBAL_READ;
                    current_state = AVOID_OBSTACLE_1;
                }
                count = 0;
                rightTurn(moveTPtr, sensorT->mapT,webServerData);
                return;
            }
            
            
            //Check whether both sensor values are highly deviated
            
            uint8_t inHighTolerance = sideSensorWithinHighTolerance(topRightDistanceInches, bottomRightDistanceInches);
            
            // |  |-----------
            // |  |
            // |  |----
            
            //Rover not in high tolerance (posibly crossing the obstacle
            if (inHighTolerance == 0) {
                // Move a little forward and go to the state where u wait for the side
                // sensor values to be in range or the front is not clear
                
                uint8_t isFrontClear = frontSensorClear(frontDistanceInches);
                
                if (isFrontClear == 1)
                {
                    //printf("Move Forward\n");
                    moveForward(moveTPtr, sensorT->mapT,webServerData, 12);
                    return;
                }
                
                else if (getOneFeetToWall(moveTPtr, webServerData, sensorT->mapT, frontDistanceInches))
                {
                    return;
                }
                
                else {
                    //printf("Left Turn\n");
                    leftTurn(moveTPtr, sensorT->mapT,webServerData);
                }
                
                return;
                
            }
            
            // Check Whether the sensor values are low deviated
            
            uint8_t isInTolerance= sideSensorsWithinTolerance(topRightDistanceInches,
                                                              bottomRightDistanceInches);
            
            if (isInTolerance == 0 && tiltCount <= 3)
            {
                if (topRightDistanceInches > bottomRightDistanceInches)
                    tiltRover(moveTPtr, sensorT->mapT,webServerData, 10, right_turn_ToRover);
                else
                    tiltRover(moveTPtr, sensorT->mapT,webServerData, 10, left_turn_ToRover);
                return;
            }
           	
			tiltCount = 0;
            // Then check if the rover is within 3 feet
            
            uint8_t in3Feet = roverWithin3Feet(topRightDistanceInches, bottomRightDistanceInches);
            
            if (in3Feet == 0) {
                rightTurn(moveTPtr, sensorT->mapT,webServerData);
                prev_state = GLOBAL_READ;
                current_state = GETTING_IN_3_FEET;
                return;
            }

//			if (topRightDistanceInches <= 9 && bottomRightDistanceInches <= 9){
//				 leftTurn(moveTPtr, sensorT->mapT,webServerData);
//				 return;
//			}
            
            
            uint8_t isFrontClear = frontSensorClear(frontDistanceInches);
            
            if (isFrontClear == 1)
            {
                //printf("Move Forward\n");
				if ( RUN == 2 )
				{
					if ( frontDistanceInches > clearDistance(sensorT->mapT, 0) && isDistanceFromMapValid == 1)
					{
					   moveForward(moveTPtr, sensorT->mapT,webServerData, clearDistance(sensorT->mapT, 0));
					}

					else
					{
						moveForwardOneUnit(moveTPtr, sensorT->mapT,webServerData);
						isDistanceFromMapValid = 0;
					}
				}

				else
				{
                	moveForwardOneUnit(moveTPtr, sensorT->mapT,webServerData);
				}
            }
            
            else if (getOneFeetToWall(moveTPtr, webServerData, sensorT->mapT, frontDistanceInches))
            {
                return;
            }
            
            else {
                
                //printf("Left Turn\n");
                leftTurn(moveTPtr, sensorT->mapT,webServerData);
            }
            
            return;
        };
            
        case GETTING_IN_3_FEET:
        {
            
            uint8_t inRange = roverWithin3Feet(topRightDistanceInches, topRightDistanceInches);
            
            if (inRange == 1) {
                prev_state = GETTING_IN_3_FEET;
                current_state = GLOBAL_READ;
                break;
            }
            
            uint8_t isFrontClear = frontSensorClear(frontDistanceInches);
            
            if (isFrontClear == 1)
            {
					//printf("Move Forward\n");
				if ( RUN == 2 )
				{
					if ( frontDistanceInches > clearDistance(sensorT->mapT, 0) && isDistanceFromMapValid == 1)
					{
					   moveForward(moveTPtr, sensorT->mapT,webServerData, clearDistance(sensorT->mapT, 0));
					}

					else
					{
						moveForwardOneUnit(moveTPtr, sensorT->mapT,webServerData);
						isDistanceFromMapValid = 0;
					}
				}

				else
				{
                	moveForwardOneUnit(moveTPtr, sensorT->mapT,webServerData);
				}                

                break;
            }
            
            else if (getOneFeetToWall(moveTPtr, webServerData, sensorT->mapT, frontDistanceInches))
            {
                break;
            }
            
            else {
                prev_state = GETTING_IN_3_FEET;
                current_state = GLOBAL_READ;
                leftTurn(moveTPtr, sensorT->mapT,webServerData);
                break;
            }
            
            break;
        };
            
        case AVOID_OBSTACLE_1:
        {
            
            // Check Whether the sensor values are low deviated
            
            uint8_t inRange = roverWithin3Feet(topRightDistanceInches, topRightDistanceInches);
            
            if (inRange == 1) {
                prev_state = AVOID_OBSTACLE_1;
                current_state = GLOBAL_READ;
                break;
            }
            
            uint8_t isFrontClear = frontSensorClear(frontDistanceInches);
            
            if (isFrontClear == 1)
            {
				moveForwardOneUnit(moveTPtr, sensorT->mapT,webServerData);
			}
            
            else if (getOneFeetToWall(moveTPtr, webServerData, sensorT->mapT, frontDistanceInches))
            {
                break;
            }
            
            else {
                //printf("Left Turn\n");
                prev_state = AVOID_OBSTACLE_1;
                current_state = GLOBAL_READ;
                leftTurn(moveTPtr, sensorT->mapT,webServerData);
            }
            
            break;
        };
            
        default:
            break;
    }
}
Beispiel #18
0
/* ----------------------------------------------------------------------- */
void play(void)
{	// initialisation
	cout << "Starting Play" << endl;
	//SetRatPosition(7,1,5,0);
	proj.present = false;
	proj.prev_x = proj.prev_y = proj.x = proj.y = 1;
	int turn = 0;
	
	updateSeqNo = true;
	join = false;
	
	for (int k = 0; k < 8; k++)
		participants[k] = prevseq_a[k] = 0;
	for (int k = 0; k < 8; k++)
		expected_seqno[k] = k+1;
	GLOBAL_ID = 0;
	checking = 1;
	checkingzero=1;
	MWEvent		event;
	MW244BPacket	incoming;

	event.eventDetail = &incoming;

	while (TRUE) {
		turn ++;
		NextEvent(&event, M->theSocket());
		if (!M->peeking())
			switch(event.eventType) {
				case EVENT_A:
					aboutFace();
					break;

				case EVENT_S:
					leftTurn();
					break;

				case EVENT_D:
					forward();
					break;

				case EVENT_F:
					rightTurn();
					break;


				case EVENT_LEFT_D:
					peekLeft();
					break;

				case EVENT_BAR:
					shoot();
					break;

				case EVENT_RIGHT_D:
					peekRight();
					break;

				case EVENT_NETWORK:
					processPacket(&event);
					break;

				case EVENT_TIMEOUT:

					checking = (++checking) % 25;					
					if (checking == 0 && !join){
						cout << "Setting ID to zero" << endl;
						join = true;
						setMapping();
					}		
					else {cout << "TIMEOUT"  << endl;
						participation = (++participation) % 30;
						manageMissiles();
						if (participation == 29)
						{ ratStates(); /* clean house */
							for (int i = 0 ; i < 8 ; i++)
								participants[i] = 0;
						}
					}
					break;

				case EVENT_INT:
					quit(0);
					break;

			}
		else
			switch (event.eventType) {
				case EVENT_RIGHT_U:
				case EVENT_LEFT_U:
					peekStop();
					break;
				case EVENT_NETWORK:
					processPacket(&event);
					break;
			}
		DoViewUpdate();
		checking ++;

		manageMissiles();
		if (join && checkingzero == 0)
		{	
			MW244BPacket p;
			makePacket(&p,'a',kills, updateSeqNo);
			sendPacketToPlayers(p);
		}

		else if (join && turn % 2 == 0)
		{
			MW244BPacket p;
			makePacket(&p,'t',-1, updateSeqNo);
			sendPacketToPlayers(p);
		}

	}
}
// Point inside triangle
// Returns true iff point (xx,yy) is inside the counter-clockwise triangle (x[3],y[3])
// REQUIRES: leftTurn()
bool pointInsideTriangle( double x[], double y[], double xx, double yy ){
	return leftTurn(x[0], y[0], x[1], y[1], xx, yy)
		&& leftTurn(x[1], y[1], x[2], y[2], xx, yy)
		&& leftTurn(x[2], y[2], x[0], y[0], xx, yy);
}
int main(void)
{
	
	//Decs
	int right_sensor= 0;
	int center_sensor = 1;
	int left_sensor = 2;
	int dark = 500; 
	int light_dark = 500;
	int last_turn = 0;
	
	
	
	printf("Starting line following program\n");
	printf("Center the robot and calibrate the left then right sensor to begin\n\n");
	
	

	//While the B button is not pressed, wait for input
	while(analog10(left_sensor) < 600)
	{
		
	}
	
	printf("Left sensor is ready\n\n");
	
	while(analog10(right_sensor) < 600)
	{
		
	}
	
	printf("Right sensor is ready\n\n");
	
	printf("Press B to begin\n");
	
	while(b_button() == 0)
	{
		
	}
	
	printf("Running\n\n");
	
	move(25, 100);
	
	while(1==1)
	{
	
		//While there is not turn available, keep on keeping on.
		while(isTurnAvailable() == 0)
		{
			moveForward(last_turn);
		}
		
		printf("Turn is available\n");
		
		if(isTurnAvailable() == 2)
		{
			leftTurn();
			last_turn = 1;
		}
		
		if(isTurnAvailable() == 1)
		{
			rightTurn();
			last_turn = 0;
		}
		
	}//End of while loop
	
	printf("Done");
	
	
	return 0;
}