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

Diff: Source3.cpp

Revision:
2:8a40834788ba
Parent:
1:5c2e40445778
Child:
3:84e5335fab91
--- a/Source3.cpp	Wed Aug 29 15:47:51 2012 +0000
+++ b/Source3.cpp	Thu Aug 30 02:00:58 2012 +0000
@@ -4,28 +4,34 @@
 #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 uint32_t SRV_PERIOD = 2500 - 1;//���/8
+const uint32_t SRV_MAX_DUTY = 2100 - 1;//�f���[�e�B�[�ő�l
+const uint32_t SRV_MIN_DUTY = 900 - 1;//�f���[�e�B�[�ŏ��l
+
+const uint8_t SRV_IDX_SHIFT = 3;//74HC238�C���f�b�N�X�̓�͒[�q��(A,B,C)
+const uint8_t SRV_IDX_NUM = 1 << SRV_IDX_SHIFT;//&#65533;C&#65533;&#65533;&#65533;f&#65533;b&#65533;N&#65533;X&#65533;&#784;&#65533;(8)
+const uint8_t 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;
+const uint8_t SRV_BANK_NUM = 3;//PWM&#65533;&#65533;&#65533;&#790;{&#65533;&#65533;
+const uint8_t 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;
 
-const int SRV_IDX_SHIFT = 3;//74HC238&#65533;C&#65533;&#65533;&#65533;f&#65533;b&#65533;N&#65533;X&#65533;&#787;&#65533;&#850;[&#65533;q&#65533;&#65533;(A,B,C)
-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)
-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;
-const int SRV_BANK_NUM = 3;//PWM&#65533;&#65533;&#65533;&#790;{&#65533;&#65533;
-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;
+volatile uint8_t SRV_Idx = 0;//&#65533;T&#65533;[&#65533;{&#65533;&#1538;&#65533;&#1410;&#65533;&#65533;p&#65533;&#771;J&#65533;E&#65533;&#65533;&#65533;^
+volatile uint32_t SRV_dutyTable[SRV_BANK_NUM][SRV_IDX_NUM];//&#65533;f&#65533;&#65533;&#65533;[&#65533;e&#65533;B&#65533;[&#65533;l
 
-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;^
-volatile short int SRV_dutyTable[SRV_BANK_NUM][SRV_IDX_NUM];//&#65533;f&#65533;&#65533;&#65533;[&#65533;e&#65533;B&#65533;[&#65533;l
+//const uint32_t SRV_PWMTable[SRV_BANK_NUM][SRV_IDX_NUM]={
+//{0,SRV_PERIOD,0,SRV_PERIOD,0,SRV_PERIOD,0,SRV_PERIOD},
+//{0,0,SRV_PERIOD,SRV_PERIOD,0,0,SRV_PERIOD,SRV_PERIOD},
+//{0,0,0,0,SRV_PERIOD,SRV_PERIOD,SRV_PERIOD,SRV_PERIOD}
+//};
 
-const short int SRV_PWMTable[SRV_BANK_NUM][SRV_IDX_NUM]={
-{0,SRV_PERIOD,0,SRV_PERIOD,0,SRV_PERIOD,0,SRV_PERIOD},
-{0,0,SRV_PERIOD,SRV_PERIOD,0,0,SRV_PERIOD,SRV_PERIOD},
-{0,0,0,0,SRV_PERIOD,SRV_PERIOD,SRV_PERIOD,SRV_PERIOD}
+const uint32_t SRV_PWMTable[SRV_BANK_NUM][SRV_IDX_NUM]={
+{0,2500,0,2500,0,2500,0,2500},
+{0,0,2500,2500,0,0,2500,2500},
+{0,0,0,0,2500,2500,2500,2500}
 };
 
 
 void Init_PWM(void);
-void SetDuty(short int ch, short int duty);
+void SetDuty(uint8_t ch, uint32_t duty);
 
 #endif  //PWM_H_2012_08_28_
 
@@ -48,21 +54,15 @@
 }
 
 void PWM1_IRQHandler (void) {
-    LPC_PWM1->TCR = (1 << 1);//reset
-    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;
-    LPC_PWM1->CTCR = 0;
-    LPC_PWM1->MCR |= (1 << 1)|(1 << 0); // MR0R, MR0
+    LPC_PWM1->LER |= 0x7E;
     
-    LPC_PWM1->MR1 = SRV_PWMTable[0][SRV_Idx];
+    LPC_PWM1->MR1 = 500;//SRV_PWMTable[0][SRV_Idx];
     LPC_PWM1->MR2 = SRV_PWMTable[1][SRV_Idx];
     LPC_PWM1->MR3 = SRV_PWMTable[2][SRV_Idx];
     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->TCR = (1 << 0)|(1 << 3); // enable
-    LPC_PWM1->PCR |= (0x3F << 9);//1-6 pwm
-    
     LPC_PWM1->IR |= (1<<0); // reset PWMMR0 flag
     
     SRV_Idx++;
@@ -104,7 +104,7 @@
     __enable_irq();
 }
 
-void SetDuty(short int ch, short int duty){
+void SetDuty(uint8_t ch, uint32_t duty){
     if(ch >= SRV_CH_NUM){
         return;
     }else if(duty < SRV_MIN_DUTY){