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:
1:5c2e40445778
Parent:
0:d114194bff43
Child:
2:8a40834788ba
--- a/Source3.cpp	Tue Aug 28 15:57:11 2012 +0000
+++ b/Source3.cpp	Wed Aug 29 15:47:51 2012 +0000
@@ -17,6 +17,13 @@
 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
 
+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}
+};
+
+
 void Init_PWM(void);
 void SetDuty(short int ch, short int duty);
 
@@ -29,24 +36,33 @@
 #endif
 
 int main(void) {
-    Init_PWM();
     
     for(int i = 0; i < SRV_CH_NUM; i++)
     {
         SetDuty(i, (SRV_MAX_DUTY + SRV_MIN_DUTY) / 2);
     }
     
+    Init_PWM();
+    
     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->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->MR1 = 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++;
@@ -71,12 +87,12 @@
     LPC_PWM1->MCR |= (1 << 1)|(1 << 0); // MR0R, MR0
 
     LPC_PWM1->MR0 = SRV_PERIOD; // PWM(74HC238&#65533;&#65533;G1)&#65533;&#65533;&#65533;2500us
-    LPC_PWM1->MR1 = SRV_PERIOD; // &#65533;&#1538;&#65533;&#1410;&#65533;&#65533;&#65533;&#65533;(74HC238&#65533;&#65533;A)
-    LPC_PWM1->MR2 = SRV_PERIOD; // &#65533;&#1538;&#65533;&#1410;&#65533;&#65533;&#65533;&#65533;(74HC238&#65533;&#65533;B)
-    LPC_PWM1->MR3 = SRV_PERIOD; // &#65533;&#1538;&#65533;&#1410;&#65533;&#65533;&#65533;&#65533;(74HC238&#65533;&#65533;C)
-    LPC_PWM1->MR4 = (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;)
-    LPC_PWM1->MR5 = (SRV_MAX_DUTY + SRV_MIN_DUTY) / 2;
-    LPC_PWM1->MR6 = (SRV_MAX_DUTY + SRV_MIN_DUTY) / 2;
+    //LPC_PWM1->MR1 = SRV_MAX_DUTY;//SRV_PERIOD; // &#65533;&#1538;&#65533;&#1410;&#65533;&#65533;&#65533;&#65533;(74HC238&#65533;&#65533;A)1
+    //LPC_PWM1->MR2 = SRV_MAX_DUTY;//SRV_PERIOD; // &#65533;&#1538;&#65533;&#1410;&#65533;&#65533;&#65533;&#65533;(74HC238&#65533;&#65533;B)
+    //LPC_PWM1->MR3 = SRV_MAX_DUTY;//SRV_PERIOD; // &#65533;&#1538;&#65533;&#1410;&#65533;&#65533;&#65533;&#65533;(74HC238&#65533;&#65533;C)
+    //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;)
+    //LPC_PWM1->MR5 = SRV_MAX_DUTY;//(SRV_MAX_DUTY + SRV_MIN_DUTY) / 2;
+    //LPC_PWM1->MR6 = SRV_MAX_DUTY;//(SRV_MAX_DUTY + SRV_MIN_DUTY) / 2;
 
     LPC_PWM1->IR |= (1<<0);//reset PWMMR0 flag