/****************************************************
 *  LineTracer.c                                         
 *  Created on: 2012/04/17 11:50:48                      
 *  Implementation of the Class LineTracer       
 *  Original author: hiroyuki.watanabe                     
 ****************************************************/

#include "ecrobot_interface.h"
#include <string.h>
#include <stdlib.h>

#include "LineTracer.h"
#include "balancer.h"
#include "InputPort.h"
#include "OutputPort.h"
#define TURN 50
#define NORMAL 50
#define BATT 8000
#define THRESHOLD  560
#include "GyroSensor.h"
#include "Motor.h"
#include "LightSensor.h"

// 前方宣言
static IS_ONLINE LineTracer_judgeLine(LineTracer* this);
static void LineTracer_calcDirection(LineTracer* this, IS_ONLINE isOnline);
static void LineTracer_run(LineTracer* this);


void LineTracer_init(LineTracer* this)
{
	this->speed = NORMAL;
	this->direction = LEFT;
}

// ライントレースを行う
void LineTracer_traceLine(LineTracer* this)
{
	IS_ONLINE judgement = LineTracer_judgeLine(this);
	LineTracer_calcDirection(this, judgement);
	LineTracer_run(this);
}

// 路面の色を判断する
static IS_ONLINE LineTracer_judgeLine(LineTracer* this)
{
	unsigned int brightness = LightSensor_getBrightness(this->lightSensor);
	
	IS_ONLINE result;
	if(brightness < THRESHOLD)
	{
		result = ON_LINE;
	}
	else
	{
		result = OUT_OF_LINE;
	}
	
	return result;
}

static void LineTracer_calcDirection(LineTracer* this, IS_ONLINE isOnline)
{
	if(isOnline == ON_LINE)
	{
		this->direction = LEFT;
	}
	else
	{
		this->direction = RIGHT;
	}
}

static void LineTracer_run(LineTracer* this)
{
	unsigned int gyro;
	unsigned int gyro_offset;
	long motor_ang_r;
	long motor_ang_l;
	int turn;
	char pwm_r;
	char pwm_l;
	
	gyro_offset = GyroSensor_getOffset(this->gyroSensor);
	gyro = GyroSensor_getAngularVelocity(this->gyroSensor);
	
	motor_ang_l = Motor_getAngle(this->leftMotor);
	motor_ang_r = Motor_getAngle(this->rightMotor);
	
	if (this->direction == LEFT)
	{
		turn = -TURN;
	}
	else
	{
		turn = -TURN;
	}	
	
	balance_control(
		(float)this->speed, (float)turn,
		(float)gyro, (float)gyro_offset,
		(float)motor_ang_l, (float)motor_ang_r,
		BATT,
		(signed char*)&pwm_l, (signed char*)&pwm_r
	);
	
	Motor_rotate(this->leftMotor,pwm_l);
	Motor_rotate(this->rightMotor,pwm_r);
} 




