vector<uint8_t> ModCom::comunicate(vector<uint8_t> buffer, bool readdata, int sizeread) {
uint8_t *buffer_out = &(buffer[0]);
uint8_t buffer_in[255];
serialport_flush(fd);
serialport_write(fd, (char*)buffer_out);
if (!readdata) {
vector<uint8_t> toreturn;
return toreturn;
} // if only send
//serial_read(fd, buffer_in, 1, DEF_TIMEOUT);
serialport_read(fd, buffer_in, sizeread, DEF_TIMEOUT);
if (buffer_in[0] != 0xFF) {
vector<uint8_t> toreturn;
return toreturn;
}
vector<uint8_t> vr(buffer_in, buffer_in + sizeread);
return vr;
}
int serialport_receive_slip(unsigned char *data, unsigned int size)
{
unsigned int received;
unsigned char cur_byte;
received = 0;
while(received != size)
{
if(serialport_read(&cur_byte, 1) != 1)
{
LOGERR("failed reading byte");
return 0;
}
if(cur_byte == 0xDB)
{
if(serialport_read(&cur_byte, 1) != 1)
{
LOGERR("failed reading byte for unslip");
return 0;
}
if(cur_byte == 0xDC)
{
*data++ = 0xC0;
}
else if(cur_byte == 0xDD)
{
*data++ = 0xDB;
}
else
{
LOGERR("unslip sequence wrong");
return 0;
}
}
else
{
*data++ = cur_byte;
}
received++;
}
return received;
}
/* Defines the shifting and speed of the visual image. */
void speedControl() {
serialport_read(fd, serialbuffer); // Grabs the next byte from usbserial.
speed = fabs(127-(rawTorque+bias)); // Absolute value of how far away from the midline you are.
speed = pow((speed/50),2); // (how far from the midline you are / 50)^2
if ((rawTorque+bias)<127) {
shift=shift+(gain*speed);
} else {
shift=shift-(gain*speed);
}
shiftWrap();
}
int serialport_receive_C0(void)
{
unsigned char b;
b = 0x00;
if(serialport_read(&b, 1) != 1)
{
return 0;
}
if(b != 0xC0)
{
LOGDEBUG("serialport_receive_C0: %02X instead of C0", b);
return 0;
}
return 1;
}
void* download_recv_thread(void* thread_arg){
static WORD RecvSum;
static BYTE RecvIndex;
static BYTE RecvCount;
int exitthread=0;
int ret;
int readlength,consumed;
BYTE dat;
while(!exitthread){
// platform_usleep(1000);
readlength = serialport_read(handle_download_port,&dat,1,0);
if(readlength>0)
{
//printf("got uart bytes dat=0x%x UartRecvStep=0x%x\n",dat,UartRecvStep);
switch (UartRecvStep)
{
case 1:
if (dat != 0xb9) goto L_CheckFirst;
UartRecvStep++;
break;
case 2:
if (dat != 0x68) goto L_CheckFirst;
UartRecvStep++;
break;
case 3:
if (dat != 0x00) goto L_CheckFirst;
UartRecvStep++;
break;
case 4:
RecvSum = 0x68 + dat;
RecvCount = dat - 6;
RecvIndex = 0;
UartRecvStep++;
break;
case 5:
RecvSum += dat;
RxBuffer[RecvIndex++] = dat;
if (RecvIndex == RecvCount) UartRecvStep++;
break;
case 6:
if (dat != HIBYTE(RecvSum)) goto L_CheckFirst;
UartRecvStep++;
break;
case 7:
if (dat != LOBYTE(RecvSum)) goto L_CheckFirst;
UartRecvStep++;
break;
case 8:
if (dat != 0x16) goto L_CheckFirst;
UartReceived = TRUE;
UartRecvStep++;
break;
L_CheckFirst:
case 0:
default:
CommInit();
UartRecvStep = (dat == 0x46 ? 1 : 0);
break;
}
}
}
return NULL;
}
int main(void)
{
modbus_t *ctx;
int i;
uint16_t tab_reg[64];
int rc;
uint16_t b[3] = {0x6101, 0x0fa0, 0x0fa0};
int fd =0;
char buf[256];
char value;
ctx = modbus_new_rtu("/dev/ttyUSB0", 38400, 'N', 8, 1);
modbus_set_slave(ctx, SERVER_ID); // 엄지 선택
modbus_set_debug(ctx, 1);
if (modbus_connect(ctx) == -1) {
fprintf(stderr, "Connection failed: %s\n",
modbus_strerror(errno));
modbus_free(ctx);
return -1;
}
usleep(15000);
x_axis_stop(ctx);
// Arduino init
fd = serialport_init("/dev/ttyUSB1", 9600);
while (1)
{
value = serialport_read(fd);
if (value != -1) {
printf("From arduino: %d \n",value);
}
usleep(1500);
switch (value) {
case 1:
printf("엄지 선택\n");
modbus_set_slave(ctx, 1);
break;
case 2:
printf("검지 선택 \n");
modbus_set_slave(ctx, 2);
break;
case 3:
printf("중지 선택\n");
modbus_set_slave(ctx, 3);
break;
case 4:
printf("약지 선택\n");
modbus_set_slave(ctx, 4);
break;
case 5:
printf("약지 선택\n");
modbus_set_slave(ctx, 5);
break;
case 'b' :
printf("b\n");
x_axis_stop(ctx);
x_axis_turn_right(ctx);
break;
case 'f' :
printf("f\n");
x_axis_stop(ctx);
x_axis_turn_left(ctx);
break;
}
}
x_axis_stop(ctx);
/* Close the connection */
modbus_close(ctx);
modbus_free(ctx);
return 0;
}
