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@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?

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