Controlor for Humanoid. Walking trajectory generator, sensor reflection etc.
Dependencies: Adafruit-PWM-Servo-Driver MPU6050 RS300 mbed
PWM.cpp
- Committer:
- syundo0730
- Date:
- 2013-02-27
- Revision:
- 16:e65c192b7ecf
- Parent:
- 11:1539d181e159
- Child:
- 21:a54bcab078ed
File content as of revision 16:e65c192b7ecf:
#include "PWM.h" //static menber variable volatile uint8_t PWM::SRV_Idx = 0; volatile uint32_t PWM::SRV_dutyTable[SRV_BANK_NUM][SRV_IDX_NUM]; PWM::PWM() { for (int i = 0; i < SRV_CH_NUM; i++) { SetDuty(i, HOMEPOS[i]); } InitPWM(); } void PWM::InitPWM() { LPC_PINCON->PINSEL3 &= ~(3 << 4); // GPIO (00) LPC_GPIO1->FIODIR |= (1 << 18); // output LPC_PINCON->PINSEL4 |= 0x555; // GPIO (00) LPC_GPIO2->FIODIR |= 0x3F; // output LPC_SC->PCLKSEL0 &= ~(3 << 12); // PCLK_TIMER0 ck/4 (00) LPC_SC->PCONP |= (1 << 6); // PCPWM1 LPC_PWM1->IR |= (1<<0);//reset PWMMR0 flag LPC_PWM1->TCR = (1 << 1);//reset LPC_PWM1->PR = SystemCoreClock / 4 / 1000000 - 1; // prescale 1000kHz LPC_PWM1->CTCR = 0; LPC_PWM1->MCR |= (1 << 1)|(1 << 0); // MR0R, MR0 LPC_PWM1->MR0 = SRV_PERIOD; LPC_PWM1->TCR = (1 << 0)|(1 << 3); // enable LPC_PWM1->PCR |= (0x3F << 9);//1-6 pwm LPC_PWM1->LER |= 0x7E; NVIC_SetPriority(PWM1_IRQn,0);//priority level (high)0~255(low) NVIC_EnableIRQ(PWM1_IRQn);//enable } void PWM::SetDuty(uint8_t ch, uint32_t duty) { if (ch >= SRV_CH_NUM) { return; } else if (duty < SRV_MIN_DUTY) { duty = SRV_MIN_DUTY; } else if (duty > SRV_MAX_DUTY) { duty = SRV_MAX_DUTY; } SRV_dutyTable[ch >> SRV_IDX_SHIFT][ch & SRV_IDX_MASK] = duty; } void PWM1_IRQHandler (void) { LPC_PWM1->MR1 = SRV_PWMTable[0][PWM::SRV_Idx]; LPC_PWM1->MR2 = SRV_PWMTable[1][PWM::SRV_Idx]; LPC_PWM1->MR3 = SRV_PWMTable[2][PWM::SRV_Idx]; LPC_PWM1->MR4 = PWM::SRV_dutyTable[0][PWM::SRV_Idx]; LPC_PWM1->MR5 = PWM::SRV_dutyTable[1][PWM::SRV_Idx]; LPC_PWM1->MR6 = PWM::SRV_dutyTable[2][PWM::SRV_Idx]; LPC_PWM1->LER |= 0x7E; LPC_PWM1->IR |= (1<<0); // reset PWMMR0 flag PWM::SRV_Idx++; if(PWM::SRV_Idx >= SRV_IDX_NUM) { PWM::SRV_Idx = 0; } }