/****************************************************************
** 文件名：motor.c   電機驅動函數
****************************************************************/
 #include "config.h"
 /*#define T0_EN TIMSK |= (1<<OCIE0)|(1<<TOIE0)
 #define T0_UEN TIMSK &=~ (1<<OCIE0)|(1<<TOIE0)
 #define T0_start TCCR0 |= (1<<WGM00)|(1<<WGM01)|(1<<COM01)|0x04;
 //0x04 0100B 代表256預分頻
 #define T0_stop TCCR0 = 0x00
 
 #define T2_EN TIMSK |= (1<<OCIE2)|(1<<TOIE2)
 #define T2_UEN TIMSK &=~ (1<<OCIE2)|(1<<TOIE2)
 #define T2_start TCCR2 = (1<<WGM20)|(1<<WGM21)|(1<<COM21)|0x06
 #define T2_stop TCCR2 = 0x00
 
 
 
/************************左電機動作*****************************/
//左電機前進
void motor_left_forward(uint8 speed)
{
 motol_uen1;
 motol_en2;
 if(speed!=0) //加入調速指令
  {
   OCR0=speed;
  }
 T0_EN;
}
//左電機后退
void motor_left_backward(uint8 speed)
{
 motol_en1;
 motol_uen2;
 if(speed!=0) //加入調速指令
  {
   OCR0=speed;
  }
 T0_EN;
}
//左電機速度設定
void motor_left_speed_set(uint8 speed)
{
 if(speed!=0)
  {
   OCR0=speed;
  } 
}
//左電機滑行
void motor_left_stop(void)
{
 motol_uen1;
 motol_uen2;
 T0_UEN;
}
//左電機急停
void motor_left_quick_stop(void)
{
 motol_en1;
 motol_en2;
 T0_UEN;
}

/*************************右電機動作*****************************/
//右電機前進
void motor_right_forward(uint8 speed)
{
 motor_en1;
 motor_uen2;
  if(speed!=0) //加入調速指令
  {
   OCR2=speed;
   while(ASSR&(1<<TCR2UB)==1) ; //異步操作需要等待 OCR2寫入完畢
  }
//T2_EN;
}
//右電機后退
void motor_right_backward(uint8 speed)
{
 motor_uen1;
 motor_en2;
 if(speed!=0) //加入調速指令
  {
   OCR2=speed;
   while(ASSR&(1<<TCR2UB)==1) ; //異步操作需要等待 OCR2寫入完畢
  }
//T2_EN;
}
//右電機速度設定
void motor_right_speed_set(uint8 speed)
{
 if(speed!=0)
  {
   OCR0=speed;
   while(ASSR&(1<<TCR2UB)==1) ; //異步操作需要等待 OCR2寫入完畢
  } 
}
//右電機滑行
void motor_right_stop(void)
{
 motor_uen1;
 motor_uen2;
T2_UEN;
}

//右電機急停
void motor_right_quick_stop(void)
{
 motor_en1;
 motor_en2;
 T2_UEN;
}

/**********************小車動作*****************************/

void straight(void) //直行
{//motor_right_quick_stop();
 motor_left_forward(0x01);
 motor_right_forward(0x01);

}

void turn_left(void) //左轉
{
motor_left_forward(0xB0);
 motor_right_forward(0x05);
}

void turn_right(void) //右轉
{
 motor_left_forward(0x05);
 motor_right_backward(0xB0);
}

void straight_back(void) //直回
{//motor_right_quick_stop();
 motor_left_backward(0x01);
motor_right_backward(0x01);
}

void stop(void)       //急停
{
 motor_left_quick_stop();
 motor_right_quick_stop();
}

void mic_turn_right(void) //向右微調
{
 motor_left_forward(0xa0);
 motor_right_backward(0x30);
}

void mic_turn_left(void) //向左微調
{
motor_left_forward(0xa0);
 motor_right_forward(0x30);
}

void flag_test(void)  //測試小車狀態
{
 uint8 temp;
 temp = PINA&0x0f; //PINA的低四位
 switch(temp)
  {
   case 0x0f:
       flag = 0;  //未偏
	   break;
   case 0x01:
   case 0x06:
   case 0x07:
   case 0x08:
   case 0x0a:
   case 0x0e:
       flag = 1;  //左偏
	   break;
   case 0x02:
   case 0x04:
   case 0x05:
   case 0x09:
   case 0x0b:
   case 0x0d:
       flag = 2;  //右偏
	   break;
   case 0x0c:
       flag = 3;  //前兩出線
	   break;
   case 0x03:
       flag = 4; //后兩出線
	   break;
   case 0x00:
       flag = 5; //脫軌
	   break;
   default:
       break;
  }
}