Solved a bug in the sync write operation.

Aolved a bug with the NVIC peripheral.

include/ram.h

@@ -107,10 +107,25 @@ typedef enum {
   BIOLOID_IMU_CONTROL             = 0x6D,
   BIOLOID_IMU_ACCEL_X_L           = 0x6E,
   BIOLOID_IMU_ACCEL_X_H           = 0x6F,
-  BIOLOID_IMU_ACCEL_Y_L           = 0x7A,
-  BIOLOID_IMU_ACCEL_Y_H           = 0x7B,
-  BIOLOID_IMU_ACCEL_Z_L           = 0x7C,
-  BIOLOID_IMU_ACCEL_Z_H           = 0x7D
+  BIOLOID_IMU_ACCEL_Y_L           = 0x70,
+  BIOLOID_IMU_ACCEL_Y_H           = 0x71,
+  BIOLOID_IMU_ACCEL_Z_L           = 0x72,
+  BIOLOID_IMU_ACCEL_Z_H           = 0x73,
+  BIOLOID_IMU_GYRO_X_L            = 0x74,
+  BIOLOID_IMU_GYRO_X_H            = 0x75,
+  BIOLOID_IMU_GYRO_Y_L            = 0x76,
+  BIOLOID_IMU_GYRO_Y_H            = 0x77,
+  BIOLOID_IMU_GYRO_Z_L            = 0x78,
+  BIOLOID_IMU_GYRO_Z_H            = 0x79,
+  BIOLOID_IMU_COMPASS_X_L         = 0x7A,
+  BIOLOID_IMU_COMPASS_X_H         = 0x7B,
+  BIOLOID_IMU_COMPASS_Y_L         = 0x7C,
+  BIOLOID_IMU_COMPASS_Y_H         = 0x7D,
+  BIOLOID_IMU_COMPASS_Z_L         = 0x7E,
+  BIOLOID_IMU_COMPASS_Z_H         = 0x7F,
+  BIOLOID_ACTION_PAGE             = 0x80,
+  BIOLOID_ACTION_CONTROL          = 0x81,
+  BIOLOID_ACTION_SATUTUS          = 0x82
 } bioloid_registers;
 
 #define      RAM_SIZE          256

src/action.c

@@ -190,7 +190,7 @@ void action_init(uint16_t period)
   {
     // angle variables
     action_moving_angles[i]=0;// fixed point 48|16 format
-    action_current_angles[i]=0;// middle of range
+    //action_current_angles[i]=0;// middle of range
     // speed variables
     action_start_speed[i]=0;// fixed point 48|16 format
     action_main_speed[i]=0;// fixed point 48|16 format

src/bioloid_stm32.c

@@ -61,7 +61,7 @@ uint8_t write_operation(uint8_t address,uint8_t length, uint8_t *data)
     word_value=(word_value*1000000)>>16;
     manager_set_period(word_value);
   }  
-  for(i=BIOLOID_MM_MODULE_EN0,j=0;i<BIOLOID_MM_MODULE_EN15;i++,j++)
+  for(i=BIOLOID_MM_MODULE_EN0,j=0;i<=BIOLOID_MM_MODULE_EN15;i++,j+=2)
   {
     if(ram_in_range(i,i,address,length))
     {
@@ -94,6 +94,15 @@ uint8_t write_operation(uint8_t address,uint8_t length, uint8_t *data)
   // write operation
   error=ram_write_table(address,length,data);
   // post-write operation
+  if(ram_in_range(BIOLOID_ACTION_PAGE,BIOLOID_ACTION_PAGE,address,length))// load the page identifier 
+    action_load_page(data[address-BIOLOID_ACTION_PAGE]);
+  if(ram_in_range(BIOLOID_ACTION_CONTROL,BIOLOID_ACTION_CONTROL,address,length))
+  {
+    if(data[address-BIOLOID_ACTION_CONTROL]&0x01)
+      action_start_page();
+    if(data[address-BIOLOID_ACTION_CONTROL]&0x02)
+      action_stop_page();
+  }
 
   return error;
 }
@@ -126,7 +135,7 @@ int32_t main(void)
   EE_ReadVariable(RETURN_LEVEL_OFFSET,&eeprom_data);
   dyn_slave_set_return_level((uint8_t)eeprom_data);
   /* initialize the IMU */
-  imu_init();
+//  imu_init();
   // initialize the Analog to digital converter
 //  adc_init();
   // initialize motion manager

src/dynamixel_master_uart_dma.c

@@ -69,6 +69,8 @@ uint8_t dyn_master_tx_buffer[MAX_BUFFER_LEN];
 volatile uint8_t dyn_master_packet_ready;
 // sending status packet flag
 volatile uint8_t dyn_master_sending_packet;
+// no answer for sync write operation
+uint8_t dyn_master_no_answer;
 // DMA initialization data structures
 DMA_InitTypeDef  DMA_TX_InitStructure;
 DMA_InitTypeDef  DMA_RX_InitStructure;
@@ -106,8 +108,8 @@ uint8_t dyn_master_receive(void)
   // wait for the status packet
   while(!dyn_master_packet_ready)
   {
-    delay_ms(10);
-    timeout_left-=10;
+    delay_ms(1);
+    timeout_left-=1;
     if(timeout_left<=0)
     {
       dyn_master_reset();
@@ -212,14 +214,19 @@ void USART_IRQHandler(void)
     USART_ClearFlag(USART,USART_FLAG_TC);
     USART_ITConfig(USART, USART_IT_TC, DISABLE);
     dyn_master_sending_packet=0x00;
-    // set up the DMA RX transaction
-    DMA_RX_InitStructure.DMA_BufferSize = 4;
-    DMA_RX_InitStructure.DMA_Memory0BaseAddr = (uint32_t)dyn_master_rx_buffer;
-    DMA_Init(USART_RX_DMA_STREAM,&DMA_RX_InitStructure);
-    DMA_ITConfig(USART_RX_DMA_STREAM,DMA_IT_TC,ENABLE);
-    DMA_Cmd(USART_RX_DMA_STREAM,ENABLE);
-    USART_DMACmd(USART, USART_DMAReq_Rx, ENABLE);
-    dyn_master_receiving_header=0x01;
+    if(dyn_master_no_answer)
+      dyn_master_no_answer=0x00;
+    else
+    {
+      // set up the DMA RX transaction
+      DMA_RX_InitStructure.DMA_BufferSize = 4;
+      DMA_RX_InitStructure.DMA_Memory0BaseAddr = (uint32_t)dyn_master_rx_buffer;
+      DMA_Init(USART_RX_DMA_STREAM,&DMA_RX_InitStructure);
+      DMA_ITConfig(USART_RX_DMA_STREAM,DMA_IT_TC,ENABLE);
+      DMA_Cmd(USART_RX_DMA_STREAM,ENABLE);
+      USART_DMACmd(USART, USART_DMAReq_Rx, ENABLE);
+      dyn_master_receiving_header=0x01;
+    }
   }
 }
 
@@ -307,6 +314,7 @@ void dyn_master_init(void)
   dyn_master_packet_ready=0x00;
   dyn_master_sending_packet=0x00;
   dyn_master_receiving_header=0x01;
+  dyn_master_no_answer=0x00;
 
   USART_DeInit(USART);
   USART_StructInit(&USART_InitStructure);
@@ -493,6 +501,7 @@ void dyn_master_sync_write(uint8_t num,uint8_t *ids,uint8_t address, uint8_t len
 {
   uint8_t i;
 
+  dyn_master_no_answer=0x01;
   // generate the sync write packet 
   dyn_init_instruction_packet(dyn_master_tx_buffer);
   // set the ping instruction

src/eeprom.c

@@ -31,7 +31,7 @@
 #define    EEPROM_SIZE               0x09
 #define    DEFAULT_DEVICE_MODEL      0x7300
 #define    DEFAULT_FIRMWARE_VERSION  0x13
-#define    DEFAULT_DEVICE_ID         0x0D
+#define    DEFAULT_DEVICE_ID         0xC0
 #define    DEFAULT_BAUDRATE          0x01
 #define    DEFAULT_RETURN_DELAY      0x00
 #define    DEFAULT_MM_PERIOD         0x01FF

src/motion_manager.c

@@ -3,12 +3,15 @@
 #include "dyn_servos.h"
 #include "gpio.h"
 #include "ram.h"
+#include "action.h"
 
 #define      MOTION_TIMER                 TIM3
 #define      MOTION_TIMER_IRQn            TIM3_IRQn
 #define      MOTION_TIMER_IRQHandler      TIM3_IRQHandler
 #define      MOTION_TIMER_CLK             RCC_APB1Periph_TIM3
 
+extern int64_t action_current_angles[MANAGER_MAX_NUM_SERVOS];
+
 // private variables
 __IO uint16_t manager_motion_period;
 uint8_t manager_num_servos;
@@ -19,17 +22,20 @@ void manager_send_motion_command(void)
 {
   uint8_t servo_ids[MANAGER_MAX_NUM_SERVOS];
   uint8_t data[2048];
-  uint16_t value;
-  uint8_t i;
+  uint8_t i,num=0;
   
   for(i=0;i<manager_num_servos;i++)
   {
-//    servo_ids[i]=manager_servos[i].id;
-//    value=manager_get_index_value(i);
-    data[i*2]=value%256;
-    data[i*2+1]=value/256;
+    if(manager_servos[i].enabled)
+    {
+      servo_ids[num]=manager_servos[i].id;
+      data[num*2]=manager_servos[i].current_value%256;
+      data[num*2+1]=manager_servos[i].current_value/256;
+      num++;
+    }
   }
-  dyn_master_sync_write(manager_num_servos,servo_ids,P_GOAL_POSITION_L,2,data);
+  if(num>0)
+    dyn_master_sync_write(manager_num_servos,servo_ids,P_GOAL_POSITION_L,2,data);
 }
 
 inline uint16_t manager_angle_to_value(uint8_t servo_id,int16_t angle)
@@ -58,20 +64,30 @@ void manager_get_current_position(void)
 {
   uint8_t i;
 
-  for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
+  for(i=0;i<manager_num_servos;i++)
   {
-    if(manager_servos[i].model!=0x0000)// servo is present
+    if(!manager_servos[i].enabled)// servo is enabled
     {
-      if(manager_servos[i].enabled)// servo is enabled
-      {
-        // the current position is fixed by one of the motion modules
-      }
-      else// servo is disabled
+      dyn_master_read_word(manager_servos[i].id,P_PRESENT_POSITION_L,&manager_servos[i].current_value);
+      manager_servos[i].current_angle=manager_value_to_angle(i,manager_servos[i].current_value);
+    }
+    ram_data[BIOLOID_MM_SERVO0_CUR_POS_L+i*2]=(manager_servos[i].current_angle&0xFF);
+    ram_data[BIOLOID_MM_SERVO0_CUR_POS_H+i*2]=(manager_servos[i].current_angle>>8);
+  }
+}
+
+void manager_get_target_position(void)
+{
+  uint8_t i;
+
+  for(i=0;i<manager_num_servos;i++)
+  {
+    if(manager_servos[i].enabled)// servo is enabled
+    {
+      if(manager_servos[i].module==MM_ACTION)
       {
-        dyn_master_read_word(manager_servos[i].id,P_PRESENT_POSITION_L,&manager_servos[i].current_value);
-        manager_servos[i].current_angle=manager_value_to_angle(i,manager_servos[i].current_value);
-        ram_data[BIOLOID_MM_SERVO0_CUR_POS_L+i*2]=(manager_servos[i].current_angle&0xFF);
-        ram_data[BIOLOID_MM_SERVO0_CUR_POS_H+i*2]=(manager_servos[i].current_angle>>8);
+        manager_servos[i].current_angle=((action_current_angles[i+1])>>16);
+        manager_servos[i].current_value=manager_angle_to_value(i,manager_servos[i].current_angle);
       }
     }
   }
@@ -87,6 +103,8 @@ void MOTION_TIMER_IRQHandler(void)
     TIM_ClearITPendingBit(MOTION_TIMER,TIM_IT_CC1);
     capture = TIM_GetCapture1(MOTION_TIMER);
     TIM_SetCompare1(MOTION_TIMER, capture + manager_motion_period);
+    // get the target angles from all modules
+    // manager_get_target_position();
     // call the action process
     // action_process();
     // call the joint motion process
@@ -96,8 +114,9 @@ void MOTION_TIMER_IRQHandler(void)
     // balance the robot
     // manager_balance();
     // send the motion commands to the servos
-    // manager_send_motion_command();
-    manager_get_current_position();
+    manager_send_motion_command();
+    // get the disabled servos position
+    // manager_get_current_position();
   }
 }
 
@@ -195,6 +214,8 @@ void manager_init(uint16_t period_us)
       manager_servos[i].current_angle=manager_value_to_angle(i,manager_servos[i].current_value);
       ram_data[BIOLOID_MM_SERVO0_CUR_POS_L+i*2]=(manager_servos[i].current_angle&0xFF);
       ram_data[BIOLOID_MM_SERVO0_CUR_POS_H+i*2]=(manager_servos[i].current_angle>>8);
+      // set the action current angles
+      action_current_angles[manager_servos[i].id]=manager_servos[i].current_angle<<16;
       manager_num_servos++;
     }
     else
@@ -276,13 +297,13 @@ inline uint8_t manager_get_num_servos(void)
 
 void manager_set_module(uint8_t servo_id,TModules module)
 {
-  if(servo_id>0 || servo_id<MANAGER_MAX_NUM_SERVOS)
+  if(servo_id>=0 && servo_id<MANAGER_MAX_NUM_SERVOS)
     manager_servos[servo_id].module=module;
 }
 
 inline TModules manager_get_module(uint8_t servo_id)
 {
-  if(servo_id>0 || servo_id<MANAGER_MAX_NUM_SERVOS)
+  if(servo_id>=0 && servo_id<MANAGER_MAX_NUM_SERVOS)
     return manager_servos[servo_id].module;
   else
     return MM_NONE;
@@ -290,19 +311,22 @@ inline TModules manager_get_module(uint8_t servo_id)
 
 inline void manager_enable_servo(uint8_t servo_id)
 {
-  if(servo_id>0 || servo_id<MANAGER_MAX_NUM_SERVOS)
+  if(servo_id>=0 && servo_id<MANAGER_MAX_NUM_SERVOS)
+  {
     manager_servos[servo_id].enabled=0x01;
+    gpio_toggle_led(WEST_LED);
+  }
 }
 
 inline void manager_disable_servo(uint8_t servo_id)
 {
-  if(servo_id>0 || servo_id<MANAGER_MAX_NUM_SERVOS)
+  if(servo_id>=0 && servo_id<MANAGER_MAX_NUM_SERVOS)
     manager_servos[servo_id].enabled=0x00;
 }
 
 inline uint8_t manager_is_servo_enabled(uint8_t servo_id)
 {
-  if(servo_id>0 || servo_id<MANAGER_MAX_NUM_SERVOS)
+  if(servo_id>=0 && servo_id<MANAGER_MAX_NUM_SERVOS)
     return manager_servos[servo_id].enabled;
   else
     return 0x00;

src/system_stm32f4xx.c

@@ -260,6 +260,8 @@ void SystemInit(void)
 #else
   SCB->VTOR = FLASH_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal FLASH */
 #endif
+
+  NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
 }
 
 /**

src/time.c

@@ -20,6 +20,7 @@ void time_init(void)
 {
   // set the time base to 1ms
   SysTick_Config(SystemCoreClock / 1000);
+  NVIC_SetPriority(SysTick_IRQn,0);
 }
 
 void delay_ms(__IO uint32_t time)