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 ;
}
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;
}
int main(void)
{
int i7, i3, max;
//int16_t capt[16]; /* variable recevant la valeur venant de l'adc */
//int16_t i; /* */
// int16_t val;
char ret;
uint16_t i15,i75;
DDRB=0xFF; /* PORT B en sortie */
DDRA=0x00; /* PORT A en entrée */
PORTB=0xFF; /* LEDS éteintes */
uart_init(); /* initialisation de l'UART, si besoin de communication uart */
/* truc bizarre pour afficher les printf par uart */
/* NDJD: C'est pas bizarre, c'est des pointeurs de fctns... */
fdevopen( uart0_send, uart0_recv);
sei(); /* Autorisation des interruptions */
adc_init(); /* Initialisation de l'adc, si utilisation de l'adc */
gp2d12 g;
printf("\n\n\n");
printf("Programme d'etalonnage du module GP2D12.\n");
do
{
printf("Sur quel port est situe le GP2 ? (1 à 7)\n");
ret = uart0_recv();
printf("%d\n",ret);
}
while( (ret<'1') || (ret>'7') );
g.pin = ret - 48;
printf("Pin %d choisi.\n",g.pin);
printf("\nMettre un obstacle à 15cm du GP2.\n");
printf("Then press a touche.\n");
while( !uart0_recv() )
wait_ms(100);
i15 = gp2d12_adc(&g,100,1);
printf("\nMettre un obstacle à 75cm du GP2.\n");
printf("Then press a touche.\n");
while( !uart0_recv() )
wait_ms(100);
i75 = gp2d12_adc(&g,100,1);
printf("\nMettre un obstacle a 7 cm\n");
printf("Then press a touche.\n");
while( !uart0_recv() )
wait_ms(100);
i7 = gp2d12_adc(&g,100,1);
printf("\nMettre un obstacle a 3 cm\n");
printf("Then press a touche.\n");
while( !uart0_recv() )
wait_ms(100);
i3 = gp2d12_adc(&g,100,1);
printf("\nC'est le moment d'avoir le max\n");
printf("Avancer l'obstacle lentement de 30 a 0 cm plusieurs fois\n");
printf("Then press a touche.\n");
g.max=0;
r=0;
uart0_register_rx_event(recu);
while(!r)
{
printf("%d \r",g.max);
max=gp2d12_adc(&g,100,1);
if (g.max <max)
{
g.max=max;
}
}
//--------------
//printf("i15=%u i75=%u\n\n",i15,i75);
gp2d12_etalon(&g,(double)i15,(double)i75,(double) i3, (double) i7);
printf("\nResultats :\n pin=%u \n a=%f\n b=%f\n ac=%f \n bc=%f \n ",g.pin,g.a,g.b,g.ac,g.bc);
printf("max=%d \n zone=0 \n zonei=0 \n cmp=0 \n\n ",g.max);
g.zone=0;
g.zonei=0;
g.cmpt=0;
while(1)
{
gp2d12_update(&g);
printf("\r zone : %d, zonei : %d, distance : %5.d cm",g.zone,g.zonei,g.d);
PORTB++;
wait_ms(1);
}
return 0;
}
