Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Menu
Open sidebar
futbot
futbot2014
Commits
ec55f73f
Commit
ec55f73f
authored
Sep 11, 2014
by
John Pereira
Browse files
dynamixel and prueba1 working
parent
f61748bb
Changes
4
Hide whitespace changes
Inline
Side-by-side
salimoo/Golero/DynamixelCustom/dynamixel.c
View file @
ec55f73f
...
...
@@ -35,12 +35,16 @@ int dxl_initialize( int devIndex, int baudnum )
//gbCommStatus = COMM_RXSUCCESS;
//giBusUsing = 0;
return
dxl_hal_open
(
devIndex
,
baudrate
);
//return dxl_hal_open(devIndex, baudrate);
gbInstructionPacket
[
0
]
=
0xff
;
gbInstructionPacket
[
1
]
=
0xff
;
serial_initialize
();
return
1
;
}
void
dxl_terminate
(
void
)
{
dxl_h
al_close
();
seri
al_close
();
}
void
dxl_tx_packet
()
...
...
@@ -75,36 +79,38 @@ void dxl_tx_packet()
return
;
}
gbInstructionPacket
[
0
]
=
0xff
;
gbInstructionPacket
[
1
]
=
0xff
;
//
gbInstructionPacket[0] = 0xff;
//
gbInstructionPacket[1] = 0xff;
for
(
i
=
0
;
i
<
(
gbInstructionPacket
[
LENGTH
]
+
1
);
i
++
)
checksum
+=
gbInstructionPacket
[
i
+
2
];
gbInstructionPacket
[
gbInstructionPacket
[
LENGTH
]
+
3
]
=
~
checksum
;
if
(
gbCommStatus
==
COMM_RXTIMEOUT
||
gbCommStatus
==
COMM_RXCORRUPT
)
dxl_hal_clear
();
/*if( gbCommStatus == COMM_RXTIMEOUT || gbCommStatus == COMM_RXCORRUPT )
dxl_hal_clear();
*/
TxNumByte
=
gbInstructionPacket
[
LENGTH
]
+
4
;
printf
(
"Envio paquete:
\n
"
);
RealTxNumByte
=
dxl_hal_tx
(
(
unsigned
char
*
)
gbInstructionPacket
,
TxNumByte
);
//RealTxNumByte = dxl_hal_tx( (unsigned char*)gbInstructionPacket, TxNumByte );
serial_write
(
(
unsigned
char
*
)
gbInstructionPacket
,
TxNumByte
);
if
(
TxNumByte
!=
RealTxNumByte
)
/*
if( TxNumByte != RealTxNumByte )
{
printf("FALLO TRNSMISION: menos bytes de los que deberia");
gbCommStatus = COMM_TXFAIL;
giBusUsing = 0;
return;
}
*/
if
(
gbInstructionPacket
[
INSTRUCTION
]
==
INST_CAPTURE
)
giBusUsing
=
0
;
/*
if( gbInstructionPacket[INSTRUCTION] == INST_READ )
dxl_hal_set_timeout( gbInstructionPacket[PARAMETER+1] + 6 );
else
dxl_hal_set_timeout( 6 );
*/
gbCommStatus
=
COMM_TXSUCCESS
;
}
...
...
@@ -128,9 +134,12 @@ void dxl_rx_packet()
gbRxGetLength
=
0
;
gbRxPacketLength
=
6
;
}
// nRead = dxl_hal_rx( (unsigned char*)&gbStatusPacket[gbRxGetLength], gbRxPacketLength - gbRxGetLength );
nRead
=
serial_read
((
unsigned
char
*
)
&
gbStatusPacket
[
gbRxGetLength
],
gbRxPacketLength
-
gbRxGetLength
);
nRead
=
dxl_hal_rx
(
(
unsigned
char
*
)
&
gbStatusPacket
[
gbRxGetLength
],
gbRxPacketLength
-
gbRxGetLength
);
gbRxGetLength
+=
nRead
;
gbRxGetLength
+=
nRead
;
/*
if( gbRxGetLength < gbRxPacketLength )
{
if( dxl_hal_timeout() == 1 )
...
...
@@ -143,7 +152,7 @@ void dxl_rx_packet()
return;
}
}
*/
// Find packet header
for
(
i
=
0
;
i
<
(
gbRxGetLength
-
1
);
i
++
)
{
...
...
@@ -178,10 +187,11 @@ void dxl_rx_packet()
return
;
}
gbRxPacketLength
=
gbStatusPacket
[
LENGTH
]
+
4
;
gbRxPacketLength
=
gbStatusPacket
[
LENGTH
]
+
4
;
if
(
gbRxGetLength
<
gbRxPacketLength
)
{
nRead
=
dxl_h
al_r
x
(
(
unsigned
char
*
)
&
gbStatusPacket
[
gbRxGetLength
],
gbRxPacketLength
-
gbRxGetLength
);
nRead
=
seri
al_r
ead
(
(
unsigned
char
*
)
&
gbStatusPacket
[
gbRxGetLength
],
gbRxPacketLength
-
gbRxGetLength
);
gbRxGetLength
+=
nRead
;
if
(
gbRxGetLength
<
gbRxPacketLength
)
{
...
...
salimoo/Golero/prueba1.c
View file @
ec55f73f
#include
"
Forward.c
"
#include
"
DynamixelCustom/dynamixel.h
"
#include
<stdlib.h>
#include
<unistd.h>
int
main
(
void
){
fsr
();
sleep
(
10000
);
fsr
();
dxl_initialize
(
1
,
1
);
dxl_set_txpacket_id
(
1
);
dxl_set_txpacket_instruction
(
3
);
dxl_set_txpacket_length
(
5
);
dxl_set_txpacket_parameter
(
0
,
25
);
dxl_set_txpacket_parameter
(
1
,
1
);
dxl_tx_packet
();
return
1
;
}
salimoo/Golero/serial.c
View file @
ec55f73f
...
...
@@ -155,7 +155,7 @@ unsigned char serial_read( unsigned char *pData, int numbyte )
/* fd: Serial device descriptor */
/********************************************************************/
//void serial_close(int fd)
int
serial_
get_qstat
e
(
void
)
int
serial_
clos
e
(
void
)
{
close
(
fd
);
return
1
;
...
...
salimoo/Golero/serial.h
View file @
ec55f73f
...
...
@@ -19,7 +19,7 @@ int fd;
void
serial_initialize
(
void
);
void
serial_write
(
unsigned
char
*
pData
,
int
numbyte
);
unsigned
char
serial_read
(
unsigned
char
*
pData
,
int
numbyte
);
int
serial_
get_qstat
e
(
void
);
int
serial_
clos
e
(
void
);
#ifdef __cplusplus
}
...
...
Write
Preview
Supports
Markdown
0%
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!
Cancel
Please
register
or
sign in
to comment