![](/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@23:0927e605af4b, 2013-11-22 (annotated)
- Committer:
- syundo0730
- Date:
- Fri Nov 22 00:30:42 2013 +0000
- Revision:
- 23:0927e605af4b
- Parent:
- 21:a54bcab078ed
first commit
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
syundo0730 | 9:d9ce965299d2 | 1 | #include "PWM.h" |
syundo0730 | 21:a54bcab078ed | 2 | #include "HomePosition.h" |
syundo0730 | 9:d9ce965299d2 | 3 | |
syundo0730 | 9:d9ce965299d2 | 4 | //static menber variable |
syundo0730 | 9:d9ce965299d2 | 5 | volatile uint8_t PWM::SRV_Idx = 0; |
syundo0730 | 9:d9ce965299d2 | 6 | volatile uint32_t PWM::SRV_dutyTable[SRV_BANK_NUM][SRV_IDX_NUM]; |
syundo0730 | 9:d9ce965299d2 | 7 | |
syundo0730 | 9:d9ce965299d2 | 8 | PWM::PWM() |
syundo0730 | 9:d9ce965299d2 | 9 | { |
syundo0730 | 9:d9ce965299d2 | 10 | for (int i = 0; i < SRV_CH_NUM; i++) { |
syundo0730 | 21:a54bcab078ed | 11 | setDuty(i, HOMEPOS[i]); |
syundo0730 | 9:d9ce965299d2 | 12 | } |
syundo0730 | 9:d9ce965299d2 | 13 | InitPWM(); |
syundo0730 | 9:d9ce965299d2 | 14 | } |
syundo0730 | 9:d9ce965299d2 | 15 | |
syundo0730 | 9:d9ce965299d2 | 16 | void PWM::InitPWM() |
syundo0730 | 9:d9ce965299d2 | 17 | { |
syundo0730 | 9:d9ce965299d2 | 18 | LPC_PINCON->PINSEL3 &= ~(3 << 4); // GPIO (00) |
syundo0730 | 9:d9ce965299d2 | 19 | LPC_GPIO1->FIODIR |= (1 << 18); // output |
syundo0730 | 9:d9ce965299d2 | 20 | |
syundo0730 | 9:d9ce965299d2 | 21 | LPC_PINCON->PINSEL4 |= 0x555; // GPIO (00) |
syundo0730 | 9:d9ce965299d2 | 22 | LPC_GPIO2->FIODIR |= 0x3F; // output |
syundo0730 | 9:d9ce965299d2 | 23 | |
syundo0730 | 9:d9ce965299d2 | 24 | LPC_SC->PCLKSEL0 &= ~(3 << 12); // PCLK_TIMER0 ck/4 (00) |
syundo0730 | 9:d9ce965299d2 | 25 | LPC_SC->PCONP |= (1 << 6); // PCPWM1 |
syundo0730 | 9:d9ce965299d2 | 26 | |
syundo0730 | 9:d9ce965299d2 | 27 | LPC_PWM1->IR |= (1<<0);//reset PWMMR0 flag |
syundo0730 | 9:d9ce965299d2 | 28 | LPC_PWM1->TCR = (1 << 1);//reset |
syundo0730 | 9:d9ce965299d2 | 29 | LPC_PWM1->PR = SystemCoreClock / 4 / 1000000 - 1; // prescale 1000kHz |
syundo0730 | 9:d9ce965299d2 | 30 | LPC_PWM1->CTCR = 0; |
syundo0730 | 9:d9ce965299d2 | 31 | LPC_PWM1->MCR |= (1 << 1)|(1 << 0); // MR0R, MR0 |
syundo0730 | 9:d9ce965299d2 | 32 | |
syundo0730 | 9:d9ce965299d2 | 33 | LPC_PWM1->MR0 = SRV_PERIOD; |
syundo0730 | 9:d9ce965299d2 | 34 | |
syundo0730 | 9:d9ce965299d2 | 35 | LPC_PWM1->TCR = (1 << 0)|(1 << 3); // enable |
syundo0730 | 9:d9ce965299d2 | 36 | LPC_PWM1->PCR |= (0x3F << 9);//1-6 pwm |
syundo0730 | 9:d9ce965299d2 | 37 | LPC_PWM1->LER |= 0x7E; |
syundo0730 | 9:d9ce965299d2 | 38 | |
syundo0730 | 9:d9ce965299d2 | 39 | NVIC_SetPriority(PWM1_IRQn,0);//priority level (high)0~255(low) |
syundo0730 | 9:d9ce965299d2 | 40 | NVIC_EnableIRQ(PWM1_IRQn);//enable |
syundo0730 | 9:d9ce965299d2 | 41 | } |
syundo0730 | 9:d9ce965299d2 | 42 | |
syundo0730 | 21:a54bcab078ed | 43 | void PWM::setDuty(uint8_t ch, uint32_t duty) |
syundo0730 | 9:d9ce965299d2 | 44 | { |
syundo0730 | 9:d9ce965299d2 | 45 | if (ch >= SRV_CH_NUM) { |
syundo0730 | 9:d9ce965299d2 | 46 | return; |
syundo0730 | 9:d9ce965299d2 | 47 | } else if (duty < SRV_MIN_DUTY) { |
syundo0730 | 9:d9ce965299d2 | 48 | duty = SRV_MIN_DUTY; |
syundo0730 | 9:d9ce965299d2 | 49 | } else if (duty > SRV_MAX_DUTY) { |
syundo0730 | 9:d9ce965299d2 | 50 | duty = SRV_MAX_DUTY; |
syundo0730 | 9:d9ce965299d2 | 51 | } |
syundo0730 | 9:d9ce965299d2 | 52 | |
syundo0730 | 9:d9ce965299d2 | 53 | SRV_dutyTable[ch >> SRV_IDX_SHIFT][ch & SRV_IDX_MASK] = duty; |
syundo0730 | 9:d9ce965299d2 | 54 | } |
syundo0730 | 9:d9ce965299d2 | 55 | |
syundo0730 | 9:d9ce965299d2 | 56 | void PWM1_IRQHandler (void) |
syundo0730 | 11:1539d181e159 | 57 | { |
syundo0730 | 9:d9ce965299d2 | 58 | LPC_PWM1->MR1 = SRV_PWMTable[0][PWM::SRV_Idx]; |
syundo0730 | 9:d9ce965299d2 | 59 | LPC_PWM1->MR2 = SRV_PWMTable[1][PWM::SRV_Idx]; |
syundo0730 | 9:d9ce965299d2 | 60 | LPC_PWM1->MR3 = SRV_PWMTable[2][PWM::SRV_Idx]; |
syundo0730 | 9:d9ce965299d2 | 61 | LPC_PWM1->MR4 = PWM::SRV_dutyTable[0][PWM::SRV_Idx]; |
syundo0730 | 9:d9ce965299d2 | 62 | LPC_PWM1->MR5 = PWM::SRV_dutyTable[1][PWM::SRV_Idx]; |
syundo0730 | 9:d9ce965299d2 | 63 | LPC_PWM1->MR6 = PWM::SRV_dutyTable[2][PWM::SRV_Idx]; |
syundo0730 | 9:d9ce965299d2 | 64 | |
syundo0730 | 9:d9ce965299d2 | 65 | LPC_PWM1->LER |= 0x7E; |
syundo0730 | 9:d9ce965299d2 | 66 | |
syundo0730 | 9:d9ce965299d2 | 67 | LPC_PWM1->IR |= (1<<0); // reset PWMMR0 flag |
syundo0730 | 9:d9ce965299d2 | 68 | |
syundo0730 | 9:d9ce965299d2 | 69 | PWM::SRV_Idx++; |
syundo0730 | 9:d9ce965299d2 | 70 | |
syundo0730 | 9:d9ce965299d2 | 71 | if(PWM::SRV_Idx >= SRV_IDX_NUM) { |
syundo0730 | 9:d9ce965299d2 | 72 | PWM::SRV_Idx = 0; |
syundo0730 | 9:d9ce965299d2 | 73 | } |
syundo0730 | 9:d9ce965299d2 | 74 | } |