/*******************************************************************************
(C) 2011 Seung-Won Lee http://whoisit.tistory.com SoftWareYi@gmail.com
코너링 램프 컨트롤러
@File CorneringLampControl.c
휠각도 센서를 직접적으로 사용 할 수 없으므로, Hall 센서를 이용하여
스티어링 휠 회전 각도를 측정하여 코너링 램프를 조절 하게 된다.
스티어링 휠은 좌우로 1회전 반을 하기 때문에 센서로 절대값을 측정 할 수 없다
그러므로 회전 판단과 에러 보정을 위해 Automata 형식의 state machine 을 채택
20km (7 pulse/sec) 까지는 80 도에서 On
20km 초과 부터 38 도에서 On
100km (35 pulse) 초과 부터 전체 Off
100km 초과 Off에서 95km (33 pulse) 진입시에 다시 On
Turn on 상태에서 일정시간 초과시에는 auto off 하여 error 상황 등에서 지나치게
오래 점등 되지 않도록 한다.
*******************************************************************************/
#include <inttypes.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay_basic.h>
#include <limits.h>
//******************************************************************************
// Configurations
//******************************************************************************
#define USE_POLLING_SENSOR_CHECKING 1 // Interrupt가 아닌 폴링 방식으로 센서값 읽기 루틴 사용
#define SENSOR_PORT PINC // ATMega8 PortC[0..5] 를 센처 입력 처리.
//******************************************************************************
// Enumerations
//******************************************************************************
typedef enum { FALSE, TRUE } BOOL;
// 센서 구별값 정의
enum { VOID, SENSOR_1, SENSOR_2, SENSOR_3, SENSOR_4, SENSOR_5, SENSOR_6 };
#define LEFT80 SENSOR_1
#define LEFT38 SENSOR_2
#define LEFT26 SENSOR_3
#define RIGHT26 SENSOR_4
#define RIGHT38 SENSOR_5
#define RIGHT80 SENSOR_6
// State 정의
enum { L440, L398, L386, L334, L322, L280, L80, L38, L26,
ZERO,
R26, R38, R80, R280, R322, R334, R386, R398, R440 };
// Cornering lamp 조절
enum { OFF, ON_LEFT, ON_RIGHT, ON_LEFT_RIGHT };
// 현재 속도 상태
enum { SPEED_LOW, SPEED_HIGH, SPEED_OVER }; // Cornering lamp On 조건 (LOW: 80 도 On / HIGH: 28 도 On / OVER: 전체 Off)
enum { SPPED_0_20km, SPPED_20_23km, SPPED_23_95km, SPPED_95_100km, SPPED_100_OVERkm };
//******************************************************************************
// Global variables
//******************************************************************************
char RotateState = ZERO;
char LastRotateState = ZERO;
unsigned char speedPulseCount = 0;
unsigned char lastSpeedPulseCount = 0;
char SpeedState = SPPED_0_20km;
char LampControlSpeed = SPEED_LOW;
unsigned long int eleapsedTimeSet = 0; // 최초 동작이후 경과 시간(초) - 범위 초과시 round off됨.
/*******************************************************************************
현재 time count 돌려주기
*******************************************************************************/
unsigned long int GetCurrentTime()
{
return eleapsedTimeSet;
}
/*******************************************************************************
저장한 time 기준으로 몇초 경과 여부 확인
*******************************************************************************/
int IsTimeOver( unsigned long int baseTime, unsigned int elapseTime )
{
if ( eleapsedTimeSet > baseTime )
return ( (eleapsedTimeSet - baseTime) > elapseTime );
else
return ( (baseTime - baseTime) > elapseTime );
}
/*******************************************************************************
Pulse count 에 따라 개별 속도 지점 상태를 갱신 한다
*******************************************************************************/
void SpeedStateTransition( unsigned char speedPulse )
{ // L LH H HO O
int i; // 0-20 20-23 23-95 95-100 100-300
unsigned char pulseForSpeedIndex[] = { 0, 7, 8, 33, 35, 255 };
unsigned char currentSpped = SPPED_0_20km;
for ( i = 0; i<sizeof(pulseForSpeedIndex)/sizeof(pulseForSpeedIndex[0])-1; i++ )
if (( pulseForSpeedIndex[i] <= speedPulse ) && ( speedPulse < pulseForSpeedIndex[i+1] ))
{
currentSpped = i;
break;
}
switch (currentSpped)
{
case SPPED_0_20km:
switch (SpeedState)
{
//case SPPED_0_20km: // Nothing todo
case SPPED_20_23km:
case SPPED_23_95km:
case SPPED_95_100km: // 1초 만에 100km->23km 는 불가능 하겠지만... 안전 차원 (이하 동일)
case SPPED_100_OVERkm:
LampControlSpeed = SPEED_LOW;
break;
}
SpeedState = SPPED_0_20km;
break;
case SPPED_20_23km:
switch (SpeedState)
{
case SPPED_0_20km: // 인접 상태에서 전이 된 경우 이전 state 유지
case SPPED_20_23km:
case SPPED_23_95km:
// Do noting
break;
case SPPED_95_100km:
case SPPED_100_OVERkm:
LampControlSpeed = SPEED_HIGH;
break;
}
SpeedState = SPPED_20_23km;
break;
case SPPED_23_95km:
switch (SpeedState)
{
case SPPED_23_95km:
break;
case SPPED_0_20km:
case SPPED_20_23km:
case SPPED_95_100km:
case SPPED_100_OVERkm:
LampControlSpeed = SPEED_HIGH;
break;
}
SpeedState = SPPED_23_95km;
break;
case SPPED_95_100km:
switch (SpeedState)
{
case SPPED_23_95km:
case SPPED_95_100km:
case SPPED_100_OVERkm:
break;
case SPPED_0_20km:
case SPPED_20_23km:
LampControlSpeed = SPEED_HIGH;
break;
}
SpeedState = SPPED_95_100km;
break;
case SPPED_100_OVERkm:
switch (SpeedState)
{
case SPPED_100_OVERkm:
break;
case SPPED_0_20km:
case SPPED_20_23km:
case SPPED_23_95km:
case SPPED_95_100km:
LampControlSpeed = SPEED_OVER;
break;
}
SpeedState = SPPED_100_OVERkm;
break;
}
}
/*******************************************************************************
Timer interrupt - 비교 대상과 일치하였을때 인터럽트
1024 분주 카운팅에 125회째 인터럽트가 발생 16,000,000 / 1024 / 125 = 125 Hz
*******************************************************************************/
SIGNAL( SIG_OUTPUT_COMPARE2 )
{
volatile static unsigned char timer_count = 0;
//if ( 1 == timer_count || 65 == timer_count )
// PORTD ^= 0x40; // Toggle on PD5
if ( 65 <= timer_count )
PORTD |= 0x40; // Toggle on PD5
else
if ( 10 >= timer_count )
PORTD &= ~0x40; // Toggle on PD5
if(timer_count >= 125){ // 125번 인터럽트가 발생하면(1초마다)
SpeedStateTransition( speedPulseCount );
timer_count=0;
lastSpeedPulseCount = speedPulseCount;
speedPulseCount = 0;
++eleapsedTimeSet; // Global elapsed time
}
++timer_count; //인터럽트 발생 횟수 기록
}
/*******************************************************************************
센서값을 읽어 Active 센서를 리턴
6개 중 한개만 Active bit (1) 고 나머지는 bit(0)을 유지 함. -> 어느 센서인지 리턴
5V TTL -> 12V relay 제어가 필요 하므로 NPN TR을 이용한다. 즉 5V 출력으로 active
*******************************************************************************/
void SetCorneringLamp( char state )
{
switch ( state )
{
case OFF:
PORTD &= 0xFC; // ******00 == off
break;
case ON_LEFT:
PORTD = (PORTD & 0xFC) | 0x01; // 0 위치 bit을 On
break;
case ON_RIGHT:
PORTD = (PORTD & 0xFC) | 0x02; // 1 위치 bit을 On
break;
case ON_LEFT_RIGHT:
PORTD = (PORTD & 0xFC) | 0x03; // 하위 두 bit On
break;
}
}
/*******************************************************************************
센서값을 읽어 Active 센서를 리턴
6개 중 한개만 Active bit (0) 고 나머지는 bit(1)을 유지 함. -> 어느 센서인지 리턴
*******************************************************************************/
char GetActiveSensor()
{
char result = VOID;
char activeSensorBit = (~SENSOR_PORT) & 0x3F; // Pull up이므로 기본 1 / 선택 센서만 0, 반전 후 하위 6bit 만
switch (activeSensorBit)
{
case 0x01 :
result = LEFT80;
break;
case 0x02 :
result = LEFT38;
break;
case 0x04 :
result = LEFT26;
break;
case 0x08 :
result = RIGHT26;
break;
case 0x10 :
result = RIGHT38;
break;
case 0x20 :
result = RIGHT80;
break;
}
return result;
}
/*******************************************************************************
새로 입력 받은 SENSOR 위치를 확인 해서 STATE를 변동 처리
6개 센서 입력에 대해 이전 state를 확인하여 회전상태를 갱신 하게 한다.
또한 이전의 잘못된 판단을 확인하여 state를 보정하는 역할도 하게 된다.
case 우측 부분에 Jumping 부분은 바로 이전 state가 check 되지 못한 error상황에서
에러를 복구하여 state전환을 허용 하기 위한 case 임.
*******************************************************************************/
void RotateStateTransition( char NewSensor )
{
LastRotateState = RotateState;
switch ( NewSensor )
{
case LEFT80 :
switch ( RotateState )
{
case L398: case L386://Jumping
RotateState = L440;
break;
case L280: case L322://Jumping
case ZERO:
case L38: case L26://Jumping
RotateState = L80;
break;
case R80: case R38://Jumping
case R322: case R334://Jumping
RotateState = R280;
break;
// Miss zero base case - 왼쪽 한바퀴 돌린 상황에서 시동걸었을 경우
// 이후 우회전하여 0로 맞추었으나, 1.5 회전 이상 돌아가는 있을 수 없는 경우 보정 처리 함.
case R440: case R398://Jumping
RotateState = R280;
break;
}
break;
case LEFT38 :
switch ( RotateState )
{
case L440:
case L386: case L334://Jumping
RotateState = L398;
break;
case L80: case L280://Jumping correction
case L26: case R26://Jumping correction
case ZERO:
RotateState = L38;
break;
case R280: case R80://Jumping
case R334: case R386://Jumping
RotateState = R322;
break;
}
break;
case LEFT26 :
switch ( RotateState )
{
case L398: case L440://Jumping
case L334: case L322://Jumping
RotateState = L386;
break;
case L38: case L80://Jumping
case ZERO:
case R26: case R38://Jumping
RotateState = L26;
break;
case R322: case R280://Jumping
case R386: case R398://Jumping
RotateState = R334;
break;
}
break;
case RIGHT26 :
switch ( RotateState )
{
case L386: case L398://Jumping
case L322: case L280://Jumping
RotateState = L334;
break;
case L26: case L38://Jumping
case ZERO:
case R38: case R80://Jumping
RotateState = R26;
break;
case R334: case R322://Jumping
case R398: case R440://Jumping
RotateState = R386;
break;
}
break;
case RIGHT38 :
switch ( RotateState )
{
case L334: case L386://Jumping
case L280: case L80://Jumping
RotateState = L322;
break;
case R26: case L26://Jumping
case R80: case R280://Jumping
case ZERO:
RotateState = R38;
break;
case R386: case R334://Jumping
case R440:
RotateState = R398;
break;
}
break;
case RIGHT80 :
switch ( RotateState )
{
case L322: case L334://Jumping
case L80: case L38://Jumping
RotateState = L280;
break;
case R38: case R26://Jumping
case R280: case R322://Jumping
case ZERO:
RotateState = R80;
break;
case R398: case R386://Jumping
RotateState = R440;
break;
// Miss zero base case - 오르쪽 한바퀴 돌린 상황에서 시동걸었을 경우
// 이후 좌회전하여 0로 맞추었으나, 1.5 회전 이상 돌아가는 있을 수 없는 경우 보정 처리 함.
case L440: case L398://Jumping
RotateState = L280;
break;
}
break;
}
}
/*******************************************************************************
Sensor polling checking routine
*******************************************************************************/
#ifdef USE_POLLING_SENSOR_CHECKING // Enable only for non-interrupt schematic
void PollingCheckAndSetRotateState()
{
static char OldActiveSensor = 0; // For sensor value chaning
char ActiveSensor = GetActiveSensor();
if ( OldActiveSensor != ActiveSensor )
{
PORTD ^= 0x20; // Toggle on PD6
OldActiveSensor = ActiveSensor;
RotateStateTransition( ActiveSensor );
}
}
#endif
/*******************************************************************************
Reverse gear position check
return TRUE for Reverse state.
*******************************************************************************/
BOOL IsReversGear()
{
return ( 0 == (PIND & 0x10) );
}
/*******************************************************************************
Set Lamp state for Reverse gear state. (Left to right Right to Left)
*******************************************************************************/
void SetCorneringLampForReverseGear( char LampState )
{
switch ( LampState )
{
case ON_LEFT:
SetCorneringLamp( ON_RIGHT ); // 임시로 LAMP 방향을 바꿈
break;
case ON_RIGHT:
SetCorneringLamp( ON_LEFT ); // 임시로 LAMP 방향을 바꿈
break;
}
}
/*******************************************************************************
INT0 (PORT D2) - 스티어링휠 회전 감지 스위치 변동 핸들러
*******************************************************************************/
ISR( INT0_vect , ISR_BLOCK )
{
#ifndef USE_POLLING_SENSOR_CHECKING // Enable code for interrupt enabled schematic
PORTD ^= 0x20; // Toggle on PD6
char ActiveSensor = GetActiveSensor();
RotateStateTransition( ActiveSensor );
#endif
}
/*******************************************************************************
INT1 (PORT D3) - 차량 속도 센서 핸들러
초당 14 pulse == 40 km 35 pulse = 100km
*******************************************************************************/
ISR( INT1_vect , ISR_BLOCK )
{
PORTD ^= 0x80; // Toggle on PD7
++speedPulseCount; // Increase speed pulse count
}
/*******************************************************************************
인터럽 관련 초기화
INT0 : 하강 edge에만
INT1 : 하강 edge에만
*******************************************************************************/
void InitInterrupt()
{
cli();
//----- INT0 ----------
GICR |= 1<<INT0 | 1<<INT1; // INT0, INT1 enable
MCUCR = 1<<ISC11 | // INT1 하강엣지에 인트럽트 발생
1<<ISC01; // INT0 하강엣지에 인트럽트 발생
sei();
}
/*******************************************************************************
시간 측정용 Timer 초기화
Atmaega8 0번 Timer는 비교기 사용이 불가 -> 2번(8bit) 사용으로 변경
*******************************************************************************/
void InitTimer(void)
{
cli();
TCCR2 = 0x0F; // ----1111 CTC모드(TOP==OCR2)로 조정하고 클럭을 1024분주하기
// → 소스클럭을 15625hz로 만들기 ( 16,000,000 hz/1024 = 15,625 )
// 15625hz/x=125hz, x=125, 0~124번 카운트 필요
OCR2 =124; // 카운트할 숫자 설정(125hz를 만들기 위해)
TCNT2 = 0x00; // 카운터를 초기화 한다음
TIMSK = 0x80; // CTC모드로 카운터인터럽트 허용
sei();
}
/*******************************************************************************
인터럽 관련 초기화
INT0 : 하강 edge에만
INT1 : 하강 edge에만
*******************************************************************************/
void InitPort()
{
DDRC = 0x00; // PORT C는 입력 (PC0~PC5 까지 6개)
PORTC = 0xFF; // Pull up
DDRD = 0xE3; // PORT D: 11100011 , INT0, INT1, PD4 만 입력으로 두고, 나머지는 출력 설정
PORTD = 0x10; // Turn off all out, Internal pullup for D4 - Reverse gear input (0 for R)
}
/*******************************************************************************
Main
RotateState는 Interrut 방식으로 갱신 될 수 있기에 RotateState를 global 변수로 갱신
*******************************************************************************/
int main( void )
{
unsigned long int TimeCount = 0;
unsigned long int BaseTimeOfTurnOn = GetCurrentTime();
char LampState = OFF;
char LastLampState = OFF;
BOOL bForceTurnOffed = FALSE;
BOOL OnReverseGear;
BOOL LastOnReverseGear = FALSE;
InitPort();
InitInterrupt();
InitTimer();
SetCorneringLamp( OFF ); // 최초 꺼진 상태
while(1)
{
#ifdef USE_POLLING_SENSOR_CHECKING // Polling logic for non-interrupt
PollingCheckAndSetRotateState();
#endif
//========== 차량 속도를 확인 하여 좌/우 회전 각도에 따라 램프 좌우 조작.===================
switch (LampControlSpeed)
{
case SPEED_LOW: // 저속에서는 좌/우 80도 회전 부터 turn on
if ( RotateState <= L80 )
LampState = ON_LEFT;
else
if ( R80 <= RotateState)
LampState = ON_RIGHT;
break;
case SPEED_HIGH: // 고속에서는 좌/우 38도 회전 부터 turn on
if ( RotateState <= L38 )
LampState = ON_LEFT;
else
if ( R38 <= RotateState)
LampState = ON_RIGHT;
break;
case SPEED_OVER: // 100km 이상 부터는 Off
LampState = OFF;
break;
}
if ( L26 <= RotateState && RotateState <= R26 ) // 좌/우 26도로 되돌아 오면 turn off
LampState = OFF;
//^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
//********** 후진기어인 경우 Left/Right 바꾼다 - 단 LampState 변수는 유지하고 출력만 임시 설정
// 후진 기어 변동시에만 수행되며 좌우와 달리 현재 state를 유지하고 임시로 출력만 변경
OnReverseGear = IsReversGear();
if ( LastOnReverseGear != OnReverseGear )
{
LastOnReverseGear = OnReverseGear;
if ( OnReverseGear )
SetCorneringLampForReverseGear( LampState );// R 상태에서 Lamp 출력
else
SetCorneringLamp( LampState ); // R 기어가 아니면 원상태로 원복
}
//^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
//********** Lamp state 변동시에만 램프 점등도 변경 처리 ***********************************
if ( LastLampState != LampState ) // Lamp 점등 여부 변경시에
{
if ( OFF != LampState ) // OFF -> ON 으로 변동시에
{
BaseTimeOfTurnOn = GetCurrentTime(); // ON 시점 시간값을 저장
bForceTurnOffed = FALSE; // 강제 Off 상태가 아님 설정(Off되기 위한 초기화)
}
if ( OnReverseGear && (OFF != LampState) ) // Gear Reverse state then exchange left/right set.
SetCorneringLampForReverseGear( LampState );// R 상태에서 Lamp 출력
else
SetCorneringLamp( LampState ); // Normal case
LastLampState = LampState;
}
//^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
//********** 장시간 Lamp 켜짐을 방지 처리
if ( (!bForceTurnOffed) && (LastLampState != OFF) )
if ( IsTimeOver( BaseTimeOfTurnOn, 60*5 ) ) // 지정 시간(초)가 지나도록 ON 이라면 강제 off한다.
{
LastLampState = LampState = OFF;
SetCorneringLamp(OFF); // 강제 꺼짐, 이후 각도/속도 변동에 의한 꺼짐-켜짐 동작이 이루어 져야 다시 켜질 수 있음
bForceTurnOffed = TRUE; // 강제 꺼짐 상태로 표식, 이후 진입 방지.
}
//^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
if ( ++TimeCount >= ULONG_MAX ) // Counting
TimeCount = 0; // Reset
}
return 0;
}
CorneringLampControl.c
