Shundo Kishi / Mbed 2 deprecated Hobby_Humanoid_controlor

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?

UserRevisionLine numberNew 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 }