int uart1_puts (const char * s)
{
while ('\0' != *s)
{
(void)uart1_putchar(*s++);
}
(void)uart1_putchar('\n');
return 1;
}
/***** Core System Tasks *****/
void Task_WriteBluetooth()
{
for(;;){
uart1_putchar(joystickDirection);
uart1_putchar(buttonState);
uart1_putchar('\n');
Task_Sleep(5);
}
}
void protocol_send_char(unsigned char ch)
{
switch(ch)
{
case STX: uart1_putchar(ESC); uart1_putchar(PSTX); break;
case ETX: uart1_putchar(ESC); uart1_putchar(PETX); break;
case RESETC: uart1_putchar(ESC); uart1_putchar(PRESET); break;
case ESC: uart1_putchar(ESC); uart1_putchar(ESC); break;
default: uart1_putchar(ch);
}
}
void protocol_send_buffer(unsigned char *src, int len)
{
uint16_t crc16;
crc16 = crc16_calc(src,len);
uart1_putchar(STX);
while(len--)
protocol_send_char(*src++);
protocol_send_char(crc16>>8);
protocol_send_char(crc16&0xff);
uart1_putchar(ETX);
}
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;
}
int main(void)
{
lowLevelInit();
wakeupInit();
exceptionHandlerInit();
int i = 0;
init_Uart0(9600);
init_Uart1(9600);
int buff = 0;
rprintf_devopen(uart0_putchar);
//rprintf_devopen(uart1_putchar);
delay_ms(2000);
rprintf("-------------------------------------------------------------------------\n");
rprintf("-- System Boot complete... --\n");
rprintf("-------------------------------------------------------------------------\n");
printf("You can use printf but it will bloat your code!!!\n");
//printf("Only STDIO printf can print floating-point numbers: %f\n", 1.23f);
delay_ms(1000);
printf("Test Memory allocation of 1024 bytes: ");
char *memPointer = malloc(1024);
printf("Memory pointer = %p\n", memPointer);
uart1_putchar('*');
while (1)
{
rprintf("-- Hello World %i --\n", ++i);
uart1_putchar('*');
delay_ms(1000);
//buff = uart1_getchar();
//uart1_putchar(buff);
}
return 0;
}
void outputStringToUART1 (char* St) {
uint8_t cnt;
if (cellVoltageReading.adcWholeWord < CELL_VOLTAGE_THRESHOLD_UART) return;
if (btFlags & (1<<btSerialBeingInput)) return; // suppress output if user in typing
if (BT_connected()) {
for (cnt = 0; cnt < strlen(St); cnt++) {
uart1_putchar(St[cnt]);
}
}
while (uart1_char_queued_out())
;
} // end of outputStringToUART1
int uart1_putchar(char ch, FILE* fp) {
// End-of-line expansion
if (ch == '\n') {
uart1_putchar('\r', fp);
}
// This must be done atomically with respect to the transmit interrupt.
clear_bit(UCSR1B, TXCIE1);
if (ser_tx_write_pos == ser_tx_read_pos && bit_is_set(UCSR1A, UDRE1)) {
// Send immediately.
UDR1 = ch;
} else {
// Something is being sent now, so add this character to the ring buffer
uint8_t next_write_pos;
while (1) {
next_write_pos = ser_tx_write_pos + 1;
if (next_write_pos == SER_TX_SIZE) {
next_write_pos = 0;
}
if (next_write_pos == ser_tx_read_pos) {
// The ring buffer is full.
// Sleep until something happens and check again.
set_bit(UCSR1B, TXCIE1);
sleep_cpu();
clear_bit(UCSR1B, TXCIE1);
} else {
break;
}
}
ser_tx_buf[ser_tx_write_pos++] = ch;
if (ser_tx_write_pos == SER_TX_SIZE) {
ser_tx_write_pos = 0;
}
}
set_bit(UCSR1B, TXCIE1);
return 0;
}
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;
}
int putchar(int c)
{
return uart1_putchar(c);
}
