Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with
or
.
Download ZIP
Branch: master
Fetching contributors…

Cannot retrieve contributors at this time

148 lines (125 sloc) 4.148 kB
#ifndef __CLEAN_CONTROLLER_H
#define __CLEAN_CONTROLLER_H
#include "motion.h"
#include "line_controller.h"
//------------------------------------------------
// Defines.
//------------------------------------------------
#define CLEANING_SPEED 1
#define CLEANING_OMEGA 0.5
#define DROPPING_SPEED 0.4
#define GO_FORWARD 0
#define GO_BACKWARD 1
#define GO_TO_EXIT 2
#define GO_DROP 3
#define GO_TO_NEXT_LINE 4
#define NOT_CROSSED 0
#define STARTED 1
#define CROSSED 2
#define REVERSE_DELAY 700
#define NEXT_LINE_DELAY 500
//------------------------------------------------
// Data structures.
//------------------------------------------------
struct clean_controller_t
{
int forward_value;
int right_value;
int mode;
int light_value;
int black_line_state;
int current_line;
};
//------------------------------------------------
// Global varriables.
//------------------------------------------------
clean_controller_t clean_controller;
//------------------------------------------------
// Functions.
//------------------------------------------------
void clean_change_mode(int mode);
void init_clean_controller( )
{
SetSensor(LIGHT_SENSOR, SENSOR_LIGHT);
SetSensorType(LIGHT_SENSOR, IN_TYPE_LIGHT_ACTIVE);
clean_controller.forward_value = SensorUS(FORWARD_RANGER);
clean_controller.right_value = SensorUS(RIGHT_RANGER);
clean_controller.mode = GO_FORWARD;
clean_controller.light_value = Sensor( LIGHT_SENSOR );
clean_controller.black_line_state = NOT_CROSSED;
clean_controller.current_line = 0;
}
int searcheLine( )
{
clean_controller.light_value = Sensor( LIGHT_SENSOR );
if( clean_controller.light_value < BLACK_LIGHT_VALUE + BIAS &&
clean_controller.black_line_state == NOT_CROSSED ) {
return STARTED;
}
else if( clean_controller.light_value > BLACK_LIGHT_VALUE + BIAS &&
clean_controller.black_line_state == STARTED ) {
return CROSSED;
}
return clean_controller.black_line_state;
}
void drop()
{
unsigned long startTime = CurrentTick();
do {
clean_controller.black_line_state = searcheLine( );
setSpeed( DROPPING_SPEED, 0 );
}
while( clean_controller.black_line_state != CROSSED &&
CurrentTick() - startTime < 400 );
setSpeed( 0, 0 );
}
void clean( )
{
switch(clean_controller.mode)
{
case GO_FORWARD:
clean_controller.black_line_state = NOT_CROSSED;
go_and_rotate(PI, CLEANING_SPEED, CLEANING_OMEGA);
clean_change_mode( GO_BACKWARD);
break;
case GO_BACKWARD:
go_and_rotate(PI/2, CLEANING_SPEED, CLEANING_OMEGA);
if( clean_controller.black_line_state == NOT_CROSSED &&
clean_controller.current_line < 2 )
clean_change_mode ( GO_TO_EXIT);
else
clean_change_mode ( GO_DROP);
break;
case GO_TO_EXIT:
clean_controller.black_line_state = NOT_CROSSED;
go_and_rotate(-PI/2, CLEANING_SPEED, CLEANING_OMEGA);
clean_change_mode( GO_DROP);
break;
case GO_DROP:
drop();
clean_change_mode( GO_TO_NEXT_LINE );
break;
case GO_TO_NEXT_LINE:
setSpeed( - CLEANING_SPEED, 0 );
Wait( REVERSE_DELAY );
setAngle( getAngle() - PI / 2, CLEANING_OMEGA );
++clean_controller.current_line;
if( clean_controller.current_line > 5 )
clean_controller.current_line = 1;
setSpeed( CLEANING_SPEED, 0 );
Wait( NEXT_LINE_DELAY * clean_controller.current_line);
setAngle( getAngle() - PI / 2, CLEANING_OMEGA );
clean_change_mode( GO_FORWARD );
break; /*
Wait( 100);
Off( CHASSIS );
Wait( 100);
break; */
}
}
void clean_change_mode(int mode )
{
setSpeed(0, 0);
clean_controller.mode = mode;
}
#endif
Jump to Line
Something went wrong with that request. Please try again.