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

Source3.cpp@1:5c2e40445778, 2012-08-29 (annotated)

Committer:
syundo0730
Date:
Wed Aug 29 15:47:51 2012 +0000
Revision:
1:5c2e40445778
Parent:
0:d114194bff43
Child:
2:8a40834788ba
second version(not move)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
syundo0730 0:d114194bff43 1 #ifndef PWM_H_2012_08_28_
syundo0730 0:d114194bff43 2 #define PWM_H_2012_08_28_
syundo0730 0:d114194bff43 3
syundo0730 0:d114194bff43 4 #include "LPC17xx.h"
syundo0730 0:d114194bff43 5 #include "mbed.h"
syundo0730 0:d114194bff43 6
syundo0730 0:d114194bff43 7 const int SRV_PERIOD = 2500 - 1;//���/8
syundo0730 0:d114194bff43 8 const int SRV_MAX_DUTY = 2100 - 1;//�f���[�e�B�[�ő�l
syundo0730 0:d114194bff43 9 const int SRV_MIN_DUTY = 900 - 1;//�f���[�e�B�[�ŏ��l
syundo0730 0:d114194bff43 10
syundo0730 0:d114194bff43 11 const int SRV_IDX_SHIFT = 3;//74HC238�C���f�b�N�X�̓�͒[�q��(A,B,C)
syundo0730 0:d114194bff43 12 const int SRV_IDX_NUM = 1 << SRV_IDX_SHIFT;//&#65533;C&#65533;&#65533;&#65533;f&#65533;b&#65533;N&#65533;X&#65533;&#784;&#65533;(8)
syundo0730 0:d114194bff43 13 const int SRV_IDX_MASK = SRV_IDX_NUM - 1;//&#65533;w&#65533;&#65533;ch&#65533;&#777;&#65533;3bit&#65533;&#65533;&#65533;&#65533;}&#65533;X&#65533;N&#65533;&#65533;&#65533;&#37053;&#65533;&#1986;&#770;&#65533;&#65533;
syundo0730 0:d114194bff43 14 const int SRV_BANK_NUM = 3;//PWM&#65533;&#65533;&#65533;&#790;{&#65533;&#65533;
syundo0730 0:d114194bff43 15 const int SRV_CH_NUM = SRV_IDX_NUM * SRV_BANK_NUM;//&#65533;&#65533;&#65533;&#65533;&#148;\&#65533;&#515;T&#65533;[&#65533;{&#65533;&#65533;&#65533;[&#65533;^&#65533;&#65533;
syundo0730 0:d114194bff43 16
syundo0730 0:d114194bff43 17 volatile short int SRV_Idx = 0;//&#65533;T&#65533;[&#65533;{&#65533;&#1538;&#65533;&#1410;&#65533;&#65533;p&#65533;&#771;J&#65533;E&#65533;&#65533;&#65533;^
syundo0730 0:d114194bff43 18 volatile short int SRV_dutyTable[SRV_BANK_NUM][SRV_IDX_NUM];//&#65533;f&#65533;&#65533;&#65533;[&#65533;e&#65533;B&#65533;[&#65533;l
syundo0730 0:d114194bff43 19
syundo0730 1:5c2e40445778 20 const short int SRV_PWMTable[SRV_BANK_NUM][SRV_IDX_NUM]={
syundo0730 1:5c2e40445778 21 {0,SRV_PERIOD,0,SRV_PERIOD,0,SRV_PERIOD,0,SRV_PERIOD},
syundo0730 1:5c2e40445778 22 {0,0,SRV_PERIOD,SRV_PERIOD,0,0,SRV_PERIOD,SRV_PERIOD},
syundo0730 1:5c2e40445778 23 {0,0,0,0,SRV_PERIOD,SRV_PERIOD,SRV_PERIOD,SRV_PERIOD}
syundo0730 1:5c2e40445778 24 };
syundo0730 1:5c2e40445778 25
syundo0730 1:5c2e40445778 26
syundo0730 0:d114194bff43 27 void Init_PWM(void);
syundo0730 0:d114194bff43 28 void SetDuty(short int ch, short int duty);
syundo0730 0:d114194bff43 29
syundo0730 0:d114194bff43 30 #endif //PWM_H_2012_08_28_
syundo0730 0:d114194bff43 31
syundo0730 0:d114194bff43 32 #ifdef __cplusplus
syundo0730 0:d114194bff43 33 extern "C" {
syundo0730 0:d114194bff43 34 void PWM1_IRQHandler();
syundo0730 0:d114194bff43 35 }
syundo0730 0:d114194bff43 36 #endif
syundo0730 0:d114194bff43 37
syundo0730 0:d114194bff43 38 int main(void) {
syundo0730 0:d114194bff43 39
syundo0730 0:d114194bff43 40 for(int i = 0; i < SRV_CH_NUM; i++)
syundo0730 0:d114194bff43 41 {
syundo0730 0:d114194bff43 42 SetDuty(i, (SRV_MAX_DUTY + SRV_MIN_DUTY) / 2);
syundo0730 0:d114194bff43 43 }
syundo0730 0:d114194bff43 44
syundo0730 1:5c2e40445778 45 Init_PWM();
syundo0730 1:5c2e40445778 46
syundo0730 0:d114194bff43 47 while (1);
syundo0730 0:d114194bff43 48 }
syundo0730 0:d114194bff43 49
syundo0730 0:d114194bff43 50 void PWM1_IRQHandler (void) {
syundo0730 1:5c2e40445778 51 LPC_PWM1->TCR = (1 << 1);//reset
syundo0730 1:5c2e40445778 52 LPC_PWM1->PR = SystemCoreClock / 4 / 1000000 - 1; // prescale 1000kHz(1us&#65533;&#65533;&#65533;&#65533;)&#65533;&#65533;&#65533;&#65533;&#65533;&#273538;&#386;&#579;^&#65533;C&#65533;}&#65533;J&#65533;E&#65533;&#65533;&#65533;^&#65533;&#65533;J&#65533;E&#65533;&#65533;&#65533;g&#65533;A&#65533;b&#65533;v&#65533;&#65533;&#65533;&#37033;&#65533;&#386;&#65533;&#65533;&#65533;&#65533;&#1874;&#65533;
syundo0730 1:5c2e40445778 53 LPC_PWM1->CTCR = 0;
syundo0730 1:5c2e40445778 54 LPC_PWM1->MCR |= (1 << 1)|(1 << 0); // MR0R, MR0
syundo0730 1:5c2e40445778 55
syundo0730 1:5c2e40445778 56 LPC_PWM1->MR1 = SRV_PWMTable[0][SRV_Idx];
syundo0730 1:5c2e40445778 57 LPC_PWM1->MR2 = SRV_PWMTable[1][SRV_Idx];
syundo0730 1:5c2e40445778 58 LPC_PWM1->MR3 = SRV_PWMTable[2][SRV_Idx];
syundo0730 0:d114194bff43 59 LPC_PWM1->MR4 = SRV_dutyTable[0][SRV_Idx];
syundo0730 0:d114194bff43 60 LPC_PWM1->MR5 = SRV_dutyTable[1][SRV_Idx];
syundo0730 0:d114194bff43 61 LPC_PWM1->MR6 = SRV_dutyTable[2][SRV_Idx];
syundo0730 1:5c2e40445778 62
syundo0730 1:5c2e40445778 63 LPC_PWM1->TCR = (1 << 0)|(1 << 3); // enable
syundo0730 1:5c2e40445778 64 LPC_PWM1->PCR |= (0x3F << 9);//1-6 pwm
syundo0730 1:5c2e40445778 65
syundo0730 0:d114194bff43 66 LPC_PWM1->IR |= (1<<0); // reset PWMMR0 flag
syundo0730 0:d114194bff43 67
syundo0730 0:d114194bff43 68 SRV_Idx++;
syundo0730 0:d114194bff43 69
syundo0730 0:d114194bff43 70 if(SRV_Idx >= SRV_IDX_NUM){
syundo0730 0:d114194bff43 71 SRV_Idx = 0;
syundo0730 0:d114194bff43 72 }
syundo0730 0:d114194bff43 73 }
syundo0730 0:d114194bff43 74
syundo0730 0:d114194bff43 75 void Init_PWM(void) {
syundo0730 0:d114194bff43 76 LPC_PINCON->PINSEL3 &= ~(3 << 4); // GPIO (00)
syundo0730 0:d114194bff43 77 LPC_GPIO1->FIODIR |= (1 << 18); // output
syundo0730 0:d114194bff43 78
syundo0730 0:d114194bff43 79 LPC_PINCON->PINSEL4 |= 0x555; // GPIO (00)
syundo0730 0:d114194bff43 80 LPC_GPIO2->FIODIR |= 0x3F; // output
syundo0730 0:d114194bff43 81
syundo0730 0:d114194bff43 82 LPC_SC->PCLKSEL0 &= ~(3 << 12); // PCLK_TIMER0 ck/4 (00)
syundo0730 0:d114194bff43 83 LPC_SC->PCONP |= (1 << 6); // PCPWM1
syundo0730 0:d114194bff43 84 LPC_PWM1->TCR = (1 << 1);//reset
syundo0730 0:d114194bff43 85 LPC_PWM1->PR = SystemCoreClock / 4 / 1000000 - 1; // prescale 1000kHz(1us&#65533;&#65533;&#65533;&#65533;)&#65533;&#65533;&#65533;&#65533;&#65533;&#273538;&#386;&#579;^&#65533;C&#65533;}&#65533;J&#65533;E&#65533;&#65533;&#65533;^&#65533;&#65533;J&#65533;E&#65533;&#65533;&#65533;g&#65533;A&#65533;b&#65533;v&#65533;&#65533;&#65533;&#37033;&#65533;&#386;&#65533;&#65533;&#65533;&#65533;&#1874;&#65533;
syundo0730 0:d114194bff43 86 LPC_PWM1->CTCR = 0;
syundo0730 0:d114194bff43 87 LPC_PWM1->MCR |= (1 << 1)|(1 << 0); // MR0R, MR0
syundo0730 0:d114194bff43 88
syundo0730 0:d114194bff43 89 LPC_PWM1->MR0 = SRV_PERIOD; // PWM(74HC238&#65533;&#65533;G1)&#65533;&#65533;&#65533;2500us
syundo0730 1:5c2e40445778 90 //LPC_PWM1->MR1 = SRV_MAX_DUTY;//SRV_PERIOD; // &#65533;&#1538;&#65533;&#1410;&#65533;&#65533;&#65533;&#65533;(74HC238&#65533;&#65533;A)1
syundo0730 1:5c2e40445778 91 //LPC_PWM1->MR2 = SRV_MAX_DUTY;//SRV_PERIOD; // &#65533;&#1538;&#65533;&#1410;&#65533;&#65533;&#65533;&#65533;(74HC238&#65533;&#65533;B)
syundo0730 1:5c2e40445778 92 //LPC_PWM1->MR3 = SRV_MAX_DUTY;//SRV_PERIOD; // &#65533;&#1538;&#65533;&#1410;&#65533;&#65533;&#65533;&#65533;(74HC238&#65533;&#65533;C)
syundo0730 1:5c2e40445778 93 //LPC_PWM1->MR4 = SRV_MAX_DUTY;//(SRV_MAX_DUTY + SRV_MIN_DUTY) / 2; //PWM(74HC238&#65533;&#65533;G1)&#65533;f&#65533;&#65533;&#65533;[&#65533;e&#65533;B&#65533;[(&#65533;j&#65533;&#65533;&#65533;[&#65533;g&#65533;&#65533;&#65533;&#65533;)
syundo0730 1:5c2e40445778 94 //LPC_PWM1->MR5 = SRV_MAX_DUTY;//(SRV_MAX_DUTY + SRV_MIN_DUTY) / 2;
syundo0730 1:5c2e40445778 95 //LPC_PWM1->MR6 = SRV_MAX_DUTY;//(SRV_MAX_DUTY + SRV_MIN_DUTY) / 2;
syundo0730 0:d114194bff43 96
syundo0730 0:d114194bff43 97 LPC_PWM1->IR |= (1<<0);//reset PWMMR0 flag
syundo0730 0:d114194bff43 98
syundo0730 0:d114194bff43 99 NVIC_EnableIRQ(PWM1_IRQn);//enable
syundo0730 0:d114194bff43 100
syundo0730 0:d114194bff43 101 LPC_PWM1->TCR = (1 << 0)|(1 << 3); // enable
syundo0730 0:d114194bff43 102 LPC_PWM1->PCR |= (0x3F << 9);//1-6 pwm
syundo0730 0:d114194bff43 103
syundo0730 0:d114194bff43 104 __enable_irq();
syundo0730 0:d114194bff43 105 }
syundo0730 0:d114194bff43 106
syundo0730 0:d114194bff43 107 void SetDuty(short int ch, short int duty){
syundo0730 0:d114194bff43 108 if(ch >= SRV_CH_NUM){
syundo0730 0:d114194bff43 109 return;
syundo0730 0:d114194bff43 110 }else if(duty < SRV_MIN_DUTY){
syundo0730 0:d114194bff43 111 duty = SRV_MIN_DUTY;
syundo0730 0:d114194bff43 112 }else if(duty > SRV_MAX_DUTY){
syundo0730 0:d114194bff43 113 duty = SRV_MAX_DUTY;
syundo0730 0:d114194bff43 114 }
syundo0730 0:d114194bff43 115 SRV_dutyTable[ch >> SRV_IDX_SHIFT][ch & SRV_IDX_MASK] = duty;
syundo0730 0:d114194bff43 116 }