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

Files at this revision

API Documentation at this revision

Revision 6:fae1b8c01b49, committed 2012-09-18

Comitter:
syundo0730
Date:
Tue Sep 18 15:49:17 2012 +0000
Parent:
5:0ca9f1bc1fb0
Parent:
4:364582b47aab
Commit message:
opp

Changed in this revision

PWM/pwm.cpp Show annotated file Show diff for this revision Revisions of this file
PWM/pwm.h Show annotated file Show diff for this revision Revisions of this file
Source3.cpp Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file 
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PWM/pwm.cpp	Tue Sep 18 15:49:17 2012 +0000
@@ -0,0 +1,76 @@
+#include "pwm.h"
+
+LBC_pwm::LBC_pwm()
+{
+    //dummy
+}
+
+void LBC_pwm::SetPWM(void)
+{
+    for(int i = 0; i < SRV_CH_NUM; i++) {
+        SetDuty(i, (SRV_MAX_DUTY + SRV_MIN_DUTY) / 2);
+    }
+}
+
+void LBC_pwm::InitPWM(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->IR |= (1<<0);//reset PWMMR0 flag
+    LPC_PWM1->TCR = (1 << 1);//reset
+    LPC_PWM1->PR = SystemCoreClock / 4 / 1000000 - 1; // prescale 1000kHz
+    LPC_PWM1->MCR |= (1 << 1)|(1 << 0); // MR0R, MR0
+
+    LPC_PWM1->MR0 = SRV_PERIOD;
+    
+    LPC_PWM1->TCR = (1 << 0)|(1 << 3); // enable
+    LPC_PWM1->PCR |= (0x3F << 9);//1-6 pwm
+    LPC_PWM1->LER |= 0x7E;
+
+    //__enable_irq();
+    //__disable_irq();
+    //NVIC_DisableIRQ(PWM1_IRQn);
+    
+    NVIC_SetPriority(PWM1_IRQn,0);//priority level (high)0~255(low)
+    NVIC_EnableIRQ(PWM1_IRQn);//enable
+}
+void LBC_pwm::SetDuty(uint8_t ch, uint32_t 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;
+}
+
+void PWM1_IRQHandler (void)
+{
+    
+    
+    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->LER |= 0x7E;
+    
+    LPC_PWM1->IR |= (1<<0); // reset PWMMR0 flag
+    
+    SRV_Idx++;
+
+    if(SRV_Idx >= SRV_IDX_NUM) {
+        SRV_Idx = 0;
+    }
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PWM/pwm.h	Tue Sep 18 15:49:17 2012 +0000
@@ -0,0 +1,49 @@
+#ifndef PWM_H_2012_08_28_
+#define PWM_H_2012_08_28_
+
+#include "LPC17xx.h"
+#include "mbed.h"
+
+/**************global values******************************/
+#define SRV_PERIOD  2500 - 1;
+#define SRV_MAX_DUTY  2100 - 1;
+#define SRV_MIN_DUTY  900 - 1;  
+#define SRV_IDX_SHIFT  3;
+#define SRV_IDX_NUM  1 << SRV_IDX_SHIFT;
+#define SRV_IDX_MASK  SRV_IDX_NUM - 1;
+#define SRV_BANK_NUM  3;
+#define SRV_CH_NUM  SRV_IDX_NUM * SRV_BANK_NUM;
+
+volatile uint8_t SRV_Idx = 0;
+volatile uint32_t SRV_dutyTable[SRV_BANK_NUM][SRV_IDX_NUM];
+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}
+    };
+extern volatile uint8_t SRV_Idx;
+extern volatile uint32_t SRV_dutyTable[SRV_BANK_NUM][SRV_IDX_NUM];
+extern const uint32_t SRV_PWMTable[SRV_BANK_NUM][SRV_IDX_NUM];
+/*********************************************************/
+
+/*******This Class modify global value********************/
+class LBC_pwm {
+  private:
+    LBC_pwm();
+
+  public:
+    void InitPWM(void);
+    void SetPWM(void);
+    void SetDuty(uint8_t ch, uint32_t duty);
+}
+/*********************************************************/
+
+//#ifdef __cplusplus
+//extern "C" {
+//#endif
+    void PWM1_IRQHandler();
+//#ifdef __cplusplus
+//}
+//#endif
+
+#endif  //PWM_H_2012_08_28_
\ No newline at end of file

--- a/Source3.cpp	Tue Sep 18 06:48:47 2012 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,126 +0,0 @@
-#ifndef PWM_H_2012_08_28_
-#define PWM_H_2012_08_28_
-
-#include "LPC17xx.h"
-#include "mbed.h"
-//#include "PowerControl.h"
-
-const uint32_t SRV_PERIOD = 2500 - 1;//
-const uint32_t SRV_MAX_DUTY = 2100 - 1;//
-const uint32_t SRV_MIN_DUTY = 900 - 1;//
-
-const uint8_t SRV_IDX_SHIFT = 3;//
-const uint8_t SRV_IDX_NUM = 1 << SRV_IDX_SHIFT;//
-const uint8_t SRV_IDX_MASK = SRV_IDX_NUM - 1;//
-const uint8_t SRV_BANK_NUM = 3;//
-const uint8_t SRV_CH_NUM = SRV_IDX_NUM * SRV_BANK_NUM;//
-
-volatile uint8_t SRV_Idx = 0;//
-volatile uint32_t SRV_dutyTable[SRV_BANK_NUM][SRV_IDX_NUM];//
-
-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}
-};
-
-void Init_PWM(void);
-void SetDuty(uint8_t ch, uint32_t duty);
-void main_th(void);
-
-#endif  //PWM_H_2012_08_28_
-
-#ifdef __cplusplus
-extern "C" {
-    void PWM1_IRQHandler();
-}
-#endif
-
-Ticker flip;
-
-int main(void) {
-    
-    for(int i = 0; i < SRV_CH_NUM; i++)
-    {
-        SetDuty(i, (SRV_MAX_DUTY + SRV_MIN_DUTY) / 2);
-    }
-    
-    Init_PWM();
-    flip.attach(&main_th,5.0);
-    
-    while (1) {
-        //SetDuty(1,SRV_MAX_DUTY);
-        //sleep();
-    }
-}
-
-void main_th(void) {
-    SetDuty(1,0);
-}
-
-void PWM1_IRQHandler (void) {
-    
-    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->LER |= 0x7E;
-    
-    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->IR |= (1<<0);//reset PWMMR0 flag
-    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->MR0 = SRV_PERIOD; // PWM(74HC238&#65533;&#65533;G1)&#65533;&#65533;&#65533;2500us
-    //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->TCR = (1 << 0)|(1 << 3); // enable
-    LPC_PWM1->PCR |= (0x3F << 9);//1-6 pwm
-    LPC_PWM1->LER |= 0x7E;
-
-    //__enable_irq();
-    //__disable_irq();
-    //NVIC_DisableIRQ(PWM1_IRQn);
-    
-    NVIC_SetPriority(PWM1_IRQn,0);//priority level (high)0~255(low)
-    NVIC_EnableIRQ(PWM1_IRQn);//enable
-}
-
-void SetDuty(uint8_t ch, uint32_t 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

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Sep 18 15:49:17 2012 +0000
@@ -0,0 +1,18 @@
+#include "LPC17xx.h"
+#include "mbed.h"
+#include "pwm.h"
+
+Ticker flip;
+LBC_pwm pwm;
+
+void main_th(void) {
+    SetDuty(1,0);
+}
+
+int main(void) {
+    
+    InitPWM();
+    flip.attach(&main_th,5.0);
+    
+    while (1);
+}
\ No newline at end of file