드론/로봇/라인트레이서 HOT라인트레이서 스위치 없애는법

 #include <mega128.h>

// Alphanumeric LCD Module functions
#asm
   .equ __lcd_port=0x18 ;PORTB
#endasm
#include <lcd.h>  
#include <delay.h>
#include <stdio.h>
#include <stdlib.h>
#include "acc.h"
#include "handle.h"
 

#define TRUE  1
#define FALSE  0
  
#define TIMER0_32PRESCALE_VALUE 2   // (1/16MHz)*32*1000000

#define SENSOR_ON_TIME   40  // 40us 후에 센서를 끔.
#define SENSOR_OFF_TIME  70  // 트리거 신호의 주기 = 240us

#define FULL_STEP   3/*2상*/  /* 1-2상여자 = 7 */
#define ZERO    0
#define RIGHT  1
#define LEFT  2
#define BOTH  3   

#define FORWARD  0x03  /* 모터 방향 설정 bit1=왼쪽, bit0=오른쪽 */

#define MOTOR_PORT    PORTD

#define SPEED_LIMIT  300
#define SPEED_START  0

#define motor_disable()  MOTOR_PORT = 0x00
#define enable()            #asm("sei") 
#define disable()           #asm("cli") 

#define sen_on()  PORTA=0x18
#define sen_off()  PORTA=0x00

#define SENSOR_MODE_ON  1
#define SENSOR_MODE_OFF  2

#define SENSOR_ADJUST   50   

#define LEFT2  1
#define LEFT1   2
#define LEFT0   3

#define RIGHT2  6
#define RIGHT1  5
#define RIGHT0  4

#define LIGHT 0
#define COMPARE 1
#define MODE LIGHT
 
#define HANDLE_RATE_CUR  70
#define HANDLE_RATE_OLD     30

typedef  unsigned char byte;
typedef  unsigned int word;
typedef  unsigned long dword;

        
byte mode; 
byte buzzer_flag;  
 
// sensor part
 
byte sensor_mode; 
word sensor_input[6];
word sensor_border; // 비교기식일때 on/off 경계선
int sensor_adjust; // 광량식일때 좌우 대칭 센서 보정
byte sensor_cnt=0;

// motor part
const byte r_phase_table[] = {0x50, 0x60, 0xA0, 0x90};
const byte l_phase_table[] = {0x05, 0x06, 0x0A, 0x09};  

byte step_status;    // 주행 방향
word speed_limit; // 최고 제한속도
word time_r,time_l; // 타이머 주기
word acc_index;
byte l_phase_index, r_phase_index; // 상 인덱스
 
// adjust
int handle_adjust=0; 
long handle_adjust_cur=0;
long handle_adjust_old=150;
int handle_gain=4;
  
// general part
byte lcd_buf[16];

word i,j;

void port_init(void)
{     
 // External SRAM page configuration:
 //              -              / 0000h - 7FFFh
 // Lower page wait state(s): None
 // Upper page wait state(s): None
 // MCUCR=0x80;// set PC7 as output. PC7 is A15 and is used to enable nCE of SRAM.
 // XMCRA=0x00;
 
 MCUCR=0x00;// set PC7 as output. PC7 is A15 and is used to enable nCE of SRAM.
 XMCRA=0x00;
 
 DDRA = 0x18;
 DDRB = 0xff; // lcd, dummy led
 DDRD = 0xff; // step motor output 
 DDRE = 0x08;    // joystick switch input, buzzer output(PORTE3)
 DDRF = 0x00; // sensor input
 
 
 SFIOR=0x00;
}
 
void tlcd_init(void)
{
 // LCD module initialization
 lcd_init(8);  
 lcd_gotoxy(0,0);
 lcd_putsf("tracer");
 lcd_gotoxy(0,1);
 lcd_putsf("akma 1.1");      
}
    
  
byte adc_on = FALSE;                       
#define ADC_VREF_TYPE 0x40
//ADC initialize
// Conversion time: 1uS
void adc_init(void)
{
 ADCSRA = 0x00; //disable adc
  ADMUX = 0x00; //select adc input 0
  ACSR  = 0x80;
  //ADCSRA = 0x89;   // 1us 검은색 수광은 이것도 되는데..
  ADCSRA = 0x8E; // 이러케 하면 adc가 먼저 win
 
  for(i=0;i<6;i++)
 {
  sensor_input[i]=0;
    }
}
void display_handle()
{

   sprintf(lcd_buf, "handle");
   lcd_gotoxy(0,0);
      lcd_puts(lcd_buf);
 
   sprintf(lcd_buf, "%8d", handle_adjust);
      lcd_gotoxy(0,1);
      lcd_puts(lcd_buf);
         delay_ms(1000);

}
 
void display_sensor()
{
  for(i=0;i<8;i+=2)
  { 
   sprintf(lcd_buf, "%d : %4d", i, sensor_input[i]);
   lcd_gotoxy(0,0);
      lcd_puts(lcd_buf);
 
   sprintf(lcd_buf, "%d : %4d",i+1, sensor_input[i+1]);
      lcd_gotoxy(0,1);
      lcd_puts(lcd_buf);
         delay_ms(1000);
  }
}

void display_sensor_compare()
{ 
 if( sensor_input[LEFT2] > sensor_border)
  {  
    lcd_buf[0] = '1';
 }else
 {
   lcd_buf[0] = '0';
 }
 
 if( sensor_input[LEFT1] > sensor_border)
 {  
    lcd_buf[1] = '1';
 }else
 {
   lcd_buf[1] = '0';
 }
 
 if( sensor_input[LEFT0] > sensor_border)
 {  
    lcd_buf[2] = '1';
 }else
 {
   lcd_buf[2] = '0';
 } 
 
     if( sensor_input[RIGHT0] > sensor_border)
 {  
    lcd_buf[3] = '1';
 }else
 {
   lcd_buf[3] = '0';
 } 
 
 if( sensor_input[RIGHT1] > sensor_border)
 {  
    lcd_buf[4] = '1';
 }else
 {
   lcd_buf[4] = '0';
 } 

 if( sensor_input[RIGHT2] > sensor_border)
 {  
    lcd_buf[5] = '1';
 }else
 {
   lcd_buf[5] = '0';
 } 
 
 lcd_gotoxy(0,1);
 lcd_puts(lcd_buf);
      
 sprintf(lcd_buf, "compare ");
 lcd_gotoxy(0,0);
 lcd_puts(lcd_buf);
  
 delay_ms(500);
 
}
 
 
// 광량식
void auto_detect()
{  
 long sensor_temp = 0;
                             
   TCNT0 = 0xff; 
 TCCR0 = 0x03; //start timer, prescale 32   
 TIMSK = 0x01; //timer 0 interupt
 enable(); // interrupt enable
 
   for(i=0;i<2;i++)
   {
     delay_ms(50);   // 센서값을 읽을 시간을 벌어줌 
  sensor_temp = (long)sensor_temp + (long)(sensor_input[1]+sensor_input[2]+sensor_input[3]-sensor_input[4]-sensor_input[5]-sensor_input[6]);
    } 
        
    sensor_adjust = (int)(sensor_temp/2);
 
 lcd_gotoxy(0,0);
 lcd_putsf("auto    ");
 
 lcd_gotoxy(0,1);
 lcd_putsf("detect  ");
 delay_ms(200);
 
 
 sprintf(lcd_buf, "adjust ");
  lcd_gotoxy(0,0);
  lcd_puts(lcd_buf);
   
 sprintf(lcd_buf, "%8d", sensor_adjust);
  lcd_gotoxy(0,1);
  lcd_puts(lcd_buf);
  delay_ms(300); 
  
   sensor_adjust = -20;//(int)(sensor_temp/10);
  
}      

//비교식
void auto_detect_compare()
{  
 long sensor_temp = 0;
                             
   TCNT0 = 0xff; 
 TCCR0 = 0x03; //start timer, prescale 32   
 TIMSK = 0x01; //timer 0 interupt
 enable(); // interrupt enable
 
   for(i=0;i<10;i++)
   {
     delay_ms(100);   // 센서값을 읽을 시간을 벌어줌 
  for(j=1;j<7;j++)    // 1,2,3센서(white) vs 4,5,6센서(black)
      sensor_temp += sensor_input[j];
    } 
   
    sensor_border = (word)((float)sensor_temp/(float)60 /* 6x10 */);
      
 lcd_gotoxy(0,0);
 lcd_putsf("auto    ");
 
 lcd_gotoxy(0,1);
 lcd_putsf("detect  ");
 delay_ms(200);
 
 
 sprintf(lcd_buf, "border ");
  lcd_gotoxy(0,0);
  lcd_puts(lcd_buf);
   
 sprintf(lcd_buf, "%8d", sensor_border);
  lcd_gotoxy(0,1);
  lcd_puts(lcd_buf);
  delay_ms(1000); 
}
 

void system_init(void)
{
 disable();       

 port_init();
 //센서를 보호하기 위해 부팅하자마자 바로 꺼준다..점등식으로만 써야하므로..
 sen_off();
 //시끄러우니 일단 끄자..

 tlcd_init();
 motor_disable();  
 adc_init();
 delay_ms(1); 
 sensor_mode = SENSOR_MODE_OFF;
 
 enable();  
}    

void start_motor(void)
{
    disable();
  
 step_status = BOTH;
 
 l_phase_index = r_phase_index = 0;
 speed_limit = SPEED_LIMIT; 
 acc_index = SPEED_START; 

 OCR1AH = (acc_tbl[acc_index]>>8)&0xff;   // register high byte
 OCR1AL = acc_tbl[acc_index]&0xff;       // register low byte
 
 OCR3AH = (acc_tbl[acc_index]>>8)&0xff;   // register high byte
 OCR3AL = acc_tbl[acc_index]&0xff;       // register low byte
 
 TCCR1B = 0x09;          //clock/1
 TCCR3B = 0x09;          //clock/1
     
 TIMSK = 0x11;
 ETIMSK = 0x10;
 
 enable();
}  
   
void beep(void)
{
 buzzer_flag =1;
 delay_ms(100);
 buzzer_flag =0;
 delay_ms(100);
}
 

void cal_trim(void)
{
  handle_adjust_cur =(long)( (((long)handle_adjust_old*(long)HANDLE_RATE_OLD) + (long)(sensor_input[1]+sensor_input[2]+sensor_input[3]-sensor_input[4]-sensor_input[5]-sensor_input[6]-sensor_adjust)*(long)HANDLE_RATE_CUR ) / (long)100);
    handle_adjust_old = handle_adjust_cur;
   handle_adjust=(int)(handle_adjust_cur*(int)handle_gain);
  
   //범위를 넘으면 시스템이 죽으므로 처리
  if(handle_adjust > 60)
   handle_adjust = 60;
 
  if(handle_adjust < -60)
   handle_adjust = -60;     

}  

void cal_trim_compare(void)
{
  if( sensor_input[LEFT2] > sensor_border)
  {  
    handle_adjust = 12;
 }
 else if( sensor_input[RIGHT2] > sensor_border)
 {
   handle_adjust = 12;
 }
 else if( sensor_input[LEFT1] > sensor_border)
 {
   handle_adjust = 8;
 }
 else if( sensor_input[RIGHT1] > sensor_border)
 {
     handle_adjust = -8;
 } 
 else if( sensor_input[LEFT0] > sensor_border)
 {
  handle_adjust = 4;
 }
 else if( sensor_input[RIGHT0] > sensor_border)
 {
  handle_adjust = -4;
 }else
 {
  handle_adjust = 1;
 }
 
 handle_adjust= (word)((float)handle_adjust * (float)handle_gain); 
      
}

void cal_speed(void)
{    
        // 가속 테이블은 140을 기준으로 141~~ 속도를 느리게
        // 0~139는 속도를 빠르게 해준다..   
 step_status = ZERO;
 acc_index++;
 if (acc_index > speed_limit) acc_index = speed_limit;    
 time_l=(word)((acc_tbl[acc_index]*handle[140+handle_adjust])>>8);   
 time_r=(word)((acc_tbl[acc_index]*handle[140-handle_adjust])>>8);
}               

// left motor phase
interrupt [TIM1_COMPA] void timer1_compa_isr(void)
{
 l_phase_index--;
 l_phase_index &= FULL_STEP;
   
   MOTOR_PORT =  r_phase_table[r_phase_index] | l_phase_table[l_phase_index] ;
   
   step_status |= LEFT;
 
  if(step_status == BOTH)
   {  

  #if(MODE==LIGHT)
     cal_trim();
  #elseif(MODE==COMPARE)
    cal_trim_compare();
   #endif
   
     cal_speed();
    
    }
              
  OCR1AH = ( time_l >> 8 ) & 0xff;
 OCR1AL = time_l & 0xff; 
}

// right motor phase
interrupt [TIM3_COMPA] void timer3_compa_isr(void)
{
   r_phase_index++;
 r_phase_index &= FULL_STEP;
 
    MOTOR_PORT =  r_phase_table[r_phase_index] | l_phase_table[l_phase_index] ;
 
 step_status |= RIGHT;
  if(step_status == BOTH)
    {
  #if(MODE==LIGHT)
     cal_trim();
  #elseif(MODE==COMPARE)
    cal_trim_compare();
   #endif
       
     cal_speed();
    }
   
  OCR3AH = ( time_r >> 8 ) & 0xff;
 OCR3AL = time_r & 0xff;  
}

  
// ADC interrupt service routine
interrupt [ADC_INT] void adc_isr(void)
{    
    sensor_input[sensor_cnt]= ADCW; 
 sensor_cnt++;
 if(sensor_cnt>=8) sensor_cnt=0;
 ADMUX =  sensor_cnt; // adc cheannel select               
}
  
// Timer 0 overflow interrupt service routine
interrupt [TIM0_OVF] void timer0_ovf_isr(void)
{    
 TCCR0 = 0x00; 
 if(sensor_mode == SENSOR_MODE_OFF)
 {
         ADCSRA|=0x40; //하얀색 수광용
         sen_on();
       sensor_mode = SENSOR_MODE_ON;
       TCNT0 = (byte)(0xff -  (SENSOR_ON_TIME/TIMER0_32PRESCALE_VALUE));   
      TCCR0 = 0x03; //start timer     
    
 }else if(sensor_mode ==  SENSOR_MODE_ON)
 {       
   //ADCSRA|=0x40; //검정색 수광용
      sen_off();
     
      sensor_mode = SENSOR_MODE_OFF;  
        TCNT0 = (byte)(0xff -  (SENSOR_OFF_TIME/TIMER0_32PRESCALE_VALUE));  
         TCCR0 = 0x03; //start timer          
 }else if(sensor_mode== (SENSOR_MODE_OFF+1))   
  {
            lcd_gotoxy(0,1);
         lcd_putsf("SENSOR");
         disable();
 }
              
}

// Timer 2 overflow interrupt service routine
interrupt [TIM2_OVF] void timer2_ovf_isr(void)
{

}
      

void main(void)
{       
 system_init(); 
 #if(MODE==LIGHT)
    auto_detect();
 #elseif(MODE==COMPARE)
   auto_detect_compare(); 
  #endif
  
   start_motor();  

 enable(); // interrupt enable
 
 while (1)
 { 

          //display_handle();
         //display_sensor();
        //display_sensor_compare();
 }  
}