Skip to content
GitLab
Explore
Sign in
Primary navigation
Search or go to…
Project
R
robotis_tools
Manage
Activity
Members
Labels
Plan
Issues
Issue boards
Milestones
Wiki
Code
Merge requests
Repository
Branches
Commits
Tags
Repository graph
Compare revisions
Build
Pipelines
Jobs
Pipeline schedules
Artifacts
Deploy
Releases
Model registry
Operate
Environments
Monitor
Incidents
Service Desk
Analyze
Value stream analytics
Contributor analytics
CI/CD analytics
Repository analytics
Model experiments
Help
Help
Support
GitLab documentation
Compare GitLab plans
Community forum
Contribute to GitLab
Provide feedback
Keyboard shortcuts
?
Snippets
Groups
Projects
Show more breadcrumbs
humanoides
robotis_tools
Commits
0b698b71
Commit
0b698b71
authored
11 years ago
by
Patrick John Grosch Obregon
Browse files
Options
Downloads
Patches
Plain Diff
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
Changes
2
Hide whitespace changes
Inline
Side-by-side
Showing
2 changed files
servo_firmware/rx28/Makefile
+2
-2
2 additions, 2 deletions
servo_firmware/rx28/Makefile
servo_firmware/rx28/src/main.c
+183
-196
183 additions, 196 deletions
servo_firmware/rx28/src/main.c
with
185 additions
and
198 deletions
servo_firmware/rx28/Makefile
+
2
−
2
View file @
0b698b71
...
@@ -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/
...
...
This diff is collapsed.
Click to expand it.
servo_firmware/rx28/src/main.c
+
183
−
196
View file @
0b698b71
...
@@ -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;
//***************************************************
}
}
}*/
}
}
}
}
This diff is collapsed.
Click to expand it.
Preview
0%
Loading
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Save comment
Cancel
Please
register
or
sign in
to comment