diff --git a/servo_firmware/mx28/Makefile b/servo_firmware/mx28/Makefile
index 46a4876e8c007984a86221061468a300df498631..027265d40e07e85980082634a6d22e3b90a4e762 100755
--- a/servo_firmware/mx28/Makefile
+++ b/servo_firmware/mx28/Makefile
@@ -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 \
diff --git a/servo_firmware/mx28/include/dynamixel_master.h b/servo_firmware/mx28/include/dynamixel_master.h
deleted file mode 100755
index 7689b09d026cc4faec7b9da589c298cda5ac48fd..0000000000000000000000000000000000000000
--- a/servo_firmware/mx28/include/dynamixel_master.h
+++ /dev/null
@@ -1,31 +0,0 @@
-#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
diff --git a/servo_firmware/mx28/include/dynamixel_slave.h b/servo_firmware/mx28/include/dynamixel_slave.h
index d03993d0ed9845ef130d8276e8dbc61041c452ed..b66de314ba1e5c5ba85d5bcebf579e4a631a30b6 100755
--- a/servo_firmware/mx28/include/dynamixel_slave.h
+++ b/servo_firmware/mx28/include/dynamixel_slave.h
@@ -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);
diff --git a/servo_firmware/mx28/src/dynamixel_master.c b/servo_firmware/mx28/src/dynamixel_master.c
deleted file mode 100755
index d5f5072d1b874a2a077491c650d44c9538d97cae..0000000000000000000000000000000000000000
--- a/servo_firmware/mx28/src/dynamixel_master.c
+++ /dev/null
@@ -1,451 +0,0 @@
-#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;
-}
-
diff --git a/servo_firmware/mx28/src/dynamixel_slave.c b/servo_firmware/mx28/src/dynamixel_slave.c
index d204d28ff322f93c8f4e928654e4ecb4e63a3431..ddeecb1012b3dfcfabfdffb000a7be738cbbb09f 100755
--- a/servo_firmware/mx28/src/dynamixel_slave.c
+++ b/servo_firmware/mx28/src/dynamixel_slave.c
@@ -1,11 +1,13 @@
#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();
}
diff --git a/servo_firmware/mx28/src/mx28_fw.c b/servo_firmware/mx28/src/mx28_fw.c
index 7bd0745b2d70b095027e3c66588a223d3ad1ccfd..7e2db16b42de6d685d95fc8fc666e907b6e08e44 100755
--- a/servo_firmware/mx28/src/mx28_fw.c
+++ b/servo_firmware/mx28/src/mx28_fw.c
@@ -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
}
}