arm 7 가속도 센서 이용 스텝모터 앞뒤(기준값 500: 평행 ) 가속하기

#include "AT91SAM7S256.h"
#include "lib_AT91SAM7S256.h"
#include "OK7S256ads.h"

//---------------------전력제어 설정-------------------------------------------
#define PMC_PCER (*(volatile unsigned int *) 0xFFFFFC10)

//----------PWM-----------------------------------------------------------------
// 모터 1
#define  PWMC_CMR0 (*(volatile unsigned int *) 0xFFFCC200)
#define  PWMC_CDTYR0  (*(volatile unsigned int *) 0xFFFCC204)
#define  PWMC_CPRDR0  (*(volatile unsigned int *) 0xFFFCC208)

//모터 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 *) 0xFFFFD800)
#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 *) 0xFFFFD810)

//-----------입출력 제어 PIO-------------------------------------------------------
#define PIO_SODR (*(volatile unsigned int *) 0xFFFFF430) //출력
#define PIO_CODR (*(volatile unsigned int *) 0xFFFFF434) //출력 하지 않음
#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_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)

//------------------------------변수 선언------------------------------------------------
/*
   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 F_DIRECTION= 0x8;//전진 PA3HIGH
unsigned int B_DIRECTION= 0x10;//후진 PA4 HIGH
unsigned int INIT; //초기화 변수
unsigned int F_RIGHT_start_SPEED=410;//전진 시 오른쪽 모터 초기 값
unsigned int B_RIGHT_start_SPEED=410;// 후진 시 오른족 모터 초기 값
unsigned int F_LEFT_start_SPEED=400;//전진 시 왼쪽모터 초기값
unsigned int B_LEFT_start_SPEED=400;//후진 시 왼쪽 모터 초기 값

unsigned int R_MAXIMAM_SPEED=210;//(최고속 주기)
unsigned int L_MAXIMAM_SPEED=210;//(최고속 주기)
//unsigned int function_jump_flag;
float sum[10];
//int  STANDING_SET[11];
int i,j;
/*------------------------------------------------------------------------------
void PIO_ISR_FUNCTION()
{

 if(PIO_ISR && 1<<7 ==0x1000000)
 { function_jump_flag=1;
  
   
 }
}
*/
//-----------디버그 유닛------------------------------------------------------
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;
   }
   
  }
}
//------------------------------------------------------------------------------------------
__inline void AT91F_PWMC_CfgMode(
        AT91PS_PWMC pPWM,   // pointer to a PWM controller
        unsigned int mode)   // PWM_MR register value
{
 pPWM->PWMC_MR = mode;
}
//------------------------------------------------------------------------------------------
int main(void)
{
// unsigned int num=1000000;
 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=0.005;
 //unsigned int STANDING_SET;
 
 
  MCU_initialize();    // initialize AT91SAM7S256 & kit
// wait for system stabilization

//AT91F_DBGU_CfgPMC();
 PMC_PCER = 1<<1;//|1<<7;
 AT91F_DBGU_CfgPIO();
//------------------------------------------------------------------------------------------ 
//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;

 AT91F_ADC_CfgPMC();
 //PMC_PCER = 1<<4;
 AT91F_ADC_SoftReset(AT91C_BASE_ADC);//무효
 //ADC_CR = 0x1<<0;
 *AT91C_ADC_MR = 0x02020400;
 //*ADC_MR = 0x02020400;클록주파수=mck/((prescal+1)*2),기동시간,샘플/홀드 시간
 AT91F_ADC_EnableChannel(AT91C_BASE_ADC, AT91C_ADC_CH5);
 //ADC_CHER = 0x1<<4;  5번채널까지 허용

//---------------------------------------------------------------------------------------------
  AT91F_PWMC_CfgPMC();    // enable clock of PWMC
  AT91F_PIO_CfgPeriph(AT91C_BASE_PIOA, AT91C_PA1_PWM1 | AT91C_PA0_PWM0, 0);
        // PWM1,PWM0 output(Peripheral A)

  AT91F_PWMC_CfgMode(AT91C_BASE_PWMC, 0x031E031E); // CLKA=CLKB = 48MHz/8/30 =200kHz
 
 //pPWM->PWMC_MR = 0x031A031E;
  //AT91F_PWMC_CfgChannel(AT91C_BASE_PWMC, 0, 0x020B, 2000, PWM0_duty); // CLKA/100 = 2kHz
   PWMC_CMR0 =  0x020B;
 PWMC_CDTYR0 = PWM0_duty;
 PWMC_CPRDR0 =F_RIGHT_start_SPEED;
// AT91F_PWMC_CfgChannel(AT91C_BASE_PWMC, 1, 0x020B, 1000, PWM1_duty); // CLKA/200 = 1kHz
   PWMC_CMR1 =  0x020B;
 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
  
  while(1)
    {
 unsigned int 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<=500 && gsenser >= 250)  // 전진 범위
  {  
   PIO_CODR=B_DIRECTION;
   PIO_SODR=(INIT|F_DIRECTION);//   전진  L297 CW/CCW 방향설정 PA03HIGH PA04LOW
   B_RIGHT_start_SPEED=400, B_LEFT_start_SPEED=400;// 후진 값 초기화
  
   
   //---------------R 모터 설정 --------------------------------------
   if(F_RIGHT_start_SPEED<=R_MAXIMAM_SPEED+20)
    PWMC_CPRDR0= R_MAXIMAM_SPEED+20;
   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<=L_MAXIMAM_SPEED+20)
   {
    PWMC_CPRDR1=L_MAXIMAM_SPEED+20; // 주파수 속도 방어코드
   }
   else
   {
    PWMC_CPRDR1=F_LEFT_start_SPEED;//전진  초기값 줌
    F_LEFT_start_SPEED-=ACCEL_VALUE;
    PWMC_CPRDR1=F_LEFT_start_SPEED;
   }
  
  }
//============================== 후진 설정=====================================================
  if(gsenser>500 && gsenser <= 750) // 후진 범위
  {  
   PIO_CODR=F_DIRECTION;//
   PIO_SODR=(INIT|B_DIRECTION);//후진 모터 바퀴 방향설정
   F_RIGHT_start_SPEED=400, F_LEFT_start_SPEED=400;//전진 값 초기화
   
   
   
   //---------------------후진 R 모터 설정-----------------------------------
   if(B_RIGHT_start_SPEED<=R_MAXIMAM_SPEED+20)
    PWMC_CPRDR0=R_MAXIMAM_SPEED+20;//가속도 최대값 방어 코드    
   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<=L_MAXIMAM_SPEED+20)
   {
    PWMC_CPRDR1=L_MAXIMAM_SPEED+20; // 주파수 속도 방어코드   
   }
   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;
  
 }
}