가속도 센서 로 스텝모터 전후진 하기

#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_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)
//------------------------------변수 선언------------------------------------------------
unsigned int F_DIRECTION= 0x8;//전진 PA3HIGH
unsigned int B_DIRECTION= 0x10;//후진 PA4 HIGH
unsigned int INIT; //초기화 변수
unsigned int start_power=400;
unsigned int MAXIMAM_P=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;
}

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=10000000;
 unsigned char ch[10];
 unsigned int total=0,gsenser;
    unsigned int PWM0_duty =MAXIMAM_P-20, PWM1_duty = MAXIMAM_P-20; // initial PWM duty value
 unsigned int ACCEL_VALUE=1;
 
 
  MCU_initialize();    // initialize AT91SAM7S256 & kit
// wait for system stabilization
AT91F_DBGU_CfgPMC();
//PMC_PCER = 1<<1;
 AT91F_DBGU_CfgPIO();
//PIO_ASR = 1<<10|1<<9;
//PIO_PDR = 1<<9|1<<10;
PIO_OER = 0x18;;  //포트PA3~PA4 출력 을 허용 한다  
 
 *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 =start_power;
// AT91F_PWMC_CfgChannel(AT91C_BASE_PWMC, 1, 0x020B, 1000, PWM1_duty); // CLKA/200 = 1kHz
   PWMC_CMR1 =  0x020B;
 PWMC_CDTYR1 = PWM1_duty;
 PWMC_CPRDR1 =start_power;

  AT91F_PWMC_StartChannel(AT91C_BASE_PWMC, AT91C_PWMC_CHID1 | AT91C_PWMC_CHID0);
      // start PWM1 and PWM0
/* PA0 모터 1 = 모터 1구동 변화량 소스값
   PA1 모터 2 = 모터 2구동 변화량 소스값
   
   PA6 리셋 스위치 단자
   PA7 모터 1(HIGH) 전진 0(LOW)후진(L 297 CW/CCW)
   PA8 모터 0(HIGH) 전진 1(LOW)후진(L 297 CW/CCW) 
  
   PA9 L297 ENABLE(HIGH) - 공통
   PA15 L297 OSC(HIGH)  2개 구동-공통
   PA16
 */   
  while(1)
    {
//-------------------------------------------------------------------------------------------------    
   /*
   PA10(L297(10) ENABLE 10번 HIGH) (L297 칩을 사용가능하게 한다),
   PA19(L297(20)=RESET(평소HIGH)
   PA16(L297(16)=HIGH(RC 회로가 초퍼 레이트를 결정한다// L297 한개를 연결할 때 GND에 연결 한다
   */
//   PIO_SODR = INIT;
         num=1000000;
  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 && PWMC_CPRDR0>MAXIMAM_P-20 && PWMC_CPRDR1>MAXIMAM_P-20) 
  { 
   PIO_CODR=B_DIRECTION;
   PIO_SODR=(INIT|F_DIRECTION);//전진 모터 바퀴 방향설정
   PWMC_CPRDR0=start_power,PWMC_CPRDR1=start_power+40;//기준 값 0도 ~6도  후진 해야함
   //ACCEL_VALUE-=1;
  // PWMC_CPRDR0=ACCEL_VALUE,PWMC_CPRDR1=ACCEL_VALUE;
  }
  if(gsenser>500 && gsenser <= 750 && PWMC_CPRDR0>MAXIMAM_P-20 && PWMC_CPRDR1>MAXIMAM_P-20)
  {
   PIO_CODR=F_DIRECTION;
   PIO_SODR=(INIT|B_DIRECTION);//후진 모터 바퀴 방향설정
   PWMC_CPRDR0=start_power,PWMC_CPRDR1=start_power+40;//기준 값 0도 ~6도  후진 해야함
   //ACCEL_VALUE-=1;
   //PWMC_CPRDR0=ACCEL_VALUE,PWMC_CPRDR1=ACCEL_VALUE;
  }
  if(PWMC_CPRDR0<=MAXIMAM_P-20 && PWMC_CPRDR0<=MAXIMAM_P-20)
   PWMC_CPRDR0=MAXIMAM_P-20,PWMC_CPRDR1=MAXIMAM_P-20; // 주파수 속도 방어코드
 
//*************************************************************************************************
      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;
 

 /*
  AT91F_PWMC_UpdateChannel(AT91C_BASE_PWMC, 0, PWM0_duty);
  break;
       if(PWMC_CPRDR0 >MAXIMAM_P)
       PWMC_CPRDR0 -= 10;
    if(PWMC_CPRDR0<=MAXIMAM_P)
    PWMC_CPRDR0=start_power;
     if(PWMC_CPRDR1 >MAXIMAM_P)
      PWMC_CPRDR1 -= 10;
    if(PWMC_CPRDR1<=MAXIMAM_P)
    PWMC_CPRDR1=start_power;
    */   
 }
}