int
main()
{
char ibuf[256], obuf[1024];
char *p, *q;
int n;
char h[] = "0123456789abcdef";
rs485_init();
sei();
rs485_send("RS485 test program by Y.Kohyama\r\n");
while ((n = rs485_readln(ibuf, sizeof(ibuf)))) {
q = obuf;
for (p = ibuf; p < ibuf + n; p++) {
*q++ = h[*p/16];
*q++ = h[*p%16];
}
*q++ = '\r';
*q++ = '\n';
*q = '\0';
rs485_send(obuf);
_delay_ms(1);
}
}
uint8_t rs485_send_frame(const uint8_t src, const uint8_t dst, const uint8_t *data, unsigned int length)
{
uint8_t *ptr = buffer;
unsigned char sum = length + 5;
*ptr++ = RS485_PREAMBLE;
*ptr++ = src;
*ptr++ = dst;
*ptr++ = length;
while (length--)
{
*ptr++ = *data++;
}
*ptr++ = 0xaa;
return rs485_send(buffer, sum);
}
int main(void)
{
int i;
int led = 0;
unsigned long count = 0;
// unsigned long baud = 57600;
unsigned long baud = 1000000;
int ret;
volatile float imuAngle[3];
// Variables to keep track for reading servos
int nRead=0, idRead=0;
uint8_t *ctableByte;
int serialWait = 0;
// Dynamixel packets and counters for input/output
DynamixelPacket *pktReadData, *pktStatus;
DynamixelPacket pktSerialInput, pktRs485Input;
init();
uart0_printf("\r\nStarting %s\r\n", CONTROLLER_NAME);
uart0_printf("Switching serial to %lu baud...\r\n", baud);
_delay_ms(100);
uart0_setbaud(baud);
uart0_printf("\r\nRunning serial at %lu baud\r\n", baud);
_delay_ms(100);
// Set Dynamixel return delay
rs485_putstrn("\xFF\xFF\xFE\x04\x03\x05\x00\xF5", 8);
while (1) {
count++;
if (count % 1000 == 0) {
// Toggle MON and Dynamixel Leds
if (led) {
led = 0;
sbi(PORTC, PORTC4);
rs485_putstrn("\xFF\xFF\xFE\x04\x03\x19\x00\xE1", 8);
}
else {
led = 1;
cbi(PORTC, PORTC4);
rs485_putstrn("\xFF\xFF\xFE\x04\x03\x19\x01\xE0", 8);
}
_delay_us(100);
}
if (uart0_recv(&pktSerialInput)) {
// RxD LED on
cbi(PORTC, PORTC1);
if (pktSerialInput.id == controlTable.id) {
switch (pktSerialInput.instruction) {
case INST_WRITE:
for (i = 1; i < pktSerialInput.length-2; i++) {
controlTablePtr[pktSerialInput.parameter[0]+i-1] =
pktSerialInput.parameter[i];
}
// Status packet
pktStatus = dynamixel_status(controlTable.id,
0, NULL, 0);
break;
case INST_READ:
pktStatus = dynamixel_status(controlTable.id, 0,
(uchar *)controlTablePtr+pktSerialInput.parameter[0],
pktSerialInput.parameter[1]);
break;
case INST_PING:
pktStatus = dynamixel_status(controlTable.id,
0, NULL, 0);
break;
default:
// Unknown command
pktStatus = dynamixel_status(controlTable.id,
1, NULL, 0);
break;
}
uart0_send(pktStatus);
}
else {
// Forward packet to RS485
rs485_send(&pktSerialInput);
if (pktSerialInput.id != DYNAMIXEL_BROADCAST_ID) {
serialWait = 1;
}
}
// RxD LED off
sbi(PORTC, PORTC1);
} // if (uart0_recv())
if (!serialWait) {
// TxD LED on
cbi(PORTC, PORTC2);
// Query servo for data in round robin fashion:
if (++nRead >= controlTable.nServo) nRead = 0;
idRead = controlTable.idMap[nRead];
pktReadData = dynamixel_instruction_read_data(idRead,
controlTable.addrRead,
controlTable.lenRead);
rs485_send(pktReadData);
// TxD LED off
sbi(PORTC, PORTC2);
} // if (!serialWait)
while (rs485_recv_timeout(&pktRs485Input, 300)) {
// Check if status packet contains requested read data
if (serialWait) {
// Forward packet to uart0
uart0_send(&pktRs485Input);
}
else if ((pktRs485Input.id == idRead) &&
(pktRs485Input.instruction == 0) &&
(pktRs485Input.length == controlTable.lenRead+2)) {
// Packet is correct return, flash EDIT Led
cbi(PORTC, PORTC3);
ctableByte = controlTable.dataRead + nRead*(controlTable.lenRead+1);
*ctableByte++ = idRead;
for (i=0; i<controlTable.lenRead; i++) {
*ctableByte++ = pktRs485Input.parameter[i];
}
sbi(PORTC, PORTC3);
}
} // while (rs485_recv())
// Timeout waiting for serial response
if (serialWait) {
if (++serialWait > 3) serialWait = 0;
}
//check to see if we got full set of adc values
//if the data is ready, it will be copied to the provided pointer
cli(); //disable interrupts to prevent race conditions while copying,
//since the interrupt-based ADC cycle will write asynchronously
ret = adc_get_data(controlTable.imuAcc);
sei(); //re-enable interrupts
/*
if (ret > 0)
ProcessImuReadings(controlTable.imuAcc,controlTable.imuAngle);
*/
/*
if (ret > 0) {
ProcessImuReadings(controlTable.imuAcc,controlTable.imuAngle);
for (i = 0; i<3;i++)
controlTable.imuAngle2[i]= 32768 + 1024* controlTable.imuAngle[i] ;
}*/
if (ret > 0) {
ProcessImuReadings(controlTable.imuAcc,imuAngle);
for (i = 0; i<3;i++)
controlTable.imuAngle[i]= 32768 + (uint16_t) 1024* imuAngle[i] ;
}
if (PINB & _BV(PB4)) {
//if pin high, the button is not pressed
controlTable.button = 0;
LED_ESTOP_PORT &= ~(_BV(LED_ESTOP_PIN));
}
else {
//if pin is low, the button is pressed
controlTable.button = 1;
LED_ESTOP_PORT |= _BV(LED_ESTOP_PIN);
}
} // while (1)
return 0;
}