diff --git a/servo_firmware/rx28/Makefile b/servo_firmware/rx28/Makefile
index e418472cb128ad2bef06dbc9978ff435f665a0c0..77f747cc66cb0d2ced0b130963de565bf9bfe270 100755
--- a/servo_firmware/rx28/Makefile
+++ b/servo_firmware/rx28/Makefile
@@ -2,8 +2,8 @@ PROJECT=rx28_fw
########################################################
# afegir tots els fitxers que s'han de compilar aquÃ
########################################################
-#SOURCES=src/main.c src/TXRX_Dynamixel.c src/CTRL_Dynamixel.c src/MEM_Dynamixel.c src/CFG_HW_Dynamixel.c
-SOURCES=src/main.c src/CTRL_Dynamixel.c src/MEM_Dynamixel.c src/CFG_HW_Dynamixel.c
+SOURCES=src/main.c src/TXRX_Dynamixel.c src/CTRL_Dynamixel.c src/MEM_Dynamixel.c src/CFG_HW_Dynamixel.c
+#SOURCES=src/main.c src/CTRL_Dynamixel.c src/MEM_Dynamixel.c src/CFG_HW_Dynamixel.c
#SOURCES=src/main.c src/CFG_HW_Dynamixel.c
OBJS=$(SOURCES:.c=.o)
SRC_DIR=./src/
diff --git a/servo_firmware/rx28/src/main.c b/servo_firmware/rx28/src/main.c
index 5a55c45d23eddd5fb0596c70981e1b1c2106a57f..7b3f04867aa679a19525e80468cb2f7c6e57670d 100755
--- a/servo_firmware/rx28/src/main.c
+++ b/servo_firmware/rx28/src/main.c
@@ -3,7 +3,7 @@
#include <avr/interrupt.h>
#include <avr/eeprom.h>
#include "CFG_HW_Dynamixel.h"
-//#include "TXRX_Dynamixel.h"
+#include "TXRX_Dynamixel.h"
#include "CTRL_Dynamixel.h"
#include "MEM_Dynamixel.h"
@@ -39,11 +39,10 @@ ISR( TIMER0_OVF_vect) {
int16_t main(void) {
unsigned char data[128], id, length, instruction, answer[2], status,
en_vector, i, j;
- uint16_t ii, jj;
// list of read only registers - to exclude from write
- /*unsigned char read_only_vector[30] = { 0, 1, 2, 36, 37, 38, 39, 40, 41, 42,
+ unsigned char read_only_vector[30] = { 0, 1, 2, 36, 37, 38, 39, 40, 41, 42,
43, 44, 45, 46, 67, 68, 71, 72, 73, 74, 75, 76, 77, 78, 79, 80, 83,
- 84, 85, 86 };*/
+ 84, 85, 86 };
// ini eeprom if first time after run reflash system
if (eeprom_read_byte((uint8_t*) Gate_Restore_Eeprom) != 0xCC) {
@@ -53,209 +52,197 @@ int16_t main(void) {
//------EEPROM initial values-------------
Restore_EepromVAR();
// get value of Motor ID
- //rs485_address = eepromVAR[ID];
+ rs485_address = eepromVAR[ID];
// configure AVR chip i/o
Config_Hardware();
// asign actual position to goal position and asume zero motor turns
Ini_Position();
// initialize the RS485 interface
- /*init_RS485();*/
+ init_RS485();
// end of inicialization
LedOFF;
- ii = 0;
- jj=1;
while (1) {
-
- Write_word_Dynamixel_RAM(Goal_Position_L,ii);
- _delay_ms(500); // funciona
- //ii=500;
- ii = ii + 100*jj;
- LedTOGGLE;
- if (ii == 900) {jj=-1;}
- if (ii == 100) {jj=1;}
-
-
- /*status = RxRS485Packet(&id, &instruction, &length, data);
- if (status == CHECK_ERROR) {
- TxRS485Packet(rs485_address, CHECKSUM_ERROR, 0, NULL);
- } else if (status == CORRECT) {
- //LedON;
- if (id == rs485_address) {
- switch (instruction) {
- //***********************************************************
- // P I N G
- case INST_PING:
- TxRS485Packet(rs485_address, NO_ERROR, 0, NULL);
- break;
- //*********************************************************
- // R E A D and send info to pc
- case INST_READ:
- if (data[0] <= 18) { // if 0 <= IDinstruction <= 18 caso eeprom
- if (data[0] >= 0) {
- switch (data[1]) // tamaño del paquete que pide
- {
- case 1: // case BYTE
- TxRx_Read_byte_Dynamixel_EEPROM(data[0],
- answer);
- break; //................................................
- case 2: // case WORD
- TxRx_Read_word_Dynamixel_EEPROM(data[0],
- answer);
- break; //................................................
- default:
- TxRS485Packet(rs485_address, INSTRUCTION_ERROR,
- 0, NULL);
- break; //................................................
- }
- } else {
- TxRS485Packet(rs485_address, INSTRUCTION_ERROR, 0,
- NULL);
- } // else id error
- } else if (data[0] <= 86) { // else if 24 <= ID <= 86 ram
- if (data[0] >= 24) {
- switch (data[1]) { // tamaño del paquete que pide
- case 1: // case BYTE
- TxRx_Read_byte_Dynamixel_RAM(data[0], answer);
- break; //................................................
- case 2: // case WORD
-
- //LedTOGGLE;
- TxRx_Read_word_Dynamixel_RAM(data[0], answer);
- break; //................................................
- default:
- TxRS485Packet(rs485_address, INSTRUCTION_ERROR,
- 0, NULL);
- break; //................................................
- }
- } else {
- TxRS485Packet(rs485_address, INSTRUCTION_ERROR, 0,
- NULL);
- } // else id error
- } else if (data[0] == 87) { // else if ID = 87 ram
- switch (data[1]) { // tamaño del paquete que pide
- case 1: // case BYTE
- TxRx_Read_byte_Dynamixel_RAM(data[0], answer);
- break; //................................................
- default:
- TxRS485Packet(rs485_address, INSTRUCTION_ERROR, 0,
- NULL);
- break; //................................................
- }
- } else {
- TxRS485Packet(rs485_address, INSTRUCTION_ERROR, 0,
- NULL);
- } // else id error
- break;
-
- //**********************************************************
- // W R I T E info send by pc
- case INST_WRITE:
- en_vector = FALSE; // list of read only registers - to exclude from write
- //unsigned char read_only_vector[30]={0,1,2,36,37,38,39,40,41,42,43,44,45,46,67,68,71,72,73,74,75,76,77,78,79,80,83,84,85,86};
- for (i = 0; i < 30; i++) {
- if (data[0] == read_only_vector[i]) {
- en_vector = TRUE;
- break;
- }
- }
-
- if (en_vector == FALSE) {
- if (data[0] <= 18) { // if 0 <= IDinstruction <= 18 caso eeprom
- if (data[0] >= 0) {
- if (Read_byte_Dynamixel_RAM(Lock) == 1) { // is eeprom write lock open
- switch (length) // tamaño del paquete que pide
- {
- case 2: // case BYTE
- TxRx_Write_byte_Dynamixel_EEPROM(
- data[0], data, length);
- break; //................................................
- case 3: // case WORD
- TxRx_Write_word_Dynamixel_EEPROM(
- data[0], data, length);
- break; //................................................
- default:
- TxRS485Packet(rs485_address,
- INSTRUCTION_ERROR, 0, NULL);
- break; //................................................
- }
- switch (data[0]) // aditional instructions for particular registers in eeprom
- {
- case ID: // case EEMPROM and BYTE
- rs485_address = eepromVAR[ID];
- break; //..................................................
- case Baud_Rate: // case EEMPROM and BYTE
- Baud_Rate_Value();
- break; //..................................................
- default:
- ;
- break; //................................................
- }
- Write_byte_Dynamixel_RAM(Lock, 0); // close eeprom write lock
- } else {
- TxRS485Packet(rs485_address,
- INSTRUCTION_ERROR, 0, NULL);
- } // eeprom write lock closed
- } else {
- TxRS485Packet(rs485_address, INSTRUCTION_ERROR,
- 0, NULL);
- } // else id error
- } else if (data[0] <= 86) { // else if 24 <= ID <= 86 ram
- if (data[0] >= 24) {
- switch (length) { // tamaño del paquete que pide
- case 2: // case BYTE
- TxRx_Write_byte_Dynamixel_RAM(data[0], data,
- length);
- break; //................................................
- case 3: // case WORD
- TxRx_Write_word_Dynamixel_RAM(data[0], data,
- length);
- break; //................................................
- default:
- TxRS485Packet(rs485_address,
- INSTRUCTION_ERROR, 0, NULL);
- break; //................................................
- }
- switch (data[0]) { // aditional instructions for particular registers in ram
- case LED: // case RAM and BYTE
- if (Read_byte_Dynamixel_RAM(LED) == 1) {
- LedON;
- } else {
- LedOFF;
- }
- break; //................................................
- default:
- ;
- break; //................................................
- }
- } else {
- TxRS485Packet(rs485_address, INSTRUCTION_ERROR,
- 0, NULL);
- } // else id error
- } else if (data[0] == 87) { // else if ID = 87 ram // case BYTE
- TxRx_Write_byte_Dynamixel_RAM(data[0], data,
- length);
- } else {
- TxRS485Packet(rs485_address, INSTRUCTION_ERROR, 0,
- NULL);
- } // else id error
- } else {
- TxRS485Packet(rs485_address, INSTRUCTION_ERROR, 0,
- NULL);
- } // else in only read register list
- break;
-
- //**********************************************************
- //********* D E F A U L T *****************************
- default:
- TxRS485Packet(rs485_address, INSTRUCTION_ERROR, 0, NULL);
- break;
- //***************************************************
- }
- }
- }*/
+ status = RxRS485Packet(&id, &instruction, &length, data);
+ if (status == CHECK_ERROR) {
+ TxRS485Packet(rs485_address, CHECKSUM_ERROR, 0, NULL);
+ } else if (status == CORRECT) {
+ //LedON;
+ if (id == rs485_address) {
+ switch (instruction) {
+ //***********************************************************
+ // P I N G
+ case INST_PING:
+ TxRS485Packet(rs485_address, NO_ERROR, 0, NULL);
+ break;
+ //*********************************************************
+ // R E A D and send info to pc
+ case INST_READ:
+ if (data[0] <= 18) { // if 0 <= IDinstruction <= 18 caso eeprom
+ if (data[0] >= 0) {
+ switch (data[1]) // tamaño del paquete que pide
+ {
+ case 1: // case BYTE
+ TxRx_Read_byte_Dynamixel_EEPROM(data[0],
+ answer);
+ break; //................................................
+ case 2: // case WORD
+ TxRx_Read_word_Dynamixel_EEPROM(data[0],
+ answer);
+ break; //................................................
+ default:
+ TxRS485Packet(rs485_address, INSTRUCTION_ERROR,
+ 0, NULL);
+ break; //................................................
+ }
+ } else {
+ TxRS485Packet(rs485_address, INSTRUCTION_ERROR, 0,
+ NULL);
+ } // else id error
+ } else if (data[0] <= 86) { // else if 24 <= ID <= 86 ram
+ if (data[0] >= 24) {
+ switch (data[1]) { // tamaño del paquete que pide
+ case 1: // case BYTE
+ TxRx_Read_byte_Dynamixel_RAM(data[0], answer);
+ break; //................................................
+ case 2: // case WORD
+
+ //LedTOGGLE;
+ TxRx_Read_word_Dynamixel_RAM(data[0], answer);
+ break; //................................................
+ default:
+ TxRS485Packet(rs485_address, INSTRUCTION_ERROR,
+ 0, NULL);
+ break; //................................................
+ }
+ } else {
+ TxRS485Packet(rs485_address, INSTRUCTION_ERROR, 0,
+ NULL);
+ } // else id error
+ } else if (data[0] == 87) { // else if ID = 87 ram
+ switch (data[1]) { // tamaño del paquete que pide
+ case 1: // case BYTE
+ TxRx_Read_byte_Dynamixel_RAM(data[0], answer);
+ break; //................................................
+ default:
+ TxRS485Packet(rs485_address, INSTRUCTION_ERROR, 0,
+ NULL);
+ break; //................................................
+ }
+ } else {
+ TxRS485Packet(rs485_address, INSTRUCTION_ERROR, 0,
+ NULL);
+ } // else id error
+ break;
+
+ //**********************************************************
+ // W R I T E info send by pc
+ case INST_WRITE:
+ en_vector = FALSE; // list of read only registers - to exclude from write
+ //unsigned char read_only_vector[30]={0,1,2,36,37,38,39,40,41,42,43,44,45,46,67,68,71,72,73,74,75,76,77,78,79,80,83,84,85,86};
+ for (i = 0; i < 30; i++) {
+ if (data[0] == read_only_vector[i]) {
+ en_vector = TRUE;
+ break;
+ }
+ }
+
+ if (en_vector == FALSE) {
+ if (data[0] <= 18) { // if 0 <= IDinstruction <= 18 caso eeprom
+ if (data[0] >= 0) {
+ if (Read_byte_Dynamixel_RAM(Lock) == 1) { // is eeprom write lock open
+ switch (length) // tamaño del paquete que pide
+ {
+ case 2: // case BYTE
+ TxRx_Write_byte_Dynamixel_EEPROM(
+ data[0], data, length);
+ break; //................................................
+ case 3: // case WORD
+ TxRx_Write_word_Dynamixel_EEPROM(
+ data[0], data, length);
+ break; //................................................
+ default:
+ TxRS485Packet(rs485_address,
+ INSTRUCTION_ERROR, 0, NULL);
+ break; //................................................
+ }
+ switch (data[0]) // aditional instructions for particular registers in eeprom
+ {
+ case ID: // case EEMPROM and BYTE
+ rs485_address = eepromVAR[ID];
+ break; //..................................................
+ case Baud_Rate: // case EEMPROM and BYTE
+ Baud_Rate_Value();
+ break; //..................................................
+ default:
+ ;
+ break; //................................................
+ }
+ Write_byte_Dynamixel_RAM(Lock, 0); // close eeprom write lock
+ } else {
+ TxRS485Packet(rs485_address,
+ INSTRUCTION_ERROR, 0, NULL);
+ } // eeprom write lock closed
+ } else {
+ TxRS485Packet(rs485_address, INSTRUCTION_ERROR,
+ 0, NULL);
+ } // else id error
+ } else if (data[0] <= 86) { // else if 24 <= ID <= 86 ram
+ if (data[0] >= 24) {
+ switch (length) { // tamaño del paquete que pide
+ case 2: // case BYTE
+ TxRx_Write_byte_Dynamixel_RAM(data[0], data,
+ length);
+ break; //................................................
+ case 3: // case WORD
+ TxRx_Write_word_Dynamixel_RAM(data[0], data,
+ length);
+ break; //................................................
+ default:
+ TxRS485Packet(rs485_address,
+ INSTRUCTION_ERROR, 0, NULL);
+ break; //................................................
+ }
+ switch (data[0]) { // aditional instructions for particular registers in ram
+ case LED: // case RAM and BYTE
+ if (Read_byte_Dynamixel_RAM(LED) == 1) {
+ LedON;
+ } else {
+ LedOFF;
+ }
+ break; //................................................
+ default:
+ ;
+ break; //................................................
+ }
+ } else {
+ TxRS485Packet(rs485_address, INSTRUCTION_ERROR,
+ 0, NULL);
+ } // else id error
+ } else if (data[0] == 87) { // else if ID = 87 ram // case BYTE
+ TxRx_Write_byte_Dynamixel_RAM(data[0], data,
+ length);
+ } else {
+ TxRS485Packet(rs485_address, INSTRUCTION_ERROR, 0,
+ NULL);
+ } // else id error
+ } else {
+ TxRS485Packet(rs485_address, INSTRUCTION_ERROR, 0,
+ NULL);
+ } // else in only read register list
+ break;
+
+ //**********************************************************
+ //********* D E F A U L T *****************************
+ default:
+ TxRS485Packet(rs485_address, INSTRUCTION_ERROR, 0, NULL);
+ break;
+ //***************************************************
+ }
+ }
+ }
}
}