모터 동작 개념및 코드

우리가 사용하는 모터드라이버는 500HZ까지 인식한다.

 

500HZ->1/500 = 2ms 이므로

 

2ms 주기까지 인식한다는 뜻이다.

 

atmega128로 짠 코드로는 Sensor update 가 200hz, 모터 제어주기는 400hZ로 동작한다.

 

이것이 의미하는 바는 센서 센서값이 1번들어올때 모터 동작을 2번 해준다는 것이다.

(물론 생각대로 정확할진 모름...)

 

이 모터드라이버의 시동방법은 최소 스로틀 입력을 주면 삐~ 하는소리와함께 시동이 걸린다.

 

그후 원하는 pwm을 입력하면 그에 맞는 모터출력이 나오게된다.

 

타이머1,3를 사용했으며 분주비는 1/8 따라서 1count 당 2Mhz -> 0.5us 이다.

 

모터출력부분 코드는 아래와같다.

 

while(set1)
   {
    
    duty1 = 1800;
    duty2 = 1800;
    OCR1A = duty1;
    OCR1B = duty2;
    OCR3A = duty1;
    OCR3B = duty2;
    //printf("%d,%d, %.2f,%.2f ,%.2f ,%d ,%d\n",m_rollAngle,m_pitchAngle,kp,ki,kd,pid1,set4);
   }
   
   while(set2)
   {
    if(set4 ==1)
    {

    sensor_read();
    pid1 = motor_pid_ctrl1(m_rollAngle);
    pid2 = motor_pid_ctrl2(m_pitchAngle);
    pid3 = motor_pid_ctrl3(m_yawAngle);
    set4 = 0;
    }
    duty1 = CENTER + pid1+pid3;
    if(duty1>3800)
    {
     duty1 = 4100;
     duty2 = 2000;
    }
    duty2 = CENTER - pid1+pid3;
    if(duty2>3800)
    {
     duty2 = 4100;
     duty1 = 2000;
    }
    
    duty3 = CENTER + pid2-pid3;
    if(duty3>3800)
    {
     duty3 = 4100;
     duty4 = 2000;
    }
    duty4 = CENTER - pid2-pid3;
    if(duty4>3800)
    {
     duty4 = 4100;
     duty3 = 2000;
    }
    
    OCR1A = duty1;
    OCR1B = duty2;
    OCR3A = duty3;
    OCR3B = duty4;
    //printf("roll : %d , pitch : %d , yaw : %d , pid1 : %d, pid2 : %d , CENTER : %d\n",m_rollAngle,m_pitchAngle,m_yawAngle,pid1,pid2,CENTER);
    //printf("roll  %d, duty1 :%d, duty2 : %d ,kp :%.2f ,ki  %.2f ,kd :%.2f ,pid1 : %d\n",m_rollAngle,duty1,duty2,kp,ki,kd,pid1);
    //printf("pitch :%d ,duty3 :%d, duty4 :%d, kp :%.2f ,ki :%.2f ,kd :%.2f ,pid2 : %d\n",m_pitchAngle,duty3,duty4,kp2,ki2,kd2,pid2);
    //printf(" yaw : %d , kp : %.2f , ki : %.2f , pid3 : %d\n",m_yawAngle,kp3,ki3,pid3);
    //printf( "rxbuf[25] : %x , rxbuf[26] : %x , checksum1 : %x , checksum : %x \n",rx_buf[25],rx_buf[26],checksum1,checksum);
    
   }
   
   while(set3)
   {
    duty1 = 0;
    duty2 = 0 ;
    duty3 = 0;
    duty4 = 0;
    OCR1A = duty1;
    OCR1B = duty2;
    OCR3A = duty3;
    OCR3B = duty4;
    //printf("roll : %d , pitch : %d , yaw : %d , pid1 : %d , pid2 : %d\n",m_rollAngle,m_pitchAngle,m_yawAngle,pid1,pid2);
    //printf( "rxbuf[25] : %x , rxbuf[26] : %x , checksum1 : %x , checksum : %x \n",rx_buf[25],rx_buf[26],checksum1,checksum);
   }

 

 

 

이 소스에 대해 설명하자면 set1,set2,set3가있다.

while문은 1의 값일때 그안의 구문을 반복하므로, uart 통신으로 set1~3까지를 셋팅한다

아래는 그에대한 코드이다.

 

ISR(USART0_RX_vect)
{
 char ch;
 ch = UDR0;
 
 if(ch == 'q')
 {
  set1 = 1;
  set2 = 0;
  set3 = 0;
 }
 else if (ch =='w')
 {
  set1 = 0;
  set2 = 1;
  set3 = 0;
 } 

else if (ch == 'e')
 {
  
  set1 = 0;
  set2 = 0;
  set3 = 1;
  
 }
 else if (ch =='a')
 {
  //kp = kp+0.1;
  //kp2 = kp2+0.1;
  CENTER=CENTER +20;
  //kp3 = kp3 +0.1;
 }
 else if (ch =='b')
 {
  //kp = kp-0.1;
  //kp2 = kp2-0.1;
  CENTER=CENTER -20;
  //kp3 = kp3 -0.1;
 }
//  else if (ch =='l')
//  {
//   //ki = ki+0.01;
//   //ki2 = ki2+0.01;
//   ki3 = ki3 + 0.01;
//   //CENTER=CENTER +50;
//  }
//  else if (ch ==';')
//  {
//   //ki = ki-0.01;
//   //ki2 = ki2-0.01;
//   ki3 = ki3- 0.01;
//   //CENTER=CENTER -50;
//  }
//  else if (ch =='[')
//  {
//   //kd = kd+0.01;
//   //kd2= kd2+0.01;
//   //CENTER=CENTER +50;
//  }
//  else if (ch ==']')
//  {
//   //kd = kd-0.01;
//   //kd2 = kd2-0.01;
//   //CENTER=CENTER -50;
//  }

}

 

각해당하는 q,w,e 키를 누르면 off, set, hovering 모드로 진입할수있다.