Skip to content

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

93 lines (76 sloc) 2.886 kb
// ----------------------------------------------------------------------- INCLUDES
// ----------------------------------------------------------------------- SYNOPSIS
#include "sound.h"
#include "display.h"
#include "motion.h"
#include "line_controller.h"
#include "beep_task.h"
#include "clean_controller.h"
#include "labirint_controller.h"
#include "grabber_controller.h"
#include "navigation.h"
// --------------------------------------------------------------------------------
/************** DEFINES *******************************************************/
#define MOVE_STRAIGHT_TIME 2000
/************** Procedure *****************************************************/
/************** TASKS *********************************************************/
task motion()
{
/*setAngle( PI, 1 ); */
//setAngle( getAngle( ) - PI, 1 );
/*init_follow_line_controller();
while(true)
follow_line(); */
// test go_to_point
init_grabber_controller();
while(true){
controll_grabber();
}
}
task show_odometry()
{
while( true ) {
Acquire( navigation_mutex );
TextOut(0, LCD_LINE2, "phi:");
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);
TextOut(0, LCD_LINE4, "state:");
NumOut( 30, LCD_LINE4, grabber_controller.state );
TextOut(0, LCD_LINE5, "light::");
NumOut( 30, LCD_LINE5, sensors.light_value );
TextOut(0, LCD_LINE6, "l.ranger:");
NumOut( 30, LCD_LINE6, sensors.ultrasonic_left );
TextOut(0, LCD_LINE7, ".ranger:");
NumOut( 30, LCD_LINE7, sensors.ultrasonic_right );
Wait(100);
ClearScreen();
}
}
task show_data( )
{
while( true ) {
Acquire( navigation_mutex );
TextOut(0, LCD_LINE1, "phi:");
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( motion, odometry_task, navigation, show_odometry );
}
Jump to Line
Something went wrong with that request. Please try again.