![](/media/cache/profiles/e5815151957be36ad2085b7a1a02c5cc.jpg.50x50_q85.jpg)
Controlor for Humanoid. Walking trajectory generator, sensor reflection etc.
Dependencies: Adafruit-PWM-Servo-Driver MPU6050 RS300 mbed
PWM.cpp@9:d9ce965299d2, 2012-09-22 (annotated)
- Committer:
- syundo0730
- Date:
- Sat Sep 22 06:16:15 2012 +0000
- Revision:
- 9:d9ce965299d2
- Child:
- 11:1539d181e159
play motion
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
syundo0730 | 9:d9ce965299d2 | 1 | #include "PWM.h" |
syundo0730 | 9:d9ce965299d2 | 2 | |
syundo0730 | 9:d9ce965299d2 | 3 | //static menber variable |
syundo0730 | 9:d9ce965299d2 | 4 | volatile uint8_t PWM::SRV_Idx = 0; |
syundo0730 | 9:d9ce965299d2 | 5 | volatile uint32_t PWM::SRV_dutyTable[SRV_BANK_NUM][SRV_IDX_NUM]; |
syundo0730 | 9:d9ce965299d2 | 6 | |
syundo0730 | 9:d9ce965299d2 | 7 | PWM::PWM() |
syundo0730 | 9:d9ce965299d2 | 8 | { |
syundo0730 | 9:d9ce965299d2 | 9 | for (int i = 0; i < SRV_CH_NUM; i++) { |
syundo0730 | 9:d9ce965299d2 | 10 | SetDuty(i, (SRV_MAX_DUTY + SRV_MIN_DUTY) / 2); |
syundo0730 | 9:d9ce965299d2 | 11 | } |
syundo0730 | 9:d9ce965299d2 | 12 | InitPWM(); |
syundo0730 | 9:d9ce965299d2 | 13 | } |
syundo0730 | 9:d9ce965299d2 | 14 | |
syundo0730 | 9:d9ce965299d2 | 15 | void PWM::InitPWM() |
syundo0730 | 9:d9ce965299d2 | 16 | { |
syundo0730 | 9:d9ce965299d2 | 17 | LPC_PINCON->PINSEL3 &= ~(3 << 4); // GPIO (00) |
syundo0730 | 9:d9ce965299d2 | 18 | LPC_GPIO1->FIODIR |= (1 << 18); // output |
syundo0730 | 9:d9ce965299d2 | 19 | |
syundo0730 | 9:d9ce965299d2 | 20 | LPC_PINCON->PINSEL4 |= 0x555; // GPIO (00) |
syundo0730 | 9:d9ce965299d2 | 21 | LPC_GPIO2->FIODIR |= 0x3F; // output |
syundo0730 | 9:d9ce965299d2 | 22 | |
syundo0730 | 9:d9ce965299d2 | 23 | LPC_SC->PCLKSEL0 &= ~(3 << 12); // PCLK_TIMER0 ck/4 (00) |
syundo0730 | 9:d9ce965299d2 | 24 | LPC_SC->PCONP |= (1 << 6); // PCPWM1 |
syundo0730 | 9:d9ce965299d2 | 25 | |
syundo0730 | 9:d9ce965299d2 | 26 | LPC_PWM1->IR |= (1<<0);//reset PWMMR0 flag |
syundo0730 | 9:d9ce965299d2 | 27 | LPC_PWM1->TCR = (1 << 1);//reset |
syundo0730 | 9:d9ce965299d2 | 28 | LPC_PWM1->PR = SystemCoreClock / 4 / 1000000 - 1; // prescale 1000kHz |
syundo0730 | 9:d9ce965299d2 | 29 | LPC_PWM1->CTCR = 0; |
syundo0730 | 9:d9ce965299d2 | 30 | LPC_PWM1->MCR |= (1 << 1)|(1 << 0); // MR0R, MR0 |
syundo0730 | 9:d9ce965299d2 | 31 | |
syundo0730 | 9:d9ce965299d2 | 32 | LPC_PWM1->MR0 = SRV_PERIOD; |
syundo0730 | 9:d9ce965299d2 | 33 | |
syundo0730 | 9:d9ce965299d2 | 34 | LPC_PWM1->TCR = (1 << 0)|(1 << 3); // enable |
syundo0730 | 9:d9ce965299d2 | 35 | LPC_PWM1->PCR |= (0x3F << 9);//1-6 pwm |
syundo0730 | 9:d9ce965299d2 | 36 | LPC_PWM1->LER |= 0x7E; |
syundo0730 | 9:d9ce965299d2 | 37 | |
syundo0730 | 9:d9ce965299d2 | 38 | NVIC_SetPriority(PWM1_IRQn,0);//priority level (high)0~255(low) |
syundo0730 | 9:d9ce965299d2 | 39 | NVIC_EnableIRQ(PWM1_IRQn);//enable |
syundo0730 | 9:d9ce965299d2 | 40 | } |
syundo0730 | 9:d9ce965299d2 | 41 | |
syundo0730 | 9:d9ce965299d2 | 42 | void PWM::SetDuty(uint8_t ch, uint32_t duty) |
syundo0730 | 9:d9ce965299d2 | 43 | { |
syundo0730 | 9:d9ce965299d2 | 44 | if (ch >= SRV_CH_NUM) { |
syundo0730 | 9:d9ce965299d2 | 45 | return; |
syundo0730 | 9:d9ce965299d2 | 46 | } else if (duty < SRV_MIN_DUTY) { |
syundo0730 | 9:d9ce965299d2 | 47 | duty = SRV_MIN_DUTY; |
syundo0730 | 9:d9ce965299d2 | 48 | } else if (duty > SRV_MAX_DUTY) { |
syundo0730 | 9:d9ce965299d2 | 49 | duty = SRV_MAX_DUTY; |
syundo0730 | 9:d9ce965299d2 | 50 | } |
syundo0730 | 9:d9ce965299d2 | 51 | |
syundo0730 | 9:d9ce965299d2 | 52 | SRV_dutyTable[ch >> SRV_IDX_SHIFT][ch & SRV_IDX_MASK] = duty; |
syundo0730 | 9:d9ce965299d2 | 53 | } |
syundo0730 | 9:d9ce965299d2 | 54 | |
syundo0730 | 9:d9ce965299d2 | 55 | void PWM1_IRQHandler (void) |
syundo0730 | 9:d9ce965299d2 | 56 | { |
syundo0730 | 9:d9ce965299d2 | 57 | const uint32_t SRV_PWMTable[SRV_BANK_NUM][SRV_IDX_NUM] = { |
syundo0730 | 9:d9ce965299d2 | 58 | {0,SRV_PERIOD,0,SRV_PERIOD,0,SRV_PERIOD,0,SRV_PERIOD}, |
syundo0730 | 9:d9ce965299d2 | 59 | {0,0,SRV_PERIOD,SRV_PERIOD,0,0,SRV_PERIOD,SRV_PERIOD}, |
syundo0730 | 9:d9ce965299d2 | 60 | {0,0,0,0,SRV_PERIOD,SRV_PERIOD,SRV_PERIOD,SRV_PERIOD} |
syundo0730 | 9:d9ce965299d2 | 61 | }; |
syundo0730 | 9:d9ce965299d2 | 62 | |
syundo0730 | 9:d9ce965299d2 | 63 | LPC_PWM1->MR1 = SRV_PWMTable[0][PWM::SRV_Idx]; |
syundo0730 | 9:d9ce965299d2 | 64 | LPC_PWM1->MR2 = SRV_PWMTable[1][PWM::SRV_Idx]; |
syundo0730 | 9:d9ce965299d2 | 65 | LPC_PWM1->MR3 = SRV_PWMTable[2][PWM::SRV_Idx]; |
syundo0730 | 9:d9ce965299d2 | 66 | LPC_PWM1->MR4 = PWM::SRV_dutyTable[0][PWM::SRV_Idx]; |
syundo0730 | 9:d9ce965299d2 | 67 | LPC_PWM1->MR5 = PWM::SRV_dutyTable[1][PWM::SRV_Idx]; |
syundo0730 | 9:d9ce965299d2 | 68 | LPC_PWM1->MR6 = PWM::SRV_dutyTable[2][PWM::SRV_Idx]; |
syundo0730 | 9:d9ce965299d2 | 69 | |
syundo0730 | 9:d9ce965299d2 | 70 | LPC_PWM1->LER |= 0x7E; |
syundo0730 | 9:d9ce965299d2 | 71 | |
syundo0730 | 9:d9ce965299d2 | 72 | LPC_PWM1->IR |= (1<<0); // reset PWMMR0 flag |
syundo0730 | 9:d9ce965299d2 | 73 | |
syundo0730 | 9:d9ce965299d2 | 74 | PWM::SRV_Idx++; |
syundo0730 | 9:d9ce965299d2 | 75 | |
syundo0730 | 9:d9ce965299d2 | 76 | if(PWM::SRV_Idx >= SRV_IDX_NUM) { |
syundo0730 | 9:d9ce965299d2 | 77 | PWM::SRV_Idx = 0; |
syundo0730 | 9:d9ce965299d2 | 78 | } |
syundo0730 | 9:d9ce965299d2 | 79 | } |