void uart0_send_command(uint8_t com, uint8_t* args){
int num_args = 0;
if(com == UART_LED_GREEN_COMMAND || com == UART_LED_RED_COMMAND)
num_args = UART_LED_COMMAND_ARGS;
uint8_t pDelimiter = UART0_LINE_END;
uart0_send(&com, 1);
uart0_send(&pDelimiter, 1);
uart0_send(args, num_args);
}
uint8_t nRF24L01_testSPI(void)
{
uint8_t dat = 0;
uint8_t datBak = 0;
uint8_t status = STATUS_PASS;
/* Dummy read
* First Read always fails
*/
dat = nRF24L01_ReadReg(CMD_READ + REG_EN_AA);
/* Read a default value of REG_EN_AA */
dat = nRF24L01_ReadReg(CMD_READ + REG_EN_AA);
if (dat != 0x3F)
{
return STATUS_FAIL;
}
/* Print SPI Read successful */
uart0_send(p_msgSpiRdPass, 15);
/* Test SPI Write and Read Back */
datBak = nRF24L01_ReadReg(CMD_READ + REG_RF_CH);
/* Write 5 to REG_RF_CH */
nRF24L01_WriteReg(CMD_WRITE + REG_RF_CH, 5);
/* Read back REG_RF_CH register */
dat = nRF24L01_ReadReg(CMD_READ + REG_RF_CH);
if (dat != 5)
{
/* Print message if comparison fails */
uart0_send(p_msgSpiExp, 17);
uart0_send(&dat, 1);
return STATUS_FAIL;
}
nRF24L01_WriteReg(CMD_WRITE + REG_RF_CH, datBak);
/* Print SPI Write successful */
uart0_send(p_msgSpiWrPass, 16);
}
char *puts(char *string)
{
char *p;
for(p = string; *p != '\0';p ++)
{
uart0_send(*p);
}
return string;
}
void ucom_sendString(uint8_t port, uint8_t *msg){
switch(port){
case UART0:
uart0_send(msg);
break;
case UART2:
uart2_send(msg);
break;
default:
//error occured
break;
}
}
void test_uart()
{
char data;
char *string = "hello word";
//串口初始化
uart0_init();
//测试发送
for(;*string != '\0'; string ++)
{
uart0_send(*string);
}
//测试接收
while(1)
{
data = uart0_recv();
uart0_send(data);
}
return ;
}
void ats_send_cmd(u8* buf,u8 len)
{
u8 cmd[32];
u8 index;
u8 i;
index= 0;
cmd[index++] = 'S';
cmd[index++] = 'L';
cmd[index++] = 'B';
cmd[index++] = len+1;//cm+arg0+arg1...+checksume
for(i=0;i<len;i++)
{
cmd[index++]=buf[i];
}
cmd[index++]=check_sum(cmd+3,index-3);
uart0_send(cmd,index);
}
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;
}
