int main(void)
{
int c;
int led = 0;
unsigned long count = 0;
unsigned long baud = 1000000;
init();
uart1_putstr("\r\nStarting CM720 Dynamixel relay controller\r\n");
uart1_printf("Switching serial to %lu baud...\r\n", baud);
_delay_ms(100);
uart1_setbaud(baud);
uart1_printf("\r\nRunning serial at %lu baud\r\n", baud);
_delay_ms(100);
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);
}
}
c = uart1_getchar();
while (c != EOF) {
// RxD LED on
cbi(PORTC, PORTC1);
rs485_putchar(c);
c = uart1_getchar();
}
// RxD LED off
sbi(PORTC, PORTC1);
c = rs485_getchar();
while (c != EOF) {
// TxD LED on
cbi(PORTC, PORTC2);
uart1_putchar(c);
c = rs485_getchar();
}
// TxD LED off
sbi(PORTC, PORTC2);
}
return 0;
}
char* uart1_gets (char * s, int size)
{
char *p = s;
int len = 0;
while (len < size)
{
int c = uart1_getchar();
if (EOF == c)
{
*p = '\0';
return NULL;
}
*p++ = c;
if ('\n' == c)
{
break;
}
}
*p = '\0';
return s;
}
int main(void)
{
UINT8 data;
BIT_SET(L_OUT_DDR,L_OUT_PIN); //L_OUT pin is output
BIT_SET(K_OUT_DDR,K_OUT_PIN); //K_OUT pin is output
K_OUT(1);
L_OUT(1);
uart1_init(10400);
uart_init(115200); //Set up UART
TimeInit(); //set up time
//set up printf
fdevopen(uart_putchar, NULL);
printf("Hello world\n");
while (1)
{
if (uart1_kbhit())
{
data=uart1_getchar();
printf("0x%02X ",data);
//uart_putchar(data);
}
if (uart_kbhit())
{
data=uart_getchar();
uart1_putchar(data);
}
}
}
int main(void)
{
int c;
int ret;
uint8_t id;
uint8_t type;
uint8_t * data;
int16_t cntr = 0;
DDRA |= _BV(PA6) | _BV(PA7);
DDRC = 0xFF;
//PORTC= 0X00;
int ii=0;
uint8_t val = 1;
init();
uart0_printf("estop initialized\r\n");
DynamixelPacket xbeePacket;
DynamixelPacketInit(&xbeePacket);
uint8_t robotId;
uint8_t robotState;
const int bufSize = 256;
uint8_t buf[bufSize];
uint8_t * pbuf;
uint8_t estopPacketLen;
const int nRobots = 9;
uint8_t mode[nRobots+1] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
estopPacketLen = DynamixelPacketWrapData(MMC_ESTOP_DEVICE_ID,
MMC_ESTOP_STATE,mode,nRobots+1,buf,bufSize);
uint8_t modVal;
while(1)
{
cntr++;
if (cntr == 1000)
cntr = 0;
modVal = cntr % 200;
if (modVal == 0)
{
//generate new packet based on the latest desired estop state
estopPacketLen = DynamixelPacketWrapData(MMC_ESTOP_DEVICE_ID,
MMC_ESTOP_STATE,commandedState,nRobots+1,buf,bufSize);
pbuf = buf;
for (ii=0; ii<estopPacketLen; ii++)
uart1_putchar(*pbuf++);
//uart0_printf("sent estop packet\r\n");
}
//show the actual status for brief moment
if (modVal < 50)
{
ShowActualStateAll();
}
else
{
ShowCommandedStateAll();
}
//read from xbee
while( (c = uart1_getchar()) != EOF )
{
//uart0_printf("%X ",c);;
ret = DynamixelPacketProcessChar(c,&xbeePacket);
if (ret > 0)
{
id = DynamixelPacketGetId(&xbeePacket);
type = DynamixelPacketGetType(&xbeePacket);
data = DynamixelPacketGetData(&xbeePacket);
if (id == MMC_ESTOP_DEVICE_ID && type == MMC_ESTOP_STATE)
{
robotId = data[0];
robotState = data[1];
if ( (robotId > 0) && (robotId < 11) && (robotState >= 0) && (robotState < 3) )
{
actualState[robotId] = robotState;
uart0_printf("Robot%d: estop = %d\r\n",robotId,robotState);
}
else
uart0_printf("xbee received bad data : Robot%d: estop = %d\r\n",robotId,robotState);
}
}
}
READ_ESTOP_STATE(1);
READ_ESTOP_STATE(2);
READ_ESTOP_STATE(3);
READ_ESTOP_STATE(4);
READ_ESTOP_STATE(5);
READ_ESTOP_STATE(6);
READ_ESTOP_STATE(7);
READ_ESTOP_STATE(8);
READ_ESTOP_STATE(9);
READ_ESTOP_STATE(10);
//check the master pause button
if ( (READ_VAL(RP_IN0_PORT,RP_IN0_PIN)) && debounceCntr[0] == DEBOUNCE_DELAY )
{
if ( commandedState[0] == 1 )
{
for (ii=0; ii<11; ii++)
commandedState[ii] = 0;
}
else if (commandedState[0]==0)
{
for (ii=0; ii<11; ii++)
commandedState[ii] = 1;
}
debounceCntr[0] = 0;
}
if (debounceCntr[0] < DEBOUNCE_DELAY)
debounceCntr[0]++;
SHOW_COMMANDED_STATE(0);
/*
if ( READ_VAL(RP_IN1_PORT,RP_IN1_PIN) )
SET_PIN(LED_OUT1_PORT,LED_OUT1_PIN);
else
CLEAR_PIN(LED_OUT1_PORT,LED_OUT1_PIN);
*/
/*
if (val == 0)
DDRC=0x00;
//val = 1;
else
val = val << 1;
PORTC = val;
*/
//PORTC = PINL;
_delay_ms(5);
}
return 0;
}
