Controlor for Humanoid. Walking trajectory generator, sensor reflection etc.
Dependencies: Adafruit-PWM-Servo-Driver MPU6050 RS300 mbed
Diff: Source3.cpp
- Revision:
- 0:d114194bff43
- Child:
- 1:5c2e40445778
diff -r 000000000000 -r d114194bff43 Source3.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Source3.cpp Tue Aug 28 15:57:11 2012 +0000 @@ -0,0 +1,100 @@ +#ifndef PWM_H_2012_08_28_ +#define PWM_H_2012_08_28_ + +#include "LPC17xx.h" +#include "mbed.h" + +const int SRV_PERIOD = 2500 - 1;//���/8 +const int SRV_MAX_DUTY = 2100 - 1;//�f���[�e�B�[�ő�l +const int SRV_MIN_DUTY = 900 - 1;//�f���[�e�B�[�ŏ��l + +const int SRV_IDX_SHIFT = 3;//74HC238�C���f�b�N�X�̓�͒[�q��(A,B,C) +const int SRV_IDX_NUM = 1 << SRV_IDX_SHIFT;//�C���f�b�N�X�̐�(8) +const int SRV_IDX_MASK = SRV_IDX_NUM - 1;//�w��ch�̉�3bit����}�X�N���邽�߂̂�� +const int SRV_BANK_NUM = 3;//PWM���̖{�� +const int SRV_CH_NUM = SRV_IDX_NUM * SRV_BANK_NUM;//����”\�ȃT�[�{���[�^�� + +volatile short int SRV_Idx = 0;//�T�[�{�؂�ւ��p�̃J�E���^ +volatile short int SRV_dutyTable[SRV_BANK_NUM][SRV_IDX_NUM];//�f���[�e�B�[�l + +void Init_PWM(void); +void SetDuty(short int ch, short int duty); + +#endif //PWM_H_2012_08_28_ + +#ifdef __cplusplus +extern "C" { + void PWM1_IRQHandler(); +} +#endif + +int main(void) { + Init_PWM(); + + for(int i = 0; i < SRV_CH_NUM; i++) + { + SetDuty(i, (SRV_MAX_DUTY + SRV_MIN_DUTY) / 2); + } + + while (1); +} + +void PWM1_IRQHandler (void) { + LPC_PWM1->MR1 = SRV_PERIOD*(SRV_Idx&1); + LPC_PWM1->MR2 = SRV_PERIOD*(SRV_Idx&2); + LPC_PWM1->MR3 = SRV_PERIOD*(SRV_Idx&4); + LPC_PWM1->MR4 = SRV_dutyTable[0][SRV_Idx]; + LPC_PWM1->MR5 = SRV_dutyTable[1][SRV_Idx]; + LPC_PWM1->MR6 = SRV_dutyTable[2][SRV_Idx]; + + LPC_PWM1->IR |= (1<<0); // reset PWMMR0 flag + + SRV_Idx++; + + if(SRV_Idx >= SRV_IDX_NUM){ + SRV_Idx = 0; + } +} + +void Init_PWM(void) { + 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->TCR = (1 << 1);//reset + LPC_PWM1->PR = SystemCoreClock / 4 / 1000000 - 1; // prescale 1000kHz(1us����)�����񂲂ƂɃ^�C�}�J�E���^��J�E���g�A�b�v���邩�Ƃ����ݒ� + LPC_PWM1->CTCR = 0; + LPC_PWM1->MCR |= (1 << 1)|(1 << 0); // MR0R, MR0 + + LPC_PWM1->MR0 = SRV_PERIOD; // PWM(74HC238��G1)���2500us + LPC_PWM1->MR1 = SRV_PERIOD; // �؂�ւ����(74HC238��A) + LPC_PWM1->MR2 = SRV_PERIOD; // �؂�ւ����(74HC238��B) + LPC_PWM1->MR3 = SRV_PERIOD; // �؂�ւ����(74HC238��C) + LPC_PWM1->MR4 = (SRV_MAX_DUTY + SRV_MIN_DUTY) / 2; //PWM(74HC238��G1)�f���[�e�B�[(�j���[�g����) + LPC_PWM1->MR5 = (SRV_MAX_DUTY + SRV_MIN_DUTY) / 2; + LPC_PWM1->MR6 = (SRV_MAX_DUTY + SRV_MIN_DUTY) / 2; + + LPC_PWM1->IR |= (1<<0);//reset PWMMR0 flag + + NVIC_EnableIRQ(PWM1_IRQn);//enable + + LPC_PWM1->TCR = (1 << 0)|(1 << 3); // enable + LPC_PWM1->PCR |= (0x3F << 9);//1-6 pwm + + __enable_irq(); +} + +void SetDuty(short int ch, short int 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; +} \ No newline at end of file