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

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

#include "DriveUnit.h"
#include "balancer.h"
#define BATT 8000
#define TURN 50
#include "GyroSensor.h"
#include "Motor.h"

void DriveUnit_drive(DriveUnit* this, DIRECTION direction, unsigned int speed)
{
	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 (direction == LEFT)
	{
		turn = -TURN;
	}
	else
	{
		turn = -TURN;
	}	
	
	balance_control(
		(float)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);
} 




