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

Solved the communication problem in the dynamixel slave module. 

Removed some unnecessary files.
parent 03afc49e
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
servo_firmware/mx28/Makefile
+ 0
− 1
View file @ 807b37fc
......@@ -47,7 +47,6 @@ MAIN_OBJS = \
src/system_init.o \
src/dynamixel.o \
src/dynamixel_slave.o \
src/dynamixel_master.o \
src/mx28_fw.o \
src/startup/startup_stm32f10x_hd.o \
......
servo_firmware/mx28/include/dynamixel_master.h deleted 100755 → 0
+ 0
− 31
View file @ 03afc49e
#ifndef _DYNAMIXEL_MASTER_H
#define _DYNAMIXEL_MASTER_H
#include "dynamixel.h"
// interrupt handlers
void USART1_IRQHandler(void);
// dynamixel master functions
void dyn_master_init(void);
void dyn_master_flush(void);
void dyn_master_power_on(void);
void dyn_master_power_off(void);
uint8_t dyn_master_is_powered_on(void);
void dyn_master_set_timeout(uint16_t timeout_ms);
void dyn_master_scan(uint8_t *num,uint8_t *ids);
uint8_t dyn_master_ping(uint8_t id);
uint8_t dyn_master_read_byte(uint8_t id,uint8_t address,uint8_t *data);
uint8_t dyn_master_read_word(uint8_t id,uint8_t address,uint16_t *data);
uint8_t dyn_master_read_table(uint8_t id,uint8_t address,uint8_t length,uint8_t *data);
uint8_t dyn_master_write_byte(uint8_t id, uint8_t address, uint8_t data);
uint8_t dyn_master_write_word(uint8_t id, uint8_t address, uint16_t data);
uint8_t dyn_master_write_table(uint8_t id, uint8_t address, uint8_t length, uint8_t *data);
uint8_t dyn_master_reg_write(uint8_t id, uint8_t address, uint8_t length, uint8_t *data);
void dyn_master_action(void);
void dyn_master_sync_write(uint8_t num,uint8_t *ids,uint8_t address, uint8_t length, uint8_t *data);
uint8_t dyn_master_reset(uint8_t id);
// repeater functions
uint8_t dyn_master_resend_inst_packet(uint8_t *inst_packet, uint8_t *status_packet);
#endif
servo_firmware/mx28/include/dynamixel_slave.h
+ 1
− 2
View file @ 807b37fc
......@@ -4,11 +4,10 @@
#include "dynamixel.h"
// interrupt handlers
void USART3_IRQHandler(void);
void USART1_IRQHandler(void);
// public functions
void dyn_slave_init(void);
uint8_t dyn_slave_pc_present(void);
void dyn_slave_set_address(uint8_t id);
uint8_t dyn_slave_get_address(void);
uint8_t dyn_slave_is_packet_ready(void);
......
servo_firmware/mx28/src/dynamixel_master.c deleted 100755 → 0
+ 0
− 451
View file @ 03afc49e
#include "dynamixel_master.h"
#include "ram.h"
#include "time.h"
#define PIN_USART1_TX GPIO_Pin_6
#define PIN_USART1_RX GPIO_Pin_7
#define PIN_ENABLE_TXD GPIO_Pin_4
#define PIN_ENABLE_RXD GPIO_Pin_5
#define PIN_ENABLE_DXLPWR GPIO_Pin_8
#define PORT_USART1_TX GPIOB
#define PORT_USART1_RX GPIOB
#define PORT_ENABLE_TXD GPIOB
#define PORT_ENABLE_RXD GPIOB
#define PORT_ENABLE_DXLPWR GPIOB
#define MAX_BUFFER_LEN 1024
// private variables
uint16_t dyn_master_timeout;// answer reception timeout
// input buffer
uint8_t dyn_master_rx_buffer[MAX_BUFFER_LEN];
uint16_t dyn_master_rx_num_data;
// output buffer
uint8_t dyn_master_tx_buffer[MAX_BUFFER_LEN];
uint16_t dyn_master_tx_num_data;
uint16_t dyn_master_tx_ptr;
// instruction packet ready flag
volatile uint8_t dyn_master_packet_ready;
// sending status packet flag
volatile uint8_t dyn_master_sending_packet;
// private functions
void dyn_parse_status_packet(void)
{
if(dyn_master_rx_num_data>3)// the length byte has been received
{
if(dyn_master_rx_num_data==(dyn_get_length(dyn_master_rx_buffer)+4))
dyn_master_packet_ready=0x01;
}
}
void dyn_master_send(void)
{
// wait until any previous transmission ends
while(dyn_master_sending_packet);
// send the first byte
dyn_master_tx_num_data=dyn_get_length(dyn_master_tx_buffer)+4;
dyn_master_sending_packet=0x01;
USART_ITConfig(USART1, USART_IT_TC, ENABLE);
}
uint8_t dyn_master_receive(void)
{
int16_t timeout_left=dyn_master_timeout;
// wait for the status packet
while(!dyn_master_packet_ready)
{
delay_ms(10);
timeout_left-=10;
if(timeout_left<=0)
return DYN_TIMEOUT;
}
dyn_master_packet_ready=0x00;
dyn_master_rx_num_data=0x00;
// check the input packet checksum
if(dyn_check_checksum(dyn_master_rx_buffer)==0xFF)
return dyn_get_status_error(dyn_master_rx_buffer);
else
return DYN_CHECKSUM_ERROR;
}
void dyn_master_enable_tx(void)
{
GPIO_ResetBits(PORT_ENABLE_RXD, PIN_ENABLE_RXD);
GPIO_SetBits(PORT_ENABLE_TXD, PIN_ENABLE_TXD);
}
void dyn_master_enable_rx(void)
{
GPIO_ResetBits(PORT_ENABLE_TXD, PIN_ENABLE_TXD);
GPIO_SetBits(PORT_ENABLE_RXD, PIN_ENABLE_RXD);
}
uint8_t dyn_master_read(uint8_t id,uint8_t address,uint8_t length,uint8_t *data)
{
uint8_t error;
// generate the read packet for the desired device
dyn_init_instruction_packet(dyn_master_tx_buffer);
// set the ping instruction
dyn_set_instruction(dyn_master_tx_buffer,DYN_READ);
// set the device id
if((error=dyn_set_id(dyn_master_tx_buffer,id))==DYN_SUCCESS)
{
// set the length to read
dyn_set_read_length(dyn_master_tx_buffer,length);
// sert the start address to start reading
dyn_set_read_address(dyn_master_tx_buffer,address);
// set the checksum
dyn_set_checksum(dyn_master_tx_buffer);
// enable transmission
dyn_master_enable_tx();
// send the data
dyn_master_send();
// wait for the transmission to end
while(dyn_master_sending_packet);
dyn_master_enable_rx();
// wait for the replay within the given timeout
error=dyn_master_receive();
if(error==DYN_NO_ERROR)
{
// read the data from the status packet
dyn_get_status_data(dyn_master_rx_buffer,data);
}
return error;
}
else
return error;
}
uint8_t dyn_master_write(uint8_t id, uint8_t address, uint8_t length, uint8_t *data)
{
uint8_t error;
// generate the read packet for the desired device
dyn_init_instruction_packet(dyn_master_tx_buffer);
// set the ping instruction
dyn_set_instruction(dyn_master_tx_buffer,DYN_WRITE);
// set the device id
if((error=dyn_set_id(dyn_master_tx_buffer,id))==DYN_SUCCESS)
{
// sert the start address to start reading
dyn_set_read_address(dyn_master_tx_buffer,address);
// set the data to write and its length
dyn_set_write_data(dyn_master_tx_buffer,length,data);
// set the checksum
dyn_set_checksum(dyn_master_tx_buffer);
// enable transmission
dyn_master_enable_tx();
// send the data
dyn_master_send();
// wait for the transmission to end
while(dyn_master_sending_packet);
dyn_master_enable_rx();
// wait for the replay within the given timeout
error=dyn_master_receive();
return error;
}
else
return error;
}
// interrupt handlers
/**
* @brief This function handles USART1 global interrupt request.
* @param None
* @retval None
*/
void USART1_IRQHandler(void)
{
if(USART_GetITStatus(USART1, USART_IT_RXNE) != RESET)
{
/* Read one byte from the receive data register */
dyn_master_rx_buffer[dyn_master_rx_num_data++] = USART_ReceiveData(USART1);
dyn_parse_status_packet();
}
if(USART_GetITStatus(USART1, USART_IT_TC) != RESET)
{
if(dyn_master_tx_num_data==0x00)// there is no more data to be sent
{
dyn_master_tx_ptr=0x00;
dyn_master_sending_packet=0x00;
// disable interrupts
USART_ITConfig(USART1, USART_IT_TC, DISABLE);
}
else// there is still data to be sent
{
dyn_master_tx_num_data--;
USART_SendData(USART1,dyn_master_tx_buffer[dyn_master_tx_ptr++]);// send the next_byte
}
}
}
// dynamixel master functions
void dyn_master_init(void)
{
uint16_t i;
GPIO_InitTypeDef GPIO_InitStructure;
NVIC_InitTypeDef NVIC_InitStructure;
USART_InitTypeDef USART_InitStructure;
// configure the GPIO pins
/* Enable the USART1 Pins Software Remapping */
GPIO_PinRemapConfig(GPIO_Remap_USART1, ENABLE);
/* Configure USART1 Rx as input floating */
GPIO_InitStructure.GPIO_Pin = PIN_USART1_RX;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
GPIO_Init(PORT_USART1_RX, &GPIO_InitStructure);
/* Configure USART1 Tx as alternate function push-pull */
GPIO_InitStructure.GPIO_Pin = PIN_USART1_TX;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
GPIO_Init(PORT_USART1_TX, &GPIO_InitStructure);
/* configure the control pins */
GPIO_InitStructure.GPIO_Pin = PIN_ENABLE_TXD | PIN_ENABLE_RXD | PIN_ENABLE_DXLPWR ;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
GPIO_Init(GPIOB, &GPIO_InitStructure);
dyn_master_enable_rx();
dyn_master_timeout=0x00;
ram_write_byte(CM730_DXL_POWER,0x00);
// initialize the buffers
for(i=0;i<MAX_BUFFER_LEN;i++)
{
dyn_master_rx_buffer[i]=0x00;
dyn_master_tx_buffer[i]=0x00;
}
dyn_master_rx_num_data=0x00;
dyn_master_tx_num_data=0x00;
dyn_master_tx_ptr=0x00;
// initialize the flags
dyn_master_packet_ready=0x00;
dyn_master_sending_packet=0x00;
USART_DeInit(USART1);
USART_StructInit(&USART_InitStructure);
// configure the serial port
USART_InitStructure.USART_BaudRate = 1000000;
USART_InitStructure.USART_WordLength = USART_WordLength_8b;
USART_InitStructure.USART_StopBits = USART_StopBits_1;
USART_InitStructure.USART_Parity = USART_Parity_No;
USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
USART_Init(USART1, &USART_InitStructure);
// configure the interrupts
NVIC_InitStructure.NVIC_IRQChannel = USART1_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
USART_ITConfig(USART1, USART_IT_RXNE, ENABLE);
USART_ITConfig(USART1, USART_IT_ORE, DISABLE);
USART_ITConfig(USART1, USART_IT_TXE, DISABLE);
USART_ITConfig(USART1, USART_IT_TC, ENABLE);
USART_ITConfig(USART1, USART_IT_CTS, DISABLE);
USART_ITConfig(USART1, USART_IT_LBD, DISABLE);
USART_ITConfig(USART1, USART_IT_IDLE, DISABLE);
USART_ITConfig(USART1, USART_IT_PE, DISABLE);
USART_ITConfig(USART1, USART_IT_ERR, DISABLE);
/* Enable the USART1 */
USART_Cmd(USART1, ENABLE);
}
void dyn_master_flush(void)
{
// flush only the reception buffer to avoid interrupting a sync_write command
dyn_master_rx_num_data=0x00;
// dyn_master_tx_num_data=0x00;
// dyn_master_tx_ptr=0x00;
// initialize the flags
dyn_master_packet_ready=0x00;
// dyn_master_sending_packet=0x00;
}
void dyn_master_power_on(void)
{
GPIO_SetBits(PORT_ENABLE_DXLPWR, PIN_ENABLE_DXLPWR);
ram_write_byte(CM730_DXL_POWER,0x01);
}
void dyn_master_power_off(void)
{
GPIO_ResetBits(PORT_ENABLE_DXLPWR, PIN_ENABLE_DXLPWR);
ram_write_byte(CM730_DXL_POWER,0x00);
}
uint8_t dyn_master_is_powered_on(void)
{
uint8_t power;
ram_read_byte(CM730_DXL_POWER,&power);
return power;
}
void dyn_master_set_timeout(uint16_t timeout_ms)
{
// save the desired timeout value
dyn_master_timeout=timeout_ms;
}
void dyn_master_scan(uint8_t *num,uint8_t *ids)
{
uint8_t i;
*num=0;
for(i=0;i<30;i++)
{
if(dyn_master_ping(i)==DYN_NO_ERROR)// the device exists
{
ids[*num]=i;
(*num)++;
}
}
}
uint8_t dyn_master_ping(uint8_t id)
{
uint8_t error;
// generate the ping packet for the desired device
dyn_init_instruction_packet(dyn_master_tx_buffer);
// set the ping instruction
dyn_set_instruction(dyn_master_tx_buffer,DYN_PING);
// set the device id
if((error=dyn_set_id(dyn_master_tx_buffer,id))==DYN_SUCCESS)
{
// set the checksum
dyn_set_checksum(dyn_master_tx_buffer);
// enable transmission
dyn_master_enable_tx();
// send the data
dyn_master_send();
// wait for the transmission to end
while(dyn_master_sending_packet);
dyn_master_enable_rx();
// wait for the replay within the given timeout
error=dyn_master_receive();
return error;
}
else
return error;
}
uint8_t dyn_master_read_byte(uint8_t id,uint8_t address,uint8_t *data)
{
return dyn_master_read(id,address,1,data);
}
uint8_t dyn_master_read_word(uint8_t id,uint8_t address,uint16_t *data)
{
uint8_t value[2];
uint8_t error;
error=dyn_master_read(id,address,2,value);
if(error==DYN_NO_ERROR)
(*data)=value[0]+value[1]*256;
return error;
}
uint8_t dyn_master_read_table(uint8_t id,uint8_t address,uint8_t length,uint8_t *data)
{
return dyn_master_read(id,address,length,data);
}
uint8_t dyn_master_write_byte(uint8_t id, uint8_t address, uint8_t data)
{
return dyn_master_write(id,address,1,&data);
}
uint8_t dyn_master_write_word(uint8_t id, uint8_t address, uint16_t data)
{
uint8_t value[2];
value[0]=data%256;
value[1]=data/256;
return dyn_master_write(id,address,2,value);
}
uint8_t dyn_master_write_table(uint8_t id, uint8_t address, uint8_t length, uint8_t *data)
{
return dyn_master_write(id,address,length,data);
}
uint8_t dyn_master_reg_write(uint8_t id, uint8_t address, uint8_t length, uint8_t *data)
{
return DYN_SUCCESS;
}
void dyn_master_action(void)
{
}
void dyn_master_sync_write(uint8_t num,uint8_t *ids,uint8_t address, uint8_t length, uint8_t *data)
{
uint8_t i;
// generate the sync write packet
dyn_init_instruction_packet(dyn_master_tx_buffer);
// set the ping instruction
dyn_set_instruction(dyn_master_tx_buffer,DYN_SYNC_WRITE);
// set the start address
dyn_set_write_address(dyn_master_tx_buffer,address);
// set the data length
dyn_set_sync_write_length(dyn_master_tx_buffer,length);
// load the data for each device
for(i=0;i<num;i++)
dyn_set_sync_write_data(dyn_master_tx_buffer,ids[i],&data[i*length]);
// set the checksum
dyn_set_checksum(dyn_master_tx_buffer);
// enable transmission
dyn_master_enable_tx();
// send the data
dyn_master_send();
// wait for the transmission to end
// while(dyn_master_sending_packet);
}
uint8_t dyn_master_reset(uint8_t id)
{
return DYN_SUCCESS;
}
// repeater functions
uint8_t dyn_master_resend_inst_packet(uint8_t *inst_packet, uint8_t *status_packet)
{
uint8_t error;
// generate the read packet for the desired device
dyn_init_instruction_packet(dyn_master_tx_buffer);
// copy the contents of the instruction packet to the output buffer
dyn_copy_packet(inst_packet,dyn_master_tx_buffer);
// enable transmission
dyn_master_enable_tx();
// send the data
dyn_master_send();
// wait for the transmission to end
while(dyn_master_sending_packet);
dyn_master_enable_rx();
// wait for the replay within the given timeout
error=dyn_master_receive();
if(error!=DYN_TIMEOUT)
{
// copy the contents of the status of the packet to the output buffer
dyn_copy_packet(dyn_master_rx_buffer,status_packet);
}
return error;
}
servo_firmware/mx28/src/dynamixel_slave.c
+ 59
− 42
View file @ 807b37fc
#include "dynamixel_slave.h"
#define PIN_USART3_TX GPIO_Pin_10
#define PIN_USART3_RX GPIO_Pin_11
#define PIN_nSLEEP GPIO_Pin_13
#define PORT_USART3_TX GPIOB
#define PORT_USART3_RX GPIOB
#define PORT_nSLEEP GPIOA
#define PIN_USART1_TX GPIO_Pin_6
#define PIN_USART1_RX GPIO_Pin_7
#define PIN_TX_EN GPIO_Pin_14
#define PIN_RX_EN GPIO_Pin_15
#define PORT_USART1_TX GPIOB
#define PORT_USART1_RX GPIOB
#define PORT_TX_EN GPIOA
#define PORT_RX_EN GPIOA
#define MAX_BUFFER_LEN 1024
......@@ -35,36 +37,48 @@ void dyn_parse_inst_packet(void)
// interrupt handlers
/**
* @brief This function handles USART3 global interrupt request.
* @brief This function handles USART1 global interrupt request.
* @param None
* @retval None
*/
void USART3_IRQHandler(void)
void USART1_IRQHandler(void)
{
if(USART_GetITStatus(USART3, USART_IT_RXNE) != RESET)// a byte has been received
if(USART_GetITStatus(USART1, USART_IT_RXNE) != RESET)// a byte has been received
{
/* Read one byte from the receive data register */
dyn_slave_rx_buffer[dyn_slave_rx_num_data++] = USART_ReceiveData(USART3);
dyn_slave_rx_buffer[dyn_slave_rx_num_data++] = USART_ReceiveData(USART1);
dyn_parse_inst_packet();
}
if(USART_GetITStatus(USART3, USART_IT_TXE) != RESET)// a byte has been send
if(USART_GetITStatus(USART1, USART_IT_TC) != RESET)// a byte has been send
{
if(dyn_slave_tx_num_data==0x00)// there is no more data to be sent
{
dyn_slave_tx_ptr=0x00;
dyn_slave_sending_packet=0x00;
// disable interrupts
USART_ITConfig(USART3, USART_IT_TXE, DISABLE);
USART_ITConfig(USART1, USART_IT_TC, DISABLE);
}
else
{
USART_SendData(USART3,dyn_slave_tx_buffer[dyn_slave_tx_ptr++]);// send the next_byte
USART_SendData(USART1,dyn_slave_tx_buffer[dyn_slave_tx_ptr++]);// send the next_byte
dyn_slave_tx_num_data--;
}
}
}
void dyn_slave_enable_tx(void)
{
GPIO_ResetBits(PORT_RX_EN, PIN_RX_EN);
GPIO_SetBits(PORT_TX_EN, PIN_TX_EN);
}
void dyn_slave_enable_rx(void)
{
GPIO_ResetBits(PORT_TX_EN, PIN_TX_EN);
GPIO_SetBits(PORT_RX_EN, PIN_RX_EN);
}
// public functions
void dyn_slave_init(void)
{
......@@ -87,30 +101,32 @@ void dyn_slave_init(void)
dyn_slave_sending_packet=0x00;
// configure the interrupts
NVIC_InitStructure.NVIC_IRQChannel = USART3_IRQn;
NVIC_InitStructure.NVIC_IRQChannel = USART1_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
// configure the GPIO pins
/* Configure USART1 Rx as input floating */
GPIO_InitStructure.GPIO_Pin = PIN_USART3_RX;
GPIO_InitStructure.GPIO_Pin = PIN_USART1_RX;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
GPIO_Init(PORT_USART3_RX, &GPIO_InitStructure);
GPIO_Init(PORT_USART1_RX, &GPIO_InitStructure);
/* Configure USART1 Tx as alternate function push-pull */
GPIO_InitStructure.GPIO_Pin = PIN_USART3_TX;
GPIO_InitStructure.GPIO_Pin = PIN_USART1_TX;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
GPIO_Init(PORT_USART3_TX, &GPIO_InitStructure);
GPIO_Init(PORT_USART1_TX, &GPIO_InitStructure);
/* configure the nSLEEP input from the FTDI device*/
GPIO_InitStructure.GPIO_Pin = PIN_nSLEEP;
GPIO_InitStructure.GPIO_Pin = PIN_TX_EN | PIN_RX_EN;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPD;
GPIO_Init(PORT_nSLEEP, &GPIO_InitStructure);
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
GPIO_Init(GPIOB, &GPIO_InitStructure);
USART_DeInit(USART3);
dyn_slave_enable_rx();
USART_DeInit(USART1);
USART_StructInit(&USART_InitStructure);
// configure the serial port
USART_InitStructure.USART_BaudRate = 1000000;
......@@ -119,26 +135,21 @@ void dyn_slave_init(void)
USART_InitStructure.USART_Parity = USART_Parity_No;
USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
USART_Init(USART3, &USART_InitStructure);
USART_Init(USART1, &USART_InitStructure);
// enable the interrupts
USART_ITConfig(USART3, USART_IT_RXNE, ENABLE);
USART_ITConfig(USART3, USART_IT_ORE, DISABLE);
USART_ITConfig(USART3, USART_IT_TXE, ENABLE);
USART_ITConfig(USART3, USART_IT_TC, DISABLE);
USART_ITConfig(USART3, USART_IT_CTS, DISABLE);
USART_ITConfig(USART3, USART_IT_LBD, DISABLE);
USART_ITConfig(USART3, USART_IT_IDLE, DISABLE);
USART_ITConfig(USART3, USART_IT_PE, DISABLE);
USART_ITConfig(USART3, USART_IT_ERR, DISABLE);
/* Enable the USART3 */
USART_Cmd(USART3, ENABLE);
}
uint8_t dyn_slave_pc_present(void)
{
return GPIO_ReadOutputDataBit(PORT_nSLEEP,PIN_nSLEEP);
USART_ITConfig(USART1, USART_IT_RXNE, ENABLE);
USART_ITConfig(USART1, USART_IT_ORE, DISABLE);
USART_ITConfig(USART1, USART_IT_TXE, DISABLE);
USART_ITConfig(USART1, USART_IT_TC, ENABLE);
USART_ITConfig(USART1, USART_IT_CTS, DISABLE);
USART_ITConfig(USART1, USART_IT_LBD, DISABLE);
USART_ITConfig(USART1, USART_IT_IDLE, DISABLE);
USART_ITConfig(USART1, USART_IT_PE, DISABLE);
USART_ITConfig(USART1, USART_IT_ERR, DISABLE);
/* Enable the USART1 */
USART_Cmd(USART1, ENABLE);
}
void dyn_slave_set_address(uint8_t id)
......@@ -177,9 +188,12 @@ void dyn_slave_send_status_packet(uint8_t error,uint8_t length, uint8_t *data)
dyn_set_status_data(dyn_slave_tx_buffer,length,data);
dyn_set_checksum(dyn_slave_tx_buffer);
// start sending the package
dyn_slave_enable_tx();
dyn_slave_tx_num_data=dyn_get_length(dyn_slave_tx_buffer)+4;
dyn_slave_sending_packet=0x01;
USART_ITConfig(USART3, USART_IT_TXE, ENABLE);
USART_ITConfig(USART1, USART_IT_TC, ENABLE);
while(dyn_slave_sending_packet==0x01);
dyn_slave_enable_rx();
}
void dyn_slave_resend_status_packet(uint8_t *packet)
......@@ -190,7 +204,10 @@ void dyn_slave_resend_status_packet(uint8_t *packet)
dyn_init_status_packet(dyn_slave_tx_buffer);
dyn_copy_packet(packet,dyn_slave_tx_buffer);
// start sending the package
dyn_slave_enable_tx();
dyn_slave_tx_num_data=dyn_get_length(dyn_slave_tx_buffer)+4;
dyn_slave_sending_packet=0x01;
USART_ITConfig(USART3, USART_IT_TXE, ENABLE);
USART_ITConfig(USART1, USART_IT_TC, ENABLE);
while(dyn_slave_sending_packet==0x01);
dyn_slave_enable_rx();
}
servo_firmware/mx28/src/mx28_fw.c
+ 15
− 13
View file @ 807b37fc
......@@ -118,6 +118,7 @@ void process_packet(uint8_t *packet)
int main(void)
{
uint8_t inst_packet[1024];
int blink_count=0;
// initialize the general features of the system
clocks_init();
......@@ -144,12 +145,7 @@ int main(void)
while(1)
{
GPIO_ResetBits(GPIOA,GPIO_Pin_13); // LED ON
delay_ms(300);
GPIO_SetBits(GPIOA,GPIO_Pin_13); // LED OFF
delay_ms(300);
/*if(dyn_slave_is_packet_ready())// check if a new instruction packet has been received
if(dyn_slave_is_packet_ready())// check if a new instruction packet has been received
{
dyn_slave_get_inst_packet(inst_packet);// get the received packet
// check the packet checksum
......@@ -170,13 +166,19 @@ int main(void)
// send a checksum error answer
dyn_slave_send_status_packet(DYN_CHECKSUM_ERROR,0,0x00);
}
} */
/*else {
GPIO_ResetBits(GPIOA,GPIO_Pin_13);
delay_ms(300);
GPIO_SetBits(GPIOA,GPIO_Pin_13);
delay_ms(300);
}*/
}
else {
blink_count++;
if(blink_count==300)
{
if(GPIO_ReadOutputDataBit(GPIOA,GPIO_Pin_13))
GPIO_ResetBits(GPIOA,GPIO_Pin_13);
else
GPIO_SetBits(GPIOA,GPIO_Pin_13);
blink_count=0;
}
}
delay_ms(1);
// GPIO_SetBits(GPIOA,GPIO_Pin_13); // LED OFF
}
}
......
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