Commit ec55f73f authored by John Pereira's avatar John Pereira
Browse files

dynamixel and prueba1 working

parent f61748bb
......@@ -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_hal_close();
serial_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_hal_rx( (unsigned char*)&gbStatusPacket[gbRxGetLength], gbRxPacketLength - gbRxGetLength );
nRead = serial_read( (unsigned char*)&gbStatusPacket[gbRxGetLength], gbRxPacketLength - gbRxGetLength );
gbRxGetLength += nRead;
if( gbRxGetLength < gbRxPacketLength )
{
......
#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;
}
......@@ -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_qstate(void)
int serial_close(void)
{
close(fd);
return 1;
......
......@@ -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_qstate(void);
int serial_close(void);
#ifdef __cplusplus
}
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment