Exemple #1
0
int main(){	
	connect_to_robot();
	initialize_robot();

	setAngles();
	firsteight();
}
Exemple #2
0
int main ()
{	
	connect_to_robot();
	initialize_robot();
	setAnglesStart();
	//wallFollowingR();	
	wallFollowingL();
}
Exemple #3
0
int main(){
	connect_to_robot();
    initialize_robot();
	set_origin();
    double curr_coord[2] = {0, 0};
    spin(curr_coord, to_rad(90));
    sleep(1);
    spin(curr_coord, to_rad(180));
    return 0;
}
void setup()
{
	connect_to_robot();
	initialize_robot();

	set_ir_angle(LEFT, -45);
	set_ir_angle(RIGHT, 45);

	set_origin();
	set_point(0, 0);
	print_square_centres();

	get_motor_encoders(&leftprev, &rightprev);
	moveBackwards(PHASE1_SPEED, PHASE1_SLOW, START_OFFSET * CM_TO_ENCODER); //move robot to centre of first square.
}
task main()
{
    int i;
    int center_position;

    initialize_robot();

    //waitForStart();

    // startTask(raise_elbow);
    startTask(raise_hand);
    startTask(lower_ramp);

    // initialize_receiver(irr_left, irr_right);

    servo[leftEye] = LSERVO_CENTER;
    servo[rightEye] = RSERVO_CENTER;

    center_position = ultrasound_mk(carrot, -24, US_DIST_POS_1, US_DIST_POS_2, US_DIST_POS_3);

    //startTask(raise_ramp);

    if (center_position == -1) {
        playImmediateTone(251, 150);
    }

    for (i = 0; i < center_position; i++) {
        playImmediateTone(251, 50);
        wait1Msec(1000);
    }

    nMotorEncoder[elbow] = 0;

    motor[elbow] = 20;
    while (nMotorEncoder[elbow] <= 3600) {
        nxtDisplayCenteredBigTextLine(3, "%d", nMotorEncoder[elbow]);
    }
    motor[elbow] = 0;

    // move_to_pole(center_position);

    while(true) {
        nxtDisplayCenteredBigTextLine(3, "%d", USreadDist(carrot));
    }
}
Exemple #6
0
int main()
{
    connect_to_robot();
    initialize_robot();
	setAngles();
    inputMotors();
    //wallFollowing();

    int i;
    for (i=0; i<4; i++){
        printf("%d\n", i);
        thetaAcc = 0.0;
        SPEED = SPEED + 8;
        //wallFollowing();
        wallFollowingR();
    }

}
Exemple #7
0
int main(){

    connect_to_robot();
    initialize_robot();
    set_origin();
    set_ir_angle(LEFT, -45);
    set_ir_angle(RIGHT, 45);
    initialize_maze();
    reset_motor_encoders();
    int i;
    for (i = 0; i < 17; i++){
        set_point(nodes[i]->x, nodes[i]->y);
    }
    double curr_coord[2] = {0, 0};
    map(curr_coord, nodes[0]);
    breadthFirstSearch(nodes[0]);
    reversePath(nodes[16]);
    printPath(nodes[0]);

    struct point* tail = malloc(sizeof(struct point));
    tail->x = nodes[0]->x;
    tail->y = nodes[0]->y;
    struct point* startpoint = tail;

    build_path(tail, nodes[0]);
    
    // Traverse to end node.
    while(tail->next){
        set_point(tail->x, tail->y); // Visual display for Simulator only.
        tail = tail->next;
    }
    tail->next = NULL; // Final node point to null.
    printf("tail: X = %f Y = %f \n", tail->x, tail->y);
    parallel(curr_coord);
    spin(curr_coord, to_rad(180));
    
    sleep(2);
    set_ir_angle(LEFT, 45);
    set_ir_angle(RIGHT, -45);

    mazeRace(curr_coord, nodes[0]);
    return 0;
}
Exemple #8
0
int main() {
	connect_to_robot();
	initialize_robot();
	set_origin();

	printf("Ghandy-Bot activated, destroy!!!\n");

	//Set front IR sensors to to face left and right
	set_ir_angle(LEFT, -45);
    set_ir_angle(RIGHT, 45);

	init_map();
    print_map();

	robot.map[1][1].walls[0] = 1;
    robot.map[1][2].walls[2] = 1;

    robot.map[1][1].walls[1] = 1;
    robot.map[2][1].walls[3] = 1;

    robot.map[1][3].walls[2] = 1;
    robot.map[1][2].walls[0] = 1;

    robot.map[0][4].walls[2] = 1;
    robot.map[0][3].walls[0] = 1;

    robot.map[3][4].walls[3] = 1;
    robot.map[2][4].walls[1] = 1;

    robot.map[3][2].walls[2] = 1;
    robot.map[3][1].walls[0] = 1;

    robot.map[3][2].walls[3] = 1;
    robot.map[2][2].walls[1] = 1;
    int i;
    for(i=1;i<=4;i++)
        robot.map[0][i].walls[3] = 1;
   	for(i=1;i<=4;i++)
        robot.map[i][1].walls[2] = 1;
    for(i=1;i<=4;i++)
        robot.map[3][i].walls[1] = 1; 
    print_map();
    /* Phase 1 */

    //Perform depth first search on maze
    //dfs(0, 0, M_PI / 2);

    /* Phase 2 */
    lee(0,1); //Update matrix with Lee costs from node [0,1]

    /* Generate shortest path to node [3,4] (finish line) */
    generate_path(3,4); 

    follow_path(); //Follow the generated path to finish

    //Phew, did we win???

	printf("Ghandy-Bot deactivated!\n");

	return 0;
}
void setup()
{
	// Add your initialization code here
	uint8_t count = 10;
	// join I2C bus (I2Cdev library doesn't do this automatically)
	Wire.begin();
	// initialize serial communication
	// (115200 chosen because it is required for Teapot Demo output, but it's
	// really up to you depending on your project)
	Serial.begin(115200);

//	while (!Serial); // wait for Leonardo enumeration, others continue immediately
//	// NOTE: 8MHz or slower host processors, like the Teensy @ 3.3v or Ardunio
//	// Pro Mini running at 3.3v, cannot handle this baud rate reliably due to
//	// the baud timing being too misaligned with processor ticks. You must use
//	// 38400 or slower in these cases, or use some kind of external separate
//	// crystal solution for the UART timer.
//	pinMode(IRQ_PIN, INPUT);
//	digitalWrite(IRQ_PIN, HIGH);
//	// initialize device
//	Serial.println(F("Initializing I2C devices..."));
//	mpu.initialize();
//	// verify connection
//	Serial.println(F("Testing device connections..."));
//	Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
//	/*
//	// wait for ready
//	Serial.println(F("\nSend any character to begin DMP programming and demo: "));
//	while (Serial.available() && Serial.read()); // empty buffer
//	while (!Serial.available()); // wait for data
//	while (Serial.available() && Serial.read()); // empty buffer again
//	*/
//	// load and configure the DMP
//	Serial.println(F("Initializing DMP..."));
//	do {
//		devStatus = mpu.dmpInitialize();
//		// make sure it worked (returns 0 if so)
//		if (devStatus == 0) {
//			count = 10;
//			// turn on the DMP, now that it's ready
//			Serial.println(F("Enabling DMP..."));
//			mpu.setDMPEnabled(true);
//			// enable Arduino interrupt detection
//			Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
//			attachInterrupt(DMP_IRQ, dmpDataReady, RISING);
//			mpuIntStatus = mpu.getIntStatus();
//			// set our DMP Ready flag so the main loop() function knows it's okay to use it
//			Serial.println(F("DMP ready! Waiting for first interrupt..."));
//			dmpReady = true;
//			// get expected DMP packet size for later comparison
//			packetSize = mpu.dmpGetFIFOPacketSize();
//			// exit the loop since everything is configured
//			// configure LED for output
//			pinMode(LED_PIN, OUTPUT);
//			return;
//		} else {
//			// ERROR!
//			// 1 = initial memory load failed
//			// 2 = DMP configuration updates failed
//			// (if it's going to break, usually the code will be 1)
//			Serial.print(F("DMP Initialization failed (code "));
//			Serial.print(devStatus);
//			Serial.println(F(")"));
//			// New attempt message
//			Serial.println("Trying again");
//		}
//	}
//	while (--count);
//	// configure LED for output
//	pinMode(LED_PIN, OUTPUT);
//	// Check if the configuration has failed
//	// if (!count) {
//	Serial.println("DMP initialization failed");
//	while (true) {
//		// Locks in infinite loop
//		digitalWrite(LED_PIN, 0);
//		delay(300);
//		digitalWrite(LED_PIN, 1);
//		delay(300);
//	}
//	// }

	while (!Serial)
	{
		digitalWrite(13, HIGH);
		delay(300);
		digitalWrite(13, LOW);
		delay(300);
	}
	pinMode (IRQ_PORT, OUTPUT);
	digitalWrite(IRQ_PORT, LOW);
	initialize_robot();

	isConnected = true;

	Serial.print(PROMPT);
}