ARM7 스텝모터 함수 푸는 중 백업용 작동 됨 가속시 문제 발생
#include "AT91SAM7S256.h"
#include "lib_AT91SAM7S256.h"
#include "OK7S256ads.h"
//---------------------전력제어 설정-------------------------------------------
#define PMC_SCER (*(volatile unsigned int *) 0xFFFFFC00)//시스템 클록 허용 레지스터
#define PMC_PCER (*(volatile unsigned int *) 0xFFFFFC10)// 주변장치 클록 허용 레지스터
//----------PWM-----------------------------------------------------------------
#define PWMC_MR (*(volatile unsigned int *) 0xFFFCC000)//분주비/디바이더 팩터 BASE Address
#define PWMC_ENA (*(volatile unsigned int *) 0xFFFCC004)//PWM ENABLE REGISTER
// 모터 1
#define PWMC_CMR0 (*(volatile unsigned int *) 0xFFFCC200) //CHANNEL MODE
#define PWMC_CDTYR0 (*(volatile unsigned int *) 0xFFFCC204)//CHANNEL DUTY CYCLE
#define PWMC_CPRDR0 (*(volatile unsigned int *) 0xFFFCC208)//CHANNEL PERIOD
//모터 2
#define PWMC_CMR1 (*(volatile unsigned int *) 0xFFFCC220)
#define PWMC_CDTYR1 (*(volatile unsigned int *) 0xFFFCC224)
#define PWMC_CPRDR1 (*(volatile unsigned int *) 0xFFFCC228)
//----ADC-------------------------------------------------------------
#define ADC_CR (*(volatile unsigned int *) 0xFFFD8000)
#define ADC_MR (*(volatile unsigned int *) 0xFFFFD004)
#define ADC_SR (*(volatile unsigned int *) 0xFFFFD81C)
#define ADC_CDR4 (*(volatile unsigned int *) 0xFFFFD83C)
#define ADC_CHER (*(volatile unsigned int *) 0xFFFD8010)
//-----------입출력 제어 PIO-------------------------------------------------------
#define PIO_SODR (*(volatile unsigned int *) 0xFFFFF430) //SET OUTPUT
#define PIO_CODR (*(volatile unsigned int *) 0xFFFFF434) //CLREAR OUTPUT
#define PIO_OER (*(volatile unsigned int *) 0xFFFFF410) //출력 방향 설정
#define PIO_PER (*(volatile unsigned int *) 0xFFFFF400) //병렬 입출력 제어 레지스터 병렬 입출력 포트로 사용한다.
#define PIO_IFER (*(volatile unsigned int *) 0xFFFFF420)
#define PIO_IER (*(volatile unsigned int *) 0xFFFFF440)
#define PIO_ODR (*(volatile unsigned int *) 0xFFFFF414)
#define PIO_ASR (*(volatile unsigned int *) 0xFFFFF470)
#define PIO_BSR (*(volatile unsigned int *) 0xFFFFF474)
#define PIO_PDR (*(volatile unsigned int *) 0xFFFFF404)
//#define PIO_ISR (*(volatile unsigned int*) 0xFFFFF44C)
//#define PIO_PUDR (*(volatile unsigned int*) 0xFFFFF460)// 풀업저항 인터럽트 레지스터
#define AIC_IDCR (*(volatile unsigned int*) 0xFFFFF124)
#define AIC_SVR ((volatile unsigned int*) 0xFFFFF080)
#define AIC_SMR ((volatile unsigned int*) 0xFFFFF000)
#define AIC_IECR (*(volatile unsigned int*) 0xFFFFF120)
#define AIC_ICCR (*(volatile unsigned int*) 0xFFFFF128)
//-----------디버그 유닛--------------------------------------------------------------
#define DBGU_MR (*(volatile unsigned int*) 0xFFFFF204)
#define DBGU_BRGR (*(volatile unsigned int*) 0xFFFFF220) // BAUD RATE GENERRATOR REGISTER.
#define DBGU_CR (*(volatile unsigned int*) 0xFFFFF200) // CONTROLL REGISTER
//------------------------------변수 선언------------------------------------------------
/*
INIT
PA0 모터 1 = 모터 1구동 변화량 소스값
PA1 모터 2 = 모터 2구동 변화량 소스값
PA2
PA3 RIGHT 모터 전후 CW/CCW 방향제어
PA4 LEFT 모터 전후 CW/CCW 방향제어
PA6 리셋 스위치 단자
// PA7 중심 잡기 기준용 입력 스위치 SETTING (풀업금지 :외부 입력)-보류xxx
PA8
PA9 L297 ENABLE(HIGH) - 공통
PA15 L297 OSC(HIGH) 2개 구동-공통
PA16
*/
unsigned int INIT; //초기화 변수
unsigned int F_RIGHT_start_SPEED=3000;//전진 시 오른쪽 모터 초기 값
unsigned int B_RIGHT_start_SPEED=3000;// 후진 시 오른족 모터 초기 값
unsigned int F_LEFT_start_SPEED=3000;//전진 시 왼쪽모터 초기값
unsigned int B_LEFT_start_SPEED=3000;//후진 시 왼쪽 모터 초기 값
unsigned int ALL_RESET_start_SPEED=3000;
unsigned int R_MAXIMAM_SPEED=200;//(최고속 주기)
unsigned int L_MAXIMAM_SPEED=200;//(최고속 주기)
float sum[10];
int i,j;
//-----------디버그 유닛------------------------------------------------------
void DBGU_TX_char(unsigned char data) //수신된데이타를 다시송신하는 구조: 에코 기능
{
while(!(*AT91C_DBGU_CSR & 0x0002));
*AT91C_DBGU_THR = data;
//while(!(*DBGU_SR &0x0002));// 데이타가 들어왔는지 검사 들어왔을때만 아래문장을 실행
//*DBGU_THR = data;// DATA 를 다시 송ㄹ신 버퍼에 넣어 준다
}
//--------------------------------------------------------------------------------------
void num_order(void)//낮은수부터 높은수 까지 정렬
{
int temp;
for(i=0;i<10;i++)
for(j=0;j<10;j++)
{
if(sum[i]>sum[j]);
{
temp=sum[j];
sum[j]=sum[i];
sum[i]=temp;
}
}
}
//------------------------------------------------------------------------------------------
//------------------------------------------------------------------------------------------
int main(void)
{
unsigned int F_DIRECTION= 0x8;//전진 PA3HIGH
unsigned int B_DIRECTION= 0x10;//후진 PA4 HIGH
unsigned int num=10000000;
unsigned char ch[10];
unsigned int total=0;
unsigned int gsenser;
unsigned int PWM0_duty =R_MAXIMAM_SPEED, PWM1_duty = L_MAXIMAM_SPEED; // initial PWM duty value
float ACCEL_VALUE=15; // 주기(주파수) 가속 값
//unsigned int STANDING_SET;
//MCU_initialize(); // initialize AT91SAM7S256 & kit
// wait for system stabilization
//AT91F_DBGU_CfgPMC();
PMC_PCER = 1<<1;//
// AT91F_PWMC_CfgPMC(); // enable clock of PWMC
PMC_PCER =1<<10;
//------------------------------------------------------------------------------------------
//PIO_ASR = 1<<10|1<<9;
//PIO_PDR = 1<<9|1<<10;
//PIO_PER |=1<<7; //병렬 I/O포트로 사용 설정 디폴트로 I/O포트로 사용 하도록 되어 있다.
//PA07풀다운 저항
// PIO_PUDR= 0x80;
// PIO_ODR = 0x80; //출력 버퍼 금지 레지스터 설정
PIO_OER |=(1<<3) //포트PA3 사용을 허용 한다
|(1<<4); //포트PA4 사용을 허용 한다
//|(1<<7) //포트PA7 사용을 허용 한다
//PIO_IFER|=1<<7; //글리치 필터 사용
//PIO_IER|=1<<7; //인터럽트 인애이블 레지스터
/*
AIC_IDCR=(unsigned int)1<<2; //마스크 설정 INTERRUPT DISABLE COMMAND REGISTER
AIC_SVR[2]=(unsigned int)PIO_ISR_FUNCTION; //인터럼트 함수 설정 등록
AIC_SMR[2]=5; //내부 인터럽트일때 H 외부 인터럽트 LOW
AIC_ICCR=(unsigned int)1<<2; //PIO A포트의 에지 검출기를 클리어 한다
AIC_IECR=(unsigned int)1<<2; //마스크 해지
*/
// *AT91C_DBGU_MR=0x00000800;
DBGU_MR = 0x800;//페리티 사용불가
// *AT91C_DBGU_BRGR = 26;
DBGU_BRGR = 26;
// *AT91C_DBGU_CR = 0x00000150;//수신부동작허용,송신부동작허용,송신부즉시리셋 동작금지
DBGU_CR = 0x150;
//------------ADC------------------------------------------------------------------------------------
// AT91F_ADC_CfgPMC();
PMC_PCER = 1<<5;// AD4번채널 허용- AD 입력포트로 사용
// AT91F_ADC_SoftReset(AT91C_BASE_ADC);//무효
ADC_CR = 0x1<<0; // ADC 내부를 하드웨어 리셋 한것 처럼 리셋한다
// *AT91C_ADC_MR = 0x02020400;
ADC_MR = 0x02020400;//클록주파수=mck/((prescal+1)*2),기동시간,샘플/홀드 시간
//AT91F_ADC_EnableChannel(AT91C_BASE_ADC, AT91C_ADC_CH5);
ADC_CHER = 0x1<<5; //AD5번채널까지 허용
//----------------------------------------------------------------------------------------------------------
// AT91F_PIO_CfgPeriph(AT91C_BASE_PIOA, AT91C_PA1_PWM1 | AT91C_PA0_PWM0, 0);
// PWM1,PWM0 output(Peripheral A)
PIO_ASR =1<<1|1<<0;
PIO_BSR =0;
PIO_PDR =1<<1|1<<0; // Set in Periph mode
//AT91F_PWMC_CfgMode(AT91C_BASE_PWMC, 0x031E031E); // CLKA=CLKB = 48MHz/8/30 =200kHz
PWMC_MR = 0x031E031E;////분주비/디바이더 팩터 PREB=BREA=8 // 1E=30
//AT91F_PWMC_CfgChannel(AT91C_BASE_PWMC, 0, 0x020B, 2000, PWM0_duty); // CLKA/100 = 2kHz
PWMC_CMR0 = 0x060B;// CPOL=1 출력파파형이 H 레벨에서 시작된다 // 채널 프리스케일러 CPRE=B CLKB
PWMC_CDTYR0 = PWM0_duty; //채널 듀티 사이클 0~PWM0_duty
PWMC_CPRDR0 =F_RIGHT_start_SPEED;
// AT91F_PWMC_CfgChannel(AT91C_BASE_PWMC, 1, 0x020B, 1000, PWM1_duty); // CLKA/200 = 1kHz
PWMC_CMR1 = 0x060B;
PWMC_CDTYR1 = PWM1_duty;
PWMC_CPRDR1 =B_LEFT_start_SPEED;
// AT91F_PWMC_StartChannel(AT91C_BASE_PWMC, AT91C_PWMC_CHID1 | AT91C_PWMC_CHID0);
// start PWM1 and PWM0
PWMC_ENA =1<<1|1<<0;// PWM PA0,PA1 출력 허용
while(1)
{
num=1000000;
//------------------------------------------------------------------------------------------
/*
PA10(L297(10) ENABLE 10번 HIGH) (L297 칩을 사용가능하게 한다),
PA19(L297(20)=RESET(평소HIGH)
PA16(L297(16)=HIGH(RC 회로가 초퍼 레이트를 결정한다// L297 한개를 연결할 때 GND에 연결 한다
*/
// PIO_SODR = INIT;
for(i = 0; i<10; i++)
{
AT91F_ADC_StartConversion(AT91C_BASE_ADC);
//ADC_CR = 0x1<<1;ADC변환시작
//while(!(AT91F_ADC_GetStatus(AT91C_BASE_ADC)&AT91C_ADC_EOC4));
//while(!(ADC_SR&(1<<4))//
sum[i] = AT91F_ADC_GetConvertedDataCH5(AT91C_BASE_ADC);
//sum += ADC_CDR5;//5번채널사용
Delay_ms(10);
}
num_order();
for(i=2;i<8;i++)//튀는값 버림
{
total +=sum[i];
}
gsenser=(total/6);//추출한 6개의 합의 평균
//************************************************************************************************
if(gsenser<=489 && gsenser >= 250) // 전진 범위
{
PIO_CODR=B_DIRECTION;
PIO_SODR=(INIT|F_DIRECTION);// 전진 L297 CW/CCW 방향설정 PA03HIGH PA04LOW
B_RIGHT_start_SPEED=ALL_RESET_start_SPEED, B_LEFT_start_SPEED=ALL_RESET_start_SPEED;// 후진 값 초기화
//---------------R 모터 설정 --------------------------------------
if(F_RIGHT_start_SPEED<=220)
PWMC_CPRDR0= 220; //(gsenser-249)*5
else //RIGHT_start_SPEED > R_MAXIMAM_SPEED+20
{
PWMC_CPRDR0=F_RIGHT_start_SPEED;
F_RIGHT_start_SPEED-=ACCEL_VALUE;// 가속
PWMC_CPRDR0=F_RIGHT_start_SPEED;
}
//--------------L 모터 설정 ----------------------------------------
if(F_LEFT_start_SPEED<=220)
{
PWMC_CPRDR1=220; // 주파수 속도 방어코드
}
else
{
PWMC_CPRDR1=F_LEFT_start_SPEED;//전진 초기값 줌
F_LEFT_start_SPEED-=ACCEL_VALUE;
PWMC_CPRDR1=F_LEFT_start_SPEED;
}
}
//===============================정지 설정===================================================
if(gsenser<=510 && gsenser >= 490) // 전진 범위
{
B_RIGHT_start_SPEED=ALL_RESET_start_SPEED, B_LEFT_start_SPEED=ALL_RESET_start_SPEED;// 후진 값 초기화
F_RIGHT_start_SPEED=ALL_RESET_start_SPEED, F_LEFT_start_SPEED=ALL_RESET_start_SPEED;// 전진 값 초기화
PWMC_CPRDR0=PWMC_CPRDR1=0;//정지
}
//============================== 후진 설정=====================================================
if(gsenser>=511 && gsenser <= 750) // 후진 범위
{
PIO_CODR=F_DIRECTION;//
PIO_SODR=(INIT|B_DIRECTION);//후진 모터 바퀴 방향설정
F_RIGHT_start_SPEED=ALL_RESET_start_SPEED, F_LEFT_start_SPEED=ALL_RESET_start_SPEED;//전진 값 초기화
//---------------------후진 R 모터 설정-----------------------------------
if(B_RIGHT_start_SPEED<=220)
PWMC_CPRDR0=220;//가속도 최대값 방어 코드
else
{
PWMC_CPRDR0=B_RIGHT_start_SPEED;
B_RIGHT_start_SPEED-=ACCEL_VALUE;
PWMC_CPRDR0=B_RIGHT_start_SPEED;
}
//---------------------후진 L 모터 설정-----------------------------------
if(B_LEFT_start_SPEED<=220)
{
PWMC_CPRDR1=220; // 주파수 속도 방어코드
}
else
{
PWMC_CPRDR1=B_LEFT_start_SPEED;//후진 초기 가속 값 줌
B_LEFT_start_SPEED-=ACCEL_VALUE;// 가속 코드
PWMC_CPRDR1=B_LEFT_start_SPEED;
}
}
//*************************************************************************************************
for(i=0;i<7;i++)
{
ch[i]= (gsenser%(10*num)-gsenser%num)/num + 48;
num/=10;
DBGU_TX_char(ch[i]);
}
DBGU_TX_char('\r');
DBGU_TX_char('\n');
Delay_ms(10);
total=0;
}
}