Skip to content
Snippets Groups Projects
Commit 4c501509 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Solvd a bug in the motion manager period generator.

parent e3fcd55d
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
include/bioloid_registers.h
+ 3
− 3
View file @ 4c501509
......@@ -10,8 +10,8 @@ extern "C" {
#define DEVICE_ID_OFFSET ((unsigned short int)0x0003)
#define BAUDRATE_OFFSET ((unsigned short int)0x0004)
#define RETURN_DELAY_OFFSET ((unsigned short int)0x0005)
#define MM_PERIOD_OFFSET ((unsigned short int)0x0006)
#define RETURN_LEVEL_OFFSET ((unsigned short int)0x0010)
#define MM_PERIOD_OFFSET ((unsigned short int)0x0010)
#define LAST_EEPROM_OFFSET ((unsigned short int)0x0020)
......@@ -196,8 +196,8 @@ typedef enum {
#define ZIGBEE_BASE_ADDRESS 0x42
#define ZIGBEE_MEM_LENGTH 6
#define MANAGER_BASE_ADDRESS 0x42
#define MANAGER_MEM_LENGTH 6
#define MANAGER_BASE_ADDRESS 0x48
#define MANAGER_MEM_LENGTH 50
#ifdef __cplusplus
}
......
include/comm.h deleted 100644 → 0
+ 0
− 18
View file @ e3fcd55d
#ifndef _COMM_H
#define _COMM_H
#ifdef __cplusplus
extern "C" {
#endif
#include "stm32f4xx.h"
// public functions
void comm_init(void);
void comm_start(void);
#ifdef __cplusplus
}
#endif
#endif
src/bioloid_dyn_slave.c
+ 2
− 2
View file @ 4c501509
......@@ -147,12 +147,12 @@ void bioloid_dyn_slave_init(void)
DYN_SLAVE_TIMER_ENABLE_CLK;
bioloid_dyn_slave_tim_handle.Instance=DYN_SLAVE_TIMER;
bioloid_dyn_slave_tim_handle.Init.Period = 1000;
bioloid_dyn_slave_tim_handle.Init.Prescaler = 42;
bioloid_dyn_slave_tim_handle.Init.Prescaler = 84;
bioloid_dyn_slave_tim_handle.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
bioloid_dyn_slave_tim_handle.Init.CounterMode = TIM_COUNTERMODE_UP;
HAL_TIM_Base_Init(&bioloid_dyn_slave_tim_handle);
// initialize the timer interrupts
HAL_NVIC_SetPriority(DYN_SLAVE_TIMER_IRQn, 3, 2);
HAL_NVIC_SetPriority(DYN_SLAVE_TIMER_IRQn, 2, 0);
HAL_NVIC_EnableIRQ(DYN_SLAVE_TIMER_IRQn);
}
......
src/comm.c deleted 100644 → 0
+ 0
− 263
View file @ e3fcd55d
#include "comm.h"
#include "gpio.h"
#include "ram.h"
#include "motion_manager.h"
#include "action.h"
#include "dynamixel_slave_uart_dma.h"
#include "dynamixel_master_uart_dma.h"
#include "adc_dma.h"
#define COMM_TIMER TIM7
#define COMM_TIMER_IRQn TIM7_IRQn
#define COMM_TIMER_IRQHandler TIM7_IRQHandler
#define COMM_TIMER_CLK RCC_APB1Periph_TIM7
// private variables
uint8_t inst_packet[MAX_DATA_LENGTH];
uint8_t status_packet[MAX_DATA_LENGTH];
uint8_t data[MAX_DATA_LENGTH];
// registered write operation variables
uint8_t reg_data[MAX_DATA_LENGTH];
uint8_t reg_address,reg_length,reg_op;
// private functions
uint8_t read_operation(uint8_t address, uint8_t length, uint8_t *data)
{
uint8_t error;
// pre-read operation
// read operation
error=ram_read_table(address,length,data);
// post-read operation
if(ram_in_range(BIOLOID_PUSHBUTTON,BIOLOID_PUSHBUTTON,address,length))
ram_data[BIOLOID_PUSHBUTTON]=0x00;
return error;
}
uint8_t write_operation(uint8_t address,uint8_t length, uint8_t *data)
{
uint8_t error=RAM_SUCCESS,byte_value,i,j,module;
uint16_t word_value;
// pre-write operation
if(ram_in_range(BIOLOID_DXL_POWER,BIOLOID_DXL_POWER,address,length))
{
if(data[BIOLOID_DXL_POWER-address]&0x01) dyn_master_enable_power();
else dyn_master_disable_power();
}
if(ram_in_range(BIOLOID_LED,BIOLOID_LED,address,length))
{
byte_value=data[BIOLOID_LED-address];
if(byte_value&0x01)
gpio_set_led(NORTH_LED);
else
gpio_clear_led(NORTH_LED);
if(byte_value&0x02)
gpio_set_led(SOUTH_LED);
else
gpio_clear_led(SOUTH_LED);
if(byte_value&0x04)
gpio_set_led(EAST_LED);
else
gpio_clear_led(EAST_LED);
if(byte_value&0x08)
gpio_set_led(WEST_LED);
else
gpio_clear_led(WEST_LED);
}
if(ram_in_range(BIOLOID_MM_PERIOD_L,BIOLOID_MM_PERIOD_H,address,length))
{
word_value=data[BIOLOID_MM_PERIOD_L-address]+(data[BIOLOID_MM_PERIOD_H-address]<<8);
manager_set_period(word_value);
}
for(i=BIOLOID_MM_MODULE_EN0,j=0;i<=BIOLOID_MM_MODULE_EN15;i++,j+=2)
{
if(ram_in_range(i,i,address,length))
{
byte_value=data[i-address];
if(byte_value&0x80) manager_enable_servo(j);
else manager_disable_servo(j);
module=(byte_value&0x70)>>4;
manager_set_module(j,module);
if(byte_value&0x08) manager_enable_servo(j+1);
else manager_disable_servo(j+1);
module=byte_value&0x07;
manager_set_module(j+1,module);
}
}
if(ram_in_range(BIOLOID_MM_CONTROL,BIOLOID_MM_CONTROL,address,length))
{
if(data[BIOLOID_MM_CONTROL-address]&0x01)
manager_enable();
else
manager_disable();
}
if(ram_in_range(BIOLOID_ACTION_CONTROL,BIOLOID_ACTION_CONTROL,address,length))
{
if(data[BIOLOID_ACTION_CONTROL-address]&0x01)
action_start_page();
if(data[BIOLOID_ACTION_CONTROL-address]&0x02)
action_stop_page();
}
if(ram_in_range(BIOLOID_ACTION_PAGE,BIOLOID_ACTION_PAGE,address,length))// load the page identifier
action_load_page(data[BIOLOID_ACTION_PAGE-address]);
// write eeprom
for(i=address,j=0;i<LAST_EEPROM_OFFSET && i<(address+length);i++,j++)
EE_WriteVariable(i,data[j]);
// write operation
// error=ram_write_table(address,length,data);
// post-write operation
return error;
}
// interrupt handlers
void COMM_TIMER_IRQHandler(void)
{
uint8_t error,length,prev_id,id;
TDynVersion version;
TIM_ClearITPendingBit(COMM_TIMER,TIM_IT_Update);
if(dyn_slave_is_packet_ready())// check if a new instruction packet has been received
{
version=dyn_slave_get_inst_packet(inst_packet);// get the received packet
// check address
if(version==DYN_VER1)
{
id=dyn_get_id(inst_packet);
if(id==dyn_slave_get_address() || id==DYN_BROADCAST_ID)// the packet is addressed to this device or it is a broadcast
{
// check the packet checksum
if(dyn_check_checksum(inst_packet)==0xFF)// the incomming packet is okay
{
// process the packet
switch(dyn_get_instruction(inst_packet))
{
case DYN_PING: if(id!=DYN_BROADCAST_ID)
dyn_slave_send_status_packet(DYN_NO_ERROR,0,data);
break;
case DYN_READ: error=read_operation(dyn_get_read_address(inst_packet),dyn_get_read_length(inst_packet),data);
if(dyn_slave_get_return_level()!=0 && id!=DYN_BROADCAST_ID)
{
if(error==RAM_SUCCESS)
dyn_slave_send_status_packet(DYN_NO_ERROR,dyn_get_read_length(inst_packet),data);
else
dyn_slave_send_status_packet(DYN_INST_ERROR,0,data);
}
break;
case DYN_WRITE: length=dyn_get_write_data(inst_packet,data);
error=write_operation(dyn_get_write_address(inst_packet),length,data);
if(dyn_slave_get_return_level()==2 && id!=DYN_BROADCAST_ID)
{
if(error==RAM_SUCCESS)
dyn_slave_send_status_packet(DYN_NO_ERROR,0,data);
else
dyn_slave_send_status_packet(DYN_INST_ERROR,0,data);
}
break;
case DYN_REG_WRITE: reg_length=dyn_get_reg_write_data(inst_packet,reg_data);
reg_address=dyn_get_reg_write_address(inst_packet);
reg_op=0x01;// signal there is a registered operation pending
if(dyn_slave_get_return_level()==2 && id!=DYN_BROADCAST_ID)
dyn_slave_send_status_packet(DYN_NO_ERROR,0,data);
break;
case DYN_ACTION: if(reg_op)
{
error=write_operation(reg_address,reg_length,reg_data);
reg_op=0x00;
}
break;
case DYN_RESET:
break;
case DYN_SYNC_READ: dyn_slave_send_status_packet(DYN_INST_ERROR,0,data);
break;
case DYN_SYNC_WRITE: if(dyn_sync_write_id_present(inst_packet,dyn_slave_get_address()))// the device is addressed
{
length=dyn_get_sync_write_data(inst_packet,dyn_slave_get_address(),data);
error=write_operation(dyn_get_sync_write_address(inst_packet),length,data);
}
break;
case DYN_BULK_READ: if(dyn_bulk_read_id_present(inst_packet,dyn_slave_get_address(),&prev_id))
{
length=dyn_get_bulk_read_length(inst_packet,dyn_slave_get_address());
error=read_operation(dyn_get_bulk_read_address(inst_packet,dyn_slave_get_address()),length,data);
if(prev_id==0xFF)// first device in the bulk read sequence
{
if(error==RAM_SUCCESS)
dyn_slave_send_status_packet(DYN_NO_ERROR,length,data);
else
dyn_slave_send_status_packet(DYN_INST_ERROR,0,data);
}
else// wait for the previous device in the sequence to send its data
{
do{
while(!dyn_slave_is_packet_ready());// wait until a new packet is received
dyn_slave_get_inst_packet(inst_packet);
}while(dyn_get_id(inst_packet)!=prev_id);
if(error==RAM_SUCCESS)
dyn_slave_send_status_packet(DYN_NO_ERROR,length,data);
else
dyn_slave_send_status_packet(DYN_INST_ERROR,0,data);
}
}
break;
case DYN_BULK_WRITE: dyn_slave_send_status_packet(DYN_INST_ERROR,0,data);
break;
default:
break;
}
}
else
{
// send a checksum error answer
if(dyn_get_id(inst_packet)!=DYN_BROADCAST_ID)
dyn_slave_send_status_packet(DYN_CHECKSUM_ERROR,0,0x00);
}
}
if(id!=dyn_slave_get_address() || id==DYN_BROADCAST_ID)// the packet is addressed to another device
{
// send the incomming packet to the dynamixel bus
if(dyn_master_resend_inst_packet(inst_packet,status_packet)!=DYN_TIMEOUT)
{
// send the answer back to the computer
dyn_slave_resend_status_packet(status_packet);
}
}
}
else// version 2
{
}
}
}
// public functions
void comm_init(void)
{
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
NVIC_InitTypeDef NVIC_InitStructure;
RCC_APB1PeriphClockCmd(COMM_TIMER_CLK,ENABLE);
TIM_TimeBaseStructure.TIM_Period = 1000;
TIM_TimeBaseStructure.TIM_Prescaler = 42;
TIM_TimeBaseStructure.TIM_ClockDivision = 0;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInit(COMM_TIMER,&TIM_TimeBaseStructure);
NVIC_InitStructure.NVIC_IRQChannel = COMM_TIMER_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 2;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
/* initialize variables */
reg_op=0x00;
}
void comm_start(void)
{
TIM_Cmd(COMM_TIMER, ENABLE);
TIM_ITConfig(COMM_TIMER, TIM_IT_Update, ENABLE);
}
src/motion_manager.c
+ 6
− 3
View file @ 4c501509
......@@ -2,6 +2,7 @@
#include "motion_manager.h"
#include "dyn_servos.h"
#include "ram.h"
#include "gpio.h"
#define MANAGER_TIMER TIM3
#define ENABLE_MANAGER_TIMER_CLK __HAL_RCC_TIM3_CLK_ENABLE()
......@@ -126,6 +127,7 @@ void MANAGER_TIMER_IRQHandler(void)
// manager_send_motion_command();
// get the disabled servos position
// manager_get_current_position();
gpio_toggle_led(RXD_LED);
}
}
}
......@@ -261,10 +263,10 @@ void manager_init(uint16_t period_us)
ENABLE_MANAGER_TIMER_CLK;
MANAGER_TIM_Handle.Instance=MANAGER_TIMER;
MANAGER_TIM_Handle.Init.Period = 0xFFFF;
MANAGER_TIM_Handle.Init.Prescaler = 42;
MANAGER_TIM_Handle.Init.Prescaler = 84;
MANAGER_TIM_Handle.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
MANAGER_TIM_Handle.Init.CounterMode = TIM_COUNTERMODE_UP;
HAL_NVIC_SetPriority(MANAGER_TIMER_IRQn, 2, 0);
HAL_NVIC_SetPriority(MANAGER_TIMER_IRQn, 2, 1);
HAL_NVIC_EnableIRQ(MANAGER_TIMER_IRQn);
/* use the internal clock */
sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
......@@ -291,7 +293,8 @@ uint16_t manager_get_period(void)
void manager_set_period(uint16_t period_us)
{
manager_motion_period=(period_us*1000000)>>16;
ram_data[BIOLOID_MM_PERIOD_L]=period_us;
ram_data[BIOLOID_MM_PERIOD_L]=period_us&0x00FF;
ram_data[BIOLOID_MM_PERIOD_H]=(period_us&0xFF00)>>8;
}
inline void manager_enable(void)
......
src/ram.c
+ 4
− 0
View file @ 4c501509
......@@ -22,6 +22,10 @@ void ram_init(void)
ram_data[BAUDRATE_OFFSET]=(uint8_t)eeprom_data;
if(EE_ReadVariable(RETURN_DELAY_OFFSET,&eeprom_data)==0)
ram_data[RETURN_DELAY_OFFSET]=(uint8_t)eeprom_data;
if(EE_ReadVariable(MM_PERIOD_OFFSET,&eeprom_data)==0)
ram_data[MM_PERIOD_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
if(EE_ReadVariable(MM_PERIOD_OFFSET+1,&eeprom_data)==0)
ram_data[MM_PERIOD_OFFSET+1]=(uint8_t)(eeprom_data&0x00FF);
if(EE_ReadVariable(RETURN_LEVEL_OFFSET,&eeprom_data)==0)
ram_data[RETURN_LEVEL_OFFSET]=(uint8_t)eeprom_data;
}
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment