Skip to content
Snippets Groups Projects
Commit 0b698b71 authored by Patrick John Grosch Obregon's avatar Patrick John Grosch Obregon
Browse files

rx28 firmware TX and RX added work - multiple turns - PD controler

parent 19ab9850
No related branches found
No related tags found
No related merge requests found
...@@ -2,8 +2,8 @@ PROJECT=rx28_fw ...@@ -2,8 +2,8 @@ PROJECT=rx28_fw
######################################################## ########################################################
# afegir tots els fitxers que s'han de compilar aquí # 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/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/CTRL_Dynamixel.c src/MEM_Dynamixel.c src/CFG_HW_Dynamixel.c
#SOURCES=src/main.c src/CFG_HW_Dynamixel.c #SOURCES=src/main.c src/CFG_HW_Dynamixel.c
OBJS=$(SOURCES:.c=.o) OBJS=$(SOURCES:.c=.o)
SRC_DIR=./src/ SRC_DIR=./src/
......
...@@ -3,7 +3,7 @@ ...@@ -3,7 +3,7 @@
#include <avr/interrupt.h> #include <avr/interrupt.h>
#include <avr/eeprom.h> #include <avr/eeprom.h>
#include "CFG_HW_Dynamixel.h" #include "CFG_HW_Dynamixel.h"
//#include "TXRX_Dynamixel.h" #include "TXRX_Dynamixel.h"
#include "CTRL_Dynamixel.h" #include "CTRL_Dynamixel.h"
#include "MEM_Dynamixel.h" #include "MEM_Dynamixel.h"
...@@ -39,11 +39,10 @@ ISR( TIMER0_OVF_vect) { ...@@ -39,11 +39,10 @@ ISR( TIMER0_OVF_vect) {
int16_t main(void) { int16_t main(void) {
unsigned char data[128], id, length, instruction, answer[2], status, unsigned char data[128], id, length, instruction, answer[2], status,
en_vector, i, j; en_vector, i, j;
uint16_t ii, jj;
// list of read only registers - to exclude from write // 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, 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 // ini eeprom if first time after run reflash system
if (eeprom_read_byte((uint8_t*) Gate_Restore_Eeprom) != 0xCC) { if (eeprom_read_byte((uint8_t*) Gate_Restore_Eeprom) != 0xCC) {
...@@ -53,209 +52,197 @@ int16_t main(void) { ...@@ -53,209 +52,197 @@ int16_t main(void) {
//------EEPROM initial values------------- //------EEPROM initial values-------------
Restore_EepromVAR(); Restore_EepromVAR();
// get value of Motor ID // get value of Motor ID
//rs485_address = eepromVAR[ID]; rs485_address = eepromVAR[ID];
// configure AVR chip i/o // configure AVR chip i/o
Config_Hardware(); Config_Hardware();
// asign actual position to goal position and asume zero motor turns // asign actual position to goal position and asume zero motor turns
Ini_Position(); Ini_Position();
// initialize the RS485 interface // initialize the RS485 interface
/*init_RS485();*/ init_RS485();
// end of inicialization // end of inicialization
LedOFF; LedOFF;
ii = 0;
jj=1;
while (1) { while (1) {
status = RxRS485Packet(&id, &instruction, &length, data);
Write_word_Dynamixel_RAM(Goal_Position_L,ii); if (status == CHECK_ERROR) {
_delay_ms(500); // funciona TxRS485Packet(rs485_address, CHECKSUM_ERROR, 0, NULL);
//ii=500; } else if (status == CORRECT) {
ii = ii + 100*jj; //LedON;
LedTOGGLE; if (id == rs485_address) {
if (ii == 900) {jj=-1;} switch (instruction) {
if (ii == 100) {jj=1;} //***********************************************************
// P I N G
case INST_PING:
/*status = RxRS485Packet(&id, &instruction, &length, data); TxRS485Packet(rs485_address, NO_ERROR, 0, NULL);
if (status == CHECK_ERROR) { break;
TxRS485Packet(rs485_address, CHECKSUM_ERROR, 0, NULL); //*********************************************************
} else if (status == CORRECT) { // R E A D and send info to pc
//LedON; case INST_READ:
if (id == rs485_address) { if (data[0] <= 18) { // if 0 <= IDinstruction <= 18 caso eeprom
switch (instruction) { if (data[0] >= 0) {
//*********************************************************** switch (data[1]) // tamaño del paquete que pide
// P I N G {
case INST_PING: case 1: // case BYTE
TxRS485Packet(rs485_address, NO_ERROR, 0, NULL); TxRx_Read_byte_Dynamixel_EEPROM(data[0],
break; answer);
//********************************************************* break; //................................................
// R E A D and send info to pc case 2: // case WORD
case INST_READ: TxRx_Read_word_Dynamixel_EEPROM(data[0],
if (data[0] <= 18) { // if 0 <= IDinstruction <= 18 caso eeprom answer);
if (data[0] >= 0) { break; //................................................
switch (data[1]) // tamaño del paquete que pide default:
{ TxRS485Packet(rs485_address, INSTRUCTION_ERROR,
case 1: // case BYTE 0, NULL);
TxRx_Read_byte_Dynamixel_EEPROM(data[0], break; //................................................
answer); }
break; //................................................ } else {
case 2: // case WORD TxRS485Packet(rs485_address, INSTRUCTION_ERROR, 0,
TxRx_Read_word_Dynamixel_EEPROM(data[0], NULL);
answer); } // else id error
break; //................................................ } else if (data[0] <= 86) { // else if 24 <= ID <= 86 ram
default: if (data[0] >= 24) {
TxRS485Packet(rs485_address, INSTRUCTION_ERROR, switch (data[1]) { // tamaño del paquete que pide
0, NULL); case 1: // case BYTE
break; //................................................ TxRx_Read_byte_Dynamixel_RAM(data[0], answer);
} break; //................................................
} else { case 2: // case WORD
TxRS485Packet(rs485_address, INSTRUCTION_ERROR, 0,
NULL); //LedTOGGLE;
} // else id error TxRx_Read_word_Dynamixel_RAM(data[0], answer);
} else if (data[0] <= 86) { // else if 24 <= ID <= 86 ram break; //................................................
if (data[0] >= 24) { default:
switch (data[1]) { // tamaño del paquete que pide TxRS485Packet(rs485_address, INSTRUCTION_ERROR,
case 1: // case BYTE 0, NULL);
TxRx_Read_byte_Dynamixel_RAM(data[0], answer); break; //................................................
break; //................................................ }
case 2: // case WORD } else {
TxRS485Packet(rs485_address, INSTRUCTION_ERROR, 0,
//LedTOGGLE; NULL);
TxRx_Read_word_Dynamixel_RAM(data[0], answer); } // else id error
break; //................................................ } else if (data[0] == 87) { // else if ID = 87 ram
default: switch (data[1]) { // tamaño del paquete que pide
TxRS485Packet(rs485_address, INSTRUCTION_ERROR, case 1: // case BYTE
0, NULL); TxRx_Read_byte_Dynamixel_RAM(data[0], answer);
break; //................................................ break; //................................................
} default:
} else { TxRS485Packet(rs485_address, INSTRUCTION_ERROR, 0,
TxRS485Packet(rs485_address, INSTRUCTION_ERROR, 0, NULL);
NULL); break; //................................................
} // else id error }
} else if (data[0] == 87) { // else if ID = 87 ram } else {
switch (data[1]) { // tamaño del paquete que pide TxRS485Packet(rs485_address, INSTRUCTION_ERROR, 0,
case 1: // case BYTE NULL);
TxRx_Read_byte_Dynamixel_RAM(data[0], answer); } // else id error
break; //................................................ break;
default:
TxRS485Packet(rs485_address, INSTRUCTION_ERROR, 0, //**********************************************************
NULL); // W R I T E info send by pc
break; //................................................ case INST_WRITE:
} en_vector = FALSE; // list of read only registers - to exclude from write
} else { //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};
TxRS485Packet(rs485_address, INSTRUCTION_ERROR, 0, for (i = 0; i < 30; i++) {
NULL); if (data[0] == read_only_vector[i]) {
} // else id error en_vector = TRUE;
break; break;
}
//********************************************************** }
// W R I T E info send by pc
case INST_WRITE: if (en_vector == FALSE) {
en_vector = FALSE; // list of read only registers - to exclude from write if (data[0] <= 18) { // if 0 <= IDinstruction <= 18 caso eeprom
//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}; if (data[0] >= 0) {
for (i = 0; i < 30; i++) { if (Read_byte_Dynamixel_RAM(Lock) == 1) { // is eeprom write lock open
if (data[0] == read_only_vector[i]) { switch (length) // tamaño del paquete que pide
en_vector = TRUE; {
break; case 2: // case BYTE
} TxRx_Write_byte_Dynamixel_EEPROM(
} data[0], data, length);
break; //................................................
if (en_vector == FALSE) { case 3: // case WORD
if (data[0] <= 18) { // if 0 <= IDinstruction <= 18 caso eeprom TxRx_Write_word_Dynamixel_EEPROM(
if (data[0] >= 0) { data[0], data, length);
if (Read_byte_Dynamixel_RAM(Lock) == 1) { // is eeprom write lock open break; //................................................
switch (length) // tamaño del paquete que pide default:
{ TxRS485Packet(rs485_address,
case 2: // case BYTE INSTRUCTION_ERROR, 0, NULL);
TxRx_Write_byte_Dynamixel_EEPROM( break; //................................................
data[0], data, length); }
break; //................................................ switch (data[0]) // aditional instructions for particular registers in eeprom
case 3: // case WORD {
TxRx_Write_word_Dynamixel_EEPROM( case ID: // case EEMPROM and BYTE
data[0], data, length); rs485_address = eepromVAR[ID];
break; //................................................ break; //..................................................
default: case Baud_Rate: // case EEMPROM and BYTE
TxRS485Packet(rs485_address, Baud_Rate_Value();
INSTRUCTION_ERROR, 0, NULL); break; //..................................................
break; //................................................ default:
} ;
switch (data[0]) // aditional instructions for particular registers in eeprom break; //................................................
{ }
case ID: // case EEMPROM and BYTE Write_byte_Dynamixel_RAM(Lock, 0); // close eeprom write lock
rs485_address = eepromVAR[ID]; } else {
break; //.................................................. TxRS485Packet(rs485_address,
case Baud_Rate: // case EEMPROM and BYTE INSTRUCTION_ERROR, 0, NULL);
Baud_Rate_Value(); } // eeprom write lock closed
break; //.................................................. } else {
default: TxRS485Packet(rs485_address, INSTRUCTION_ERROR,
; 0, NULL);
break; //................................................ } // else id error
} } else if (data[0] <= 86) { // else if 24 <= ID <= 86 ram
Write_byte_Dynamixel_RAM(Lock, 0); // close eeprom write lock if (data[0] >= 24) {
} else { switch (length) { // tamaño del paquete que pide
TxRS485Packet(rs485_address, case 2: // case BYTE
INSTRUCTION_ERROR, 0, NULL); TxRx_Write_byte_Dynamixel_RAM(data[0], data,
} // eeprom write lock closed length);
} else { break; //................................................
TxRS485Packet(rs485_address, INSTRUCTION_ERROR, case 3: // case WORD
0, NULL); TxRx_Write_word_Dynamixel_RAM(data[0], data,
} // else id error length);
} else if (data[0] <= 86) { // else if 24 <= ID <= 86 ram break; //................................................
if (data[0] >= 24) { default:
switch (length) { // tamaño del paquete que pide TxRS485Packet(rs485_address,
case 2: // case BYTE INSTRUCTION_ERROR, 0, NULL);
TxRx_Write_byte_Dynamixel_RAM(data[0], data, break; //................................................
length); }
break; //................................................ switch (data[0]) { // aditional instructions for particular registers in ram
case 3: // case WORD case LED: // case RAM and BYTE
TxRx_Write_word_Dynamixel_RAM(data[0], data, if (Read_byte_Dynamixel_RAM(LED) == 1) {
length); LedON;
break; //................................................ } else {
default: LedOFF;
TxRS485Packet(rs485_address, }
INSTRUCTION_ERROR, 0, NULL); break; //................................................
break; //................................................ default:
} ;
switch (data[0]) { // aditional instructions for particular registers in ram break; //................................................
case LED: // case RAM and BYTE }
if (Read_byte_Dynamixel_RAM(LED) == 1) { } else {
LedON; TxRS485Packet(rs485_address, INSTRUCTION_ERROR,
} else { 0, NULL);
LedOFF; } // else id error
} } else if (data[0] == 87) { // else if ID = 87 ram // case BYTE
break; //................................................ TxRx_Write_byte_Dynamixel_RAM(data[0], data,
default: length);
; } else {
break; //................................................ TxRS485Packet(rs485_address, INSTRUCTION_ERROR, 0,
} NULL);
} else { } // else id error
TxRS485Packet(rs485_address, INSTRUCTION_ERROR, } else {
0, NULL); TxRS485Packet(rs485_address, INSTRUCTION_ERROR, 0,
} // else id error NULL);
} else if (data[0] == 87) { // else if ID = 87 ram // case BYTE } // else in only read register list
TxRx_Write_byte_Dynamixel_RAM(data[0], data, break;
length);
} else { //**********************************************************
TxRS485Packet(rs485_address, INSTRUCTION_ERROR, 0, //********* D E F A U L T *****************************
NULL); default:
} // else id error TxRS485Packet(rs485_address, INSTRUCTION_ERROR, 0, NULL);
} else { break;
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;
//***************************************************
}
}
}*/
} }
} }
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment