diff --git a/include/adc_dma.h b/include/adc_dma.h
index 0fd1dc41c7b437f8177404d968c381193d708393..db29c8d33f3945d9ff928d1d50cc85c48f5db6fc 100755
--- a/include/adc_dma.h
+++ b/include/adc_dma.h
@@ -10,7 +10,7 @@ extern "C" {
#define ADC_NUM_CHANNELS 12
typedef enum {ADC_CH1=0,ADC_CH2=1,ADC_CH3=2,ADC_CH4=3,ADC_CH5=4,ADC_CH6=5,ADC_CH7=6,ADC_CH8=7,
- ADC_CH9=8,ADC_CH10=9,ADC_CH12=10,ADC_CH14=11} adc_ch_t;
+ ADC_CH9=8,ADC_CH10=9,ADC_CH12=10} adc_ch_t;
void adc_init(void);
void adc_start(void);
diff --git a/include/darwin_dyn_master_v2.h b/include/darwin_dyn_master_v2.h
index b4a1b34d01c20f6e411ad3902e9d86682697c99c..a7e5d190f4dc6a65dd022c5c8d2a59656915501f 100644
--- a/include/darwin_dyn_master_v2.h
+++ b/include/darwin_dyn_master_v2.h
@@ -12,6 +12,8 @@ extern "C" {
extern TDynamixelMaster darwin_dyn_master_v2;
void darwin_dyn_master_v2_init(void);
+inline void darwin_dyn_master_v2_enable_power(void);
+inline void darwin_dyn_master_v2_disable_power(void);
#ifdef __cplusplus
}
diff --git a/include/darwin_registers.h b/include/darwin_registers.h
index 8b5c1913202a02e71889b2a537a98d92ded6df50..6cf97e222fddecd38a478dbd4298510851220d53 100644
--- a/include/darwin_registers.h
+++ b/include/darwin_registers.h
@@ -254,10 +254,6 @@ typedef enum {
DARWIN_ADC_TEMP_H = 0x012C,
DARWIN_ADC_CH12_L = 0x012D,
DARWIN_ADC_CH12_H = 0x012E,
- DARWIN_ADC_VREF_L = 0x012F,
- DARWIN_ADC_VREF_H = 0x0130,
- DARWIN_ADC_CH14_L = 0x0131,
- DARWIN_ADC_CH14_H = 0x0132,
DARWIN_IMU_CNTRL = 0x013B, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
// accel_det | gyro_det | calibrating | running | | start_cal | stop | start
DARWIN_IMU_CAL_SAMPLES_L = 0x013C,
@@ -276,8 +272,8 @@ typedef enum {
DARWIN_IMU_ACCEL_Z_H = 0x0149,
DARWIN_MM_NUM_SERVOS = 0x014A,
- DARWIN_MM_CNTRL = 0x014B, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
- // scanning | fwd fall | bwd fall | | | Enable power | Enable balance | Enable manager
+ DARWIN_MM_CNTRL = 0x014B, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // scanning | fwd fall | bwd fall | | Enable power v2 | Enable power | Enable balance | Enable manager
DARWIN_MM_MODULE_EN0 = 0x014C, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0i
// Enable servo 0 | assigned module | Enable servo 1 | assigned module
// | 000 -> none |
@@ -630,6 +626,7 @@ typedef enum {
#define MANAGER_ENABLE 0x01
#define MANAGER_EN_BAL 0x02
#define MANAGER_EN_PWR 0x04
+#define MANAGER_EN_PWR_V2 0x08
#define MANAGER_SCANNING 0x80
#define MANAGER_FWD_FALL 0x40
#define MANAGER_BWD_FALL 0x20
diff --git a/include/smart_charger.h b/include/smart_charger.h
index 5d3587c7bc3748040028b426690c3840186c1f9b..42c3217353f678c2f0f4d82fc63bbc35732c5478 100644
--- a/include/smart_charger.h
+++ b/include/smart_charger.h
@@ -118,4 +118,4 @@ void smart_charger_process(void);
}
#endif
-#endif
\ No newline at end of file
+#endif
diff --git a/src/adc_dma.c b/src/adc_dma.c
index d24f466d8bdf495add858283b077871a3f177eb5..eb0f821a5e19c6faea28b2ff64462cbdfb3b4547 100755
--- a/src/adc_dma.c
+++ b/src/adc_dma.c
@@ -7,15 +7,13 @@
#define ADC1_CH4 ADC_CHANNEL_7
#define ADC1_CH5 ADC_CHANNEL_8
#define ADC1_CH6 ADC_CHANNEL_TEMPSENSOR
-#define ADC1_CH7 ADC_CHANNEL_VREFINT
#define ADC2_CH1 ADC_CHANNEL_9
#define ADC2_CH2 ADC_CHANNEL_10
#define ADC2_CH3 ADC_CHANNEL_11
#define ADC2_CH4 ADC_CHANNEL_12
-#define ADC2_CH5 ADC_CHANNEL_13
-#define ADC2_CH6 ADC_CHANNEL_14
-#define ADC2_CH7 ADC_CHANNEL_15
+#define ADC2_CH5 ADC_CHANNEL_14
+#define ADC2_CH6 ADC_CHANNEL_15
#define ADC1_CH1_PIN GPIO_PIN_0
#define ADC1_CH1_PORT GPIOA
@@ -47,15 +45,12 @@
#define ADC2_CH4_PIN GPIO_PIN_2
#define ADC2_CH4_PORT GPIOC
#define ADC2_CH4_ENABLE_PORT_CLK __HAL_RCC_GPIOC_CLK_ENABLE()
-#define ADC2_CH5_PIN GPIO_PIN_3
+#define ADC2_CH5_PIN GPIO_PIN_4
#define ADC2_CH5_PORT GPIOC
#define ADC2_CH5_ENABLE_PORT_CLK __HAL_RCC_GPIOC_CLK_ENABLE()
-#define ADC2_CH6_PIN GPIO_PIN_4
+#define ADC2_CH6_PIN GPIO_PIN_5
#define ADC2_CH6_PORT GPIOC
#define ADC2_CH6_ENABLE_PORT_CLK __HAL_RCC_GPIOC_CLK_ENABLE()
-#define ADC2_CH7_PIN GPIO_PIN_5
-#define ADC2_CH7_PORT GPIOC
-#define ADC2_CH7_ENABLE_PORT_CLK __HAL_RCC_GPIOC_CLK_ENABLE()
#define ADC2_ENABLE_CLK __ADC2_CLK_ENABLE()
@@ -82,7 +77,7 @@ ADC_HandleTypeDef hadc2;
DMA_HandleTypeDef hdma_adc1;
TIM_HandleTypeDef ADC_TIM_Handle;
-uint32_t adc_data[9];// temporal buffer to store ADC data before conversion
+uint32_t adc_data[6];// temporal buffer to store ADC data before conversion
uint16_t adc_period_ms;
// private functions
@@ -153,7 +148,7 @@ void ADC_DMA_IRQHandler(void)
if(__HAL_DMA_GET_IT_SOURCE(&hdma_adc1, DMA_IT_TC) != RESET)
{
__HAL_DMA_CLEAR_FLAG(&hdma_adc1, __HAL_DMA_GET_TC_FLAG_INDEX(&hdma_adc1));
- for(i=0;i<7;i++)
+ for(i=0;i<6;i++)
{
if(i==5)
{
@@ -202,7 +197,6 @@ void adc_init(void)
ADC2_CH4_ENABLE_PORT_CLK;
ADC2_CH5_ENABLE_PORT_CLK;
ADC2_CH6_ENABLE_PORT_CLK;
- ADC2_CH7_ENABLE_PORT_CLK;
ADC_ENABLE_DMA_CLK;
@@ -214,7 +208,7 @@ void adc_init(void)
hadc1.Init.DiscontinuousConvMode = DISABLE;
hadc1.Init.ExternalTrigConv = ADC_EXTERNALTRIGCONV_T1_CC1;
hadc1.Init.DataAlign = ADC_DATAALIGN_RIGHT;
- hadc1.Init.NbrOfConversion = 7;
+ hadc1.Init.NbrOfConversion = 6;
HAL_ADC_Init(&hadc1);
multimode.Mode = ADC_DUALMODE_REGSIMULT;
@@ -246,10 +240,6 @@ void adc_init(void)
sConfig.Rank = 6;
HAL_ADC_ConfigChannel(&hadc1, &sConfig);
- sConfig.Channel = ADC1_CH7;
- sConfig.Rank = 7;
- HAL_ADC_ConfigChannel(&hadc1, &sConfig);
-
// configure GPIO
GPIO_InitStruct.Pin = ADC1_CH1_PIN|ADC1_CH2_PIN|ADC1_CH3_PIN|ADC1_CH4_PIN;
GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
@@ -266,7 +256,7 @@ void adc_init(void)
hadc2.Init.DiscontinuousConvMode = DISABLE;
hadc2.Init.ExternalTrigConv = ADC_SOFTWARE_START;//ADC_EXTERNALTRIGCONV_T1_CC1;
hadc2.Init.DataAlign = ADC_DATAALIGN_RIGHT;
- hadc2.Init.NbrOfConversion = 7;
+ hadc2.Init.NbrOfConversion = 6;
HAL_ADC_Init(&hadc2);
// configure ADC2 channels
@@ -295,13 +285,9 @@ void adc_init(void)
sConfig.Rank = 6;
HAL_ADC_ConfigChannel(&hadc2, &sConfig);
- sConfig.Channel = ADC2_CH7;
- sConfig.Rank = 7;
- HAL_ADC_ConfigChannel(&hadc2, &sConfig);
-
// configure GPIO
GPIO_InitStruct.Pin = ADC2_CH2_PIN|ADC2_CH3_PIN|ADC2_CH4_PIN|ADC2_CH5_PIN|
- ADC2_CH6_PIN|ADC2_CH7_PIN;
+ ADC2_CH6_PIN;
GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
@@ -366,7 +352,7 @@ void adc_init(void)
HAL_ADCEx_Calibration_Start(&hadc2);
HAL_ADC_Start(&hadc2);
- HAL_ADCEx_MultiModeStart_DMA(&hadc1,adc_data,7);
+ HAL_ADCEx_MultiModeStart_DMA(&hadc1,adc_data,6);
}
void adc_start(void)
@@ -426,8 +412,6 @@ uint16_t adc_get_channel(adc_ch_t channel)
break;
case ADC_CH12: value=ram_data[DARWIN_ADC_CH12_L]+ram_data[DARWIN_ADC_CH12_H]*256;
break;
- case ADC_CH14: value=ram_data[DARWIN_ADC_CH14_L]+ram_data[DARWIN_ADC_CH14_H]*256;
- break;
default: value=0x0000;
}
@@ -462,8 +446,6 @@ uint16_t adc_get_channel_raw(adc_ch_t channel)
break;
case ADC_CH12: value=(adc_data[5]&0xFFFF0000)>>16;
break;
- case ADC_CH14: value=(adc_data[6]&0xFFFF0000)>>16;
- break;
default: value=0x0000;
}
diff --git a/src/darwin_dyn_master_v2.c b/src/darwin_dyn_master_v2.c
index 5dc5e5daf5ded2ca691a234b8c2dc9e1d6f9aae4..5c61157286f0d9323dab2590fa78b68a76c2227c 100755
--- a/src/darwin_dyn_master_v2.c
+++ b/src/darwin_dyn_master_v2.c
@@ -11,6 +11,10 @@
#define TX_EN_PIN GPIO_PIN_1
#define TX_EN_GPIO_PORT GPIOA
+#define POWER_GPIO_CLK __GPIOC_CLK_ENABLE()
+#define POWER_PIN GPIO_PIN_3
+#define POWER_GPIO_PORT GPIOC
+
/* private variables */
TDynamixelMaster darwin_dyn_master_v2;
TTime darwin_dyn_master_v2_timer;
@@ -81,3 +85,15 @@ void darwin_dyn_master_v2_init(void)
dyn_master_set_return_level(&darwin_dyn_master_v2,return_all);
}
+inline void darwin_dyn_master_v2_enable_power(void)
+{
+ HAL_GPIO_WritePin(POWER_GPIO_PORT,POWER_PIN,GPIO_PIN_SET);
+ ram_data[DARWIN_MM_CNTRL]|=MANAGER_EN_PWR_V2;
+}
+
+inline void darwin_dyn_master_v2_disable_power(void)
+{
+ HAL_GPIO_WritePin(POWER_GPIO_PORT,POWER_PIN,GPIO_PIN_RESET);
+ ram_data[DARWIN_MM_CNTRL]&=(~MANAGER_EN_PWR_V2);
+}
+
diff --git a/src/motion_manager.c b/src/motion_manager.c
index 65f06bf2d20f20592bc365d76ab4193dcb766fa5..4d61378490962f3f53f417208d94010dcee1a4fd 100755
--- a/src/motion_manager.c
+++ b/src/motion_manager.c
@@ -692,6 +692,10 @@ void manager_process_write_cmd(unsigned short int address,unsigned short int len
darwin_dyn_master_enable_power();
else
darwin_dyn_master_disable_power();
+ if(data[DARWIN_MM_CNTRL-address]&MANAGER_EN_PWR_V2)
+ darwin_dyn_master_v2_enable_power();
+ else
+ darwin_dyn_master_v2_disable_power();
}
// balance gains
for(i=MM_BAL_KNEE_GAIN_OFFSET;i<=MM_BAL_HIP_ROLL_GAIN_OFFSET+1;i++)