Permalink
Switch branches/tags
Nothing to show
Find file
Fetching contributors…
Cannot retrieve contributors at this time
110 lines (90 sloc) 2.93 KB
#ifndef __LINE_CONTROLLER_H
#define __LINE_CONTROLLER_H
#include "motion.h"
//------------------------------------------------
// Defines.
//------------------------------------------------
#define WHITE_LIGHT_VALUE 60
#define BLACK_LIGHT_VALUE 35
#define BIAS 10
#define SPEED 1
#define OMEGA 0.4
#define LIGHT_SENSOR IN_3
#define STATE_MOVING 0
#define STATE_ROTATING_LEFT 1
#define STATE_ROTATING_RIGHT 2
#define STATE_ROTATING_TO_BEST 3
#define ROTATING_TIME 400
//------------------------------------------------
// Data structures.
//------------------------------------------------
struct follow_line_controller_t
{
int state;
int best_value;
float best_angle;
unsigned long start_rotating_time;
};
//------------------------------------------------
// Global varriables.
//------------------------------------------------
follow_line_controller_t follow_line_controller;
//------------------------------------------------
// Functions.
//------------------------------------------------
void init_follow_line_controller( )
{
SetSensor(LIGHT_SENSOR, SENSOR_LIGHT);
follow_line_controller.state = STATE_MOVING;
follow_line_controller.best_value = WHITE_LIGHT_VALUE;
follow_line_controller.best_angle = getAngle( );
follow_line_controller.start_rotating_time = 0;
}
void changeState( int newState)
{
setSpeed(0, 0);
follow_line_controller.state = newState;
follow_line_controller.start_rotating_time = CurrentTick();
}
void follow_line( )
{
int currentValue = Sensor( LIGHT_SENSOR );
switch( follow_line_controller.state ) {
case STATE_MOVING:
//if white...
if( currentValue > BLACK_LIGHT_VALUE + BIAS ) {
changeState(STATE_ROTATING_LEFT);
follow_line_controller.best_value = currentValue;
follow_line_controller.best_angle = getAngle();
}else
setSpeed(SPEED, 0);
break;
case STATE_ROTATING_LEFT:
if( CurrentTick() - follow_line_controller.start_rotating_time < ROTATING_TIME ) {
setSpeed(0, OMEGA);
if (currentValue < follow_line_controller.best_value){
follow_line_controller.best_value = currentValue;
follow_line_controller.best_angle = getAngle();
}
}
else
changeState(STATE_ROTATING_RIGHT);
break;
case STATE_ROTATING_RIGHT:
if ( CurrentTick() - follow_line_controller.start_rotating_time < ROTATING_TIME ){
setSpeed(0, -OMEGA);
if (currentValue < follow_line_controller.best_value){
follow_line_controller.best_value = currentValue;
follow_line_controller.best_angle = getAngle();
}
}
else
changeState(STATE_ROTATING_TO_BEST);
break;
case STATE_ROTATING_TO_BEST:
setAngle(follow_line_controller.best_angle, OMEGA);
changeState(STATE_MOVING);
break;
};
}
#endif