Skip to content
This repository

HTTPS clone URL

Subversion checkout URL

You can clone with HTTPS or Subversion.

Download ZIP
branch: master
Fetching contributors…

Cannot retrieve contributors at this time

file 56 lines (42 sloc) 1.777 kb
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56
// ----------------------------------------------------------------------- INCLUDES
// ----------------------------------------------------------------------- SYNOPSIS
#include "motion.h"
#include "navigation.h"

// --------------------------------------------------------------------------------

/************** DEFINES *******************************************************/


/************** Procedure *****************************************************/

/************** TASKS *********************************************************/

task test_go_to_point()
{
   // test go_to_point
   point_controller_t controller;
   init_point_controller(controller);
   position_t goal;

   goal.x = 1.3;
   goal.y = 0;

   while(!go_to_point(goal, controller, 0.2, 0.2) );
}

task show_data()
{
    while( true ) {
        Acquire( navigation_mutex );
        TextOut(0, LCD_LINE2, "US 1:");
NumOut( 30, LCD_LINE1, absolute_position.phi );
        TextOut(0, LCD_LINE2, "x:");
NumOut( 30, LCD_LINE2, absolute_position.x );
TextOut(0, LCD_LINE3, "y:");
        NumOut( 30, LCD_LINE3, absolute_position.y);
Release( navigation_mutex);
        Wait(100);

        ClearScreen();
    }
}



task main()
{
    SetSensor(IN_1, SENSOR_LOWSPEED); // ultrasonic sensor is connected to the nxt port 1
    SetSensor(IN_2, SENSOR_LOWSPEED); // ultrasonic sensor is connected to the nxt port 2
    //SetSensorType(IN_3, IN_TYPE_LIGHT_ACTIVE); // set active light sensing
    //SetSensor(IN_4, SENSOR_LOWSPEED); // color sensor is connected to the nxt port 4
    //SetSensor(IN_4, SENSOR_LIGHT); // light sensor is connected to the nxt port
    
    Precedes( test_go_to_point, show_data, navigation );
}
Something went wrong with that request. Please try again.