void main()
{
uart_init(9600);
vt_init();
sonar_t so;
sonar_t so2;
sonar_init(&so, A0, A1);
sonar_init(&so2, A2, A3);
sei();
// end
int16_t res;
while(1) {
vt_clear();
vt_home();
uart_puts_pgm(PSTR("SONAR\r\n==================="));
// first
vt_goto(99, 99); // move cursor away
sonar_start(&so);
while(sonar_busy());
res = sonar_result();
// print value
vt_goto(0, 2);
uart_puts_pgm(PSTR("A: "));
if (res < 0) {
uart_puts_pgm(PSTR("No Obstacle"));
} else {
uart_puti(res, 1);
uart_puts_pgm(PSTR(" cm"));
}
// second
vt_goto(99, 99); // move cursor away
sonar_start(&so2);
while(sonar_busy());
res = sonar_result();
// print value
vt_goto(0, 3);
uart_puts_pgm(PSTR("B: "));
if (res < 0) {
uart_puts_pgm(PSTR("No Obstacle"));
} else {
uart_puti(res, 1);
uart_puts_pgm(PSTR(" cm"));
}
vt_goto(99, 99); // move cursor away
_delay_ms(200);
}
}
void main()
{
lcd_init();
// Init sonar
sonar_t so;
sonar_init(&so, A0, A1);
sei();
while(1) {
// Measure
sonar_start(&so);
while(sonar_busy);
int16_t res = sonar_result;
// Print
lcd_clear();
if (sonar_result < 0) {
put_str(lcd, "---");
} else {
put_i16f(lcd, res, 1); // one decimal place
put_str(lcd, " cm");
}
// a delay...
_delay_ms(200);
}
}
int main(void)
{
struct heading result;
/*u08 k=0;
u08 b=0;
u08 choice=0;
u08 button=0;*/
u08 ir=0;
u08 i=0;
x = 64;
y = 48;
initialize();
motor_init();
servo_init();
set_motor_power(0,0);
set_motor_power(1,0);
set_motor_power(2,0);
set_motor_power(3,0);
compass_init();
sonar_init();
calCompass();
while(1)
{
clear_screen();
result = compass();
/* use calibration data */
result.x -= compZero.x;
result.y -= compZero.y;
print_string("x "); /* 2 */
print_int(result.x); /* 3 */
print_string(" y "); /* 3 */
print_int(result.y); /* 3 */
/* print_string(" s "); */ /* 3 */
/* print_int(getSonar(0)); */ /* 3 */
/* =17 */
next_line();
/* print_string("a "); */ /* 2 */
/* print_int(IR(0)); */ /* 3 */
/* print_string(" b "); */ /* 3 */
/* print_int(analog(1)); */ /* 3 */
/* print_string(" c "); */ /* 3 */
/* print_int(analog(2)); */ /* 3 */
/* print_int(i++); */ /* =17 */
/* print_int(distance(0));
print_string(" "); */
print_int(sizeof(int));
delay_ms(200);
/* print_int(i);
i=sweep(); */
}
}
int
main(void){
cli();
CLOCK_8MHZ();
DDRD = 0xff;
PORTD = 0xff; // Turn on both LEDs
uart_init();
sonar_init();
sei();
uart_write((uint8_t*)"UART\r\n", 6);
trigger_sonar();
for(;;);
}
void custom_init(void)
{
int c;
/*
* Below are custom configurations for your robot. This is where
* you set up digital output ports, analog ports, sensors, etc.
*/
/* Set some of the 16 I/O ports as inputs */
for (c = 1; c <= 16; ++c)
io_set_direction(c, IO_DIRECTION_IN);
/*
* Configure the number of analog input ports. Ports 1 through N
* will be analog, and N+1 through 16 will be digital.
*/
io_set_analog_port_count(ANALOG_PORTS);
/* Set some of the 16 I/O ports as outputs (digital output only). */
/* These can be used to drive LEDs and other low-current devices. */
//for (c = 9; c <= 16; ++c)
// io_set_direction(c, IO_DIRECTION_OUT);
/* Initialize the values on the digital outputs. */
//for (c = 9; c <= 16; ++c)
// io_set_digital(c,0);
/* Enable shaft encoder on any interrupt port */
/* The count can be read back with shaft_encoder_read_count() */
shaft_encoder_enable_std(LEFT_ENCODER_INTERRUPT_PORT);
shaft_encoder_enable_std(RIGHT_ENCODER_INTERRUPT_PORT);
/* Example: Quad encoder requires two pots */
// quad_encoder_enable(6, 8);
/* Enable sonar device and timer. */
if ( sonar_init(SONAR_INTERRUPT_PORT, SONAR_OUTPUT_PORT) != OV_OK )
printf("sonar_init() failed.\n");
timer_start(1);
controller_print_version();
}
void readings_stream_mode()
{
const uint8_t BUF_LENGTH = 100;
char buf[BUF_LENGTH];
enum {
reading_ir = 1,
reading_sonar = 2,
reading_sonar_mode = 3
} reading;
init_push_buttons();
lcd_init();
ir_init();
sonar_init();
usart_init();
usart_drain_rx();
while (true) {
switch (wait_button("Readings Stream")) {
case reading_ir:
snprintf(buf, BUF_LENGTH, "IR: %u\n", ir_raw_reading());
break;
case reading_sonar:
snprintf(buf, BUF_LENGTH, "Sonar: %u\n", sonar_raw_reading());
break;
case reading_sonar_mode:
while (true) {
snprintf(buf, BUF_LENGTH, "Sonar: %u\n", sonar_raw_reading());
usart_tx_buf(buf);
}
break;
default:
buf[0] = '\0';
}
usart_tx_buf(buf);
}
}
int
main(int argc, char *argv[])
{
motor_t rightMotor = {
&PORTB, &DDRB, PORTB7, // PWM
&PORTC, &DDRC, PORTC1, // Direction
&OCR0A // Top value
};
motor_t leftMotor = {
&PORTD, &DDRD, PORTD0, // PWM
&PORTB, &DDRB, PORTB0, // Direction
&OCR0B // Top value
};
//uint8_t sonarDistance = 0;
cli();
NO_CLK_PRESCALE();
//uart_init();
radio_init(HOV1_ADDRESS, RECEIVE_MODE);
sonar_init();
motorInit(&rightMotor);
motorInit(&leftMotor);
pwmInit();
sei();
stop = Event_Init();
sendPing();
while(!receivedInit);
Task_Create((void*)(&motor_task),0, PERIODIC, MOTOR);
Task_Create((void*)(&fire_sonar),0, PERIODIC, FIRE);
Task_Create((void*)(&listen_sonar),0,PERIODIC, LISTEN);
Task_Create((void*)(&sendRadio),0,PERIODIC, RADIO);
Task_Create((void*)(&stopSystem), 0,SYSTEM, STOP);
setMotorDirection(&rightMotor, FORWARD);
setMotorDirection(&leftMotor, FORWARD);
return 0;
}
int main() {
board_init();
uart_init();
esc_init();
rgbled_init();
rgbled_set(0xFF8000, 100);
spektrum_init();
rgbled_set(0xFF8000, 100);
xbee_init();
i2c_init();
alt_init();
mag_init();
mpu_init();
sonar_init();
ins_init();
inscomp_init();
altitude_init();
controller_init();
basestation_init();
flight_init();
controlpanel_run();
}
int main()
{
// string buffer for printing to UART
uint8_t output[128];
// disable the clock prescaler
clock8MHz();
// disable interrupts during setup
cli();
uart_init(UART_38400);
sonar_init();
// enable interrupts
sei();
for (;;)
{
sonar_trigger();
_delay_ms(36);
if (sonar_echo_received() == 1) {
distance = 0.0171 * sonar_get_distance() - 0.8192;
snprintf((char*)output, 128, "distance %d\n\r", (int)distance);
print(output);
}else {
snprintf((char*)output, 128, "sonar failed\n\r");
print(output);
}
_delay_ms(1000);
}
return 0;
}
int main(void)
{
uint16_t counter;
char c;
uint8_t stop = TRUE;
usart_init();
sonar_init();
rtc_setup();
counter = 0;
usart_resume(0);
strcpy_P(usart->tx0_buffer, PSTR("\nTsunami Simulator "));
strcat_P(usart->tx0_buffer, PSTR(GITREL));
strcat_P(usart->tx0_buffer, PSTR("\n\nConnected!\n"));
usart_printstr(0, NULL);
rtc_start();
while (1) {
/* Restart the counter */
rtc_clear();
c = usart_getchar(0, FALSE);
switch (c) {
case '0':
stop = TRUE;
break;
case '1':
stop = FALSE;
break;
default:
break;
}
if (stop) {
/* stop the code */
while(usart_getchar(0, FALSE) != '1');
stop = FALSE;
rtc_clear();
}
/* send the trigger */
sonar_trigger();
/* clear all the data */
sonar_clear();
/*
* Wait 40mS maximum and collect all the
* data during the period.
*/
while (rtc_us < 4000)
sonar_set();
/*
* speed = ((i * SCALEuS)/1000000) * 340 / 2
* where:
* i * SCALEuS is the duration in uS of the signal.
* (i * SCALEuS)/1000000 is the same in seconds.
* 340 is the speed of the sound and
* /2 we need only half of the way.
* The simplyfied formula in cm.
* 340 mm/msec = 34cm/msec = 0.029 msec/cm = 29 uS/cm
* dist (cm) = T (uS) / 29 /2.
*/
usart->tx0_buffer = utoa(counter, usart->tx0_buffer, 10);
usart_printstr(0, NULL);
usart_printstr(0, " ");
counter++;
sonar_print();
/* if the counter has already reach 50mS,
* then this cycle takes too long.
*/
if (rtc_us > 5000)
usart_printstr(0, "Warning! Time overrun.\n");
/* Wait up to 50mS before restart */
while (rtc_us < 5000);
}
return(0);
}
int main(void)
{
initialize();
servo_init();
motor_init();
sonar_init();
set_servo_position(0, 120); //center the servo
print_string("Rodentia Demo");
next_line();
print_string("by Austin");
led_on();
delay_ms(100);
led_off();
delay_ms(100);
led_on();
delay_ms(100);
led_off();
//wait for a start signal
while(!get_sw1())
{
/*u08 temp;
clear_screen();
temp = analog(0);
print_int(temp);*/
delay_ms(50);
}
led_on();
while(1)
{ //find and point toward heading with minimum sonar reading
u08 heading = 0;
/*u16 minHeading = 0;
u16 left = 0;
u08 aLeft = 1;
u16 right = 0;
u08 aRight = 1;*/
u16 sonar = 0x0;
u16 temp = 0;
set_servo_position(0, 120); //center the servo
set_motor_power(0, MIN_POWER);
set_motor_power(1, -MIN_POWER);
while(heading < 170) //what's the magic number?
{
/*u08 tempL = analog(1);
u08 tempR = analog(2);
if(tempL < 25 && aLeft)
{
set_motor_power(0, 0);
left++;
aLeft = 0;
}
if(tempL > 180 && !aLeft)
{
set_motor_power(0, 0);
left++;
aLeft = 1;
}
if(tempR < 25 && aRight)
{
set_motor_power(1, 0);
right++;
aRight = 0;
}
if(tempR > 180 && !aRight)
{
set_motor_power(1, 0);
right++;
aRight = 1;
}
if(left > heading && right > heading)*/
{
heading++;
temp = getSonar(0);
if(IR(0) > 70)
{
temp = 0;
}
if(temp > sonar)
{
sonar = temp;
//minHeading = heading;
}
clear_screen();
print_string("here=");
print_int(temp);
print_string(" max=");
print_int(sonar);
/*next_line();
print_string("Hdng=");
print_int(heading);
print_string(" minH=");
print_int(minHeading);
if(left < right)
{ //left motor on
set_motor_power(0, MIN_POWER);
}
else if (right < left)
{ //right motor on
set_motor_power(1, -MIN_POWER);
}
else
{
set_motor_power(0, MIN_POWER);
set_motor_power(1, -MIN_POWER);
}*/
}
}
if(sonar > 140)
{ //set a reasonable maximum value for sonar
sonar = 140;
}
//turn back to minumum heading
temp = 0;
while( temp < sonar )
{
temp = getSonar(0) + 3;
if(IR(0) > 70)
{
temp = 0;
}
clear_screen();
print_string("here=");
print_int(temp);
print_string(" max=");
print_int(sonar);
}
/*set_motor_power(0, -MIN_POWER);
set_motor_power(1, MIN_POWER);
while(heading > minHeading)
{
u08 tempL = analog(1);
u08 tempR = analog(2);
if(tempL < 25 && aLeft)
{
set_motor_power(0, 0);
left--;
aLeft = 0;
}
if(tempL > 180 && !aLeft)
{
set_motor_power(0, 0);
left--;
aLeft = 1;
}
if(tempR < 25 && aRight)
{
set_motor_power(1, 0);
right--;
aRight = 0;
}
if(tempR > 180 && !aRight)
{
set_motor_power(1, 0);
right--;
aRight = 1;
}
if(left < heading && right < heading)
{
heading--;
clear_screen();
print_string("L=");
print_int(left);
print_string(" R=");
print_int(right);
next_line();
print_string("Hdng=");
print_int(heading);
print_string(" minH=");
print_int(minHeading);
if(left > right)
{ //left motor on
set_motor_power(0, -MIN_POWER);
}
else if (right > left)
{ //right motor on
set_motor_power(1, MIN_POWER);
}
else
{
set_motor_power(0, -MIN_POWER);
set_motor_power(1, MIN_POWER);
}
}
}*/
set_motor_power(0, 0);
set_motor_power(1, 0);
//scan with IR for obstacles
delay_ms(500);
u08 dir=120;
s08 dDir = 1;
u08 dist = IR(0);
set_motor_power(0, 100);
set_motor_power(1, 100);
while(dist < 100)
{
dir += dDir;
set_servo_position(0, dir);
if(dir >= 200)
{
dDir = -1;
}
if(dir <= 40)
{
dDir = 1;
}
dist = IR(0);
clear_screen();
print_string("Distance=");
print_int(dist);
}
}
/*u08 temp;
while(1)
{
clear_screen();
temp = analog(0);
print_int(temp);
set_servo_position(0,temp);
delay_ms(50);
}*/
}
void init_state() {
static int first = 1;
int retval;
if(first) {
if(debug)
spawn_debug_thread();
#ifdef USE_KILL_SWITCH
//initialize kill switch
retval = switch_init();
if(retval != SWITCH_NO_ERROR) {
snprintf(state_data.error_str, sizeof(state_data.error_str), SWITCH_ERROR_STR(retval));
next_state = ERROR_STATE;
return;
}
kill_switch_initialized = 1;
#endif
#ifdef USE_GPS
// initialize gps
retval = gps_init();
if(retval != GPS_NO_ERROR) {
snprintf(state_data.error_str, sizeof(state_data.error_str), GPS_ERROR_STR(retval));
next_state = ERROR_STATE;
return;
}
gps_initialized = 1;
#endif
#ifdef USE_SONAR
// initialize sonar sensors
retval = sonar_init();
if(retval != SONAR_NO_ERROR) {
snprintf(state_data.error_str, sizeof(state_data.error_str), SONAR_ERROR_STR(retval));
next_state = ERROR_STATE;
return;
}
sonar_initialized = 1;
#endif
#ifdef USE_COMPASS
//initialize compass
retval = compass_init();
if(retval != COMPASS_NO_ERROR) {
snprintf(state_data.error_str, sizeof(state_data.error_str), SONAR_ERROR_STR(retval));
next_state = ERROR_STATE;
return;
}
compass_initialized = 1;
#endif
#ifdef USE_CAMERA
//initialize camera
retval = camera_init();
if(retval != CAMERA_NO_ERROR) {
snprintf(state_data.error_str, sizeof(state_data.error_str), CAMERA_ERROR_STR(retval));
next_state = ERROR_STATE;
return;
}
retval = camera_start_tracking();
if(retval != CAMERA_NO_ERROR) {
snprintf(state_data.error_str, sizeof(state_data.error_str), CAMERA_ERROR_STR(retval));
next_state = ERROR_STATE;
return;
}
camera_initialized = 1;
#endif
#ifdef USE_CAR
retval = init_car();
if(retval != CAR_NO_ERROR) {
snprintf(state_data.error_str, sizeof(state_data.error_str), CAR_ERROR_STR(retval));
next_state = ERROR_STATE;
return;
}
car_initialized = 1;
#endif
spawn_device_threads();
first = 0;
}
if (kill_switch_asserted)
next_state = PAUSE_STATE;
else
next_state = NAVIGATION_STATE;
//next_state = TRACK_STATE;
}
int threads_linux_init(void)
{
int status = 0;
int n = 0;
int return_value = THREADS_LINUX_SUCCESS;
// Init data
status = sensors_init(&battery_data, &gps_data, &imu_data, &pitot_data, &pwm_read_data, &pwm_write_data, &scp1000_data, &sonar_data);
if(status != SENSORS_SUCCESS)
{
return_value = THREADS_LINUX_FAILURE;
}
if(return_value != THREADS_LINUX_FAILURE)
{
status = calibration_init(&calibration_local_coordinate_system_data, &calibration_local_fields_data, &calibration_altimeter_data);
if(status != CALIBRATION_SUCCESS)
{
return_value = THREADS_LINUX_FAILURE;
}
}
if(return_value != THREADS_LINUX_FAILURE)
{
status = gps_init(&gps_measure);
if(status != 1)
{
return_value = THREADS_LINUX_FAILURE;
}
}
if(return_value != THREADS_LINUX_FAILURE)
{
status = imu_init(&imu_measure);
if(status != 1)
{
return_value = THREADS_LINUX_FAILURE;
}
}
if(return_value != THREADS_LINUX_FAILURE)
{
status = magnetometer_init(&magnetometer_measure);
if(status != 1)
{
return_value = THREADS_LINUX_FAILURE;
}
}
if(return_value != THREADS_LINUX_FAILURE)
{
status = sonar_init(&sonar_measure);
if(status != 1)
{
return_value = THREADS_LINUX_FAILURE;
}
}
if(return_value != THREADS_LINUX_FAILURE)
{
status = estimation_init(ESTIMATION_DO_NOT_ESTIMATE_ACCEL_BIAS, &estimation_data);
if(status != ESTIMATION_SUCCESS)
{
return_value = THREADS_LINUX_FAILURE;
}
}
if(return_value != THREADS_LINUX_FAILURE)
{
status = control_init(&control_data);
if(status != CONTROL_SUCCESS)
{
return_value = THREADS_LINUX_FAILURE;
}
}
if(return_value != THREADS_LINUX_FAILURE)
{
status = protocol_init();
if(status != PROTOCOL_SUCCESS)
{
return_value = THREADS_LINUX_FAILURE;
}
}
if(return_value != THREADS_LINUX_FAILURE)
{
status = ui_init();
if(status != UI_SUCCESS)
{
return_value = THREADS_LINUX_FAILURE;
}
}
if(return_value != THREADS_LINUX_FAILURE)
{
status = datalogger_init();
if(status != DATALOGGER_SUCCESS)
{
return_value = THREADS_LINUX_FAILURE;
}
}
// Main Loop
if(return_value != THREADS_LINUX_FAILURE)
{
threads_linux_timer_start_task_1();
usleep(0.5*task_1_period_us);
threads_linux_timer_start_task_2();
usleep(5*task_1_period_us);
while(quittask == 0)
{
#if ANU_COMPILE_FOR_OVERO
#else
if(datalogger_status() == DATALOGGER_RUNNING) datalogger_update_IPC();
#endif
if(++n>100)
{
if(datalogger_status() == DATALOGGER_RUNNING) datalogger_write_file();
n = 0;
}
usleep(5*task_1_period_us);
}
threads_linux_timer_stop_task_1();
usleep(TASK1_PERIOD_US);
threads_linux_timer_stop_task_2();
usleep(TASK2_PERIOD_US);
}
status = datalogger_close();
if(status != DATALOGGER_SUCCESS)
{
return_value = THREADS_LINUX_FAILURE;
}
status = ui_close();
if(status != UI_SUCCESS)
{
return_value = THREADS_LINUX_FAILURE;
}
status = protocol_close();
if(status != PROTOCOL_SUCCESS)
{
return_value = THREADS_LINUX_FAILURE;
}
status = control_close();
if(status != CONTROL_SUCCESS)
{
return_value = THREADS_LINUX_FAILURE;
}
status = estimation_close(&estimation_data);
if(status != ESTIMATION_SUCCESS)
{
return_value = THREADS_LINUX_FAILURE;
}
status = magnetometer_close(&magnetometer_measure);
if(status != 1)
{
return_value = THREADS_LINUX_FAILURE;
}
status = imu_close(&imu_measure);
if(status != 1)
{
return_value = THREADS_LINUX_FAILURE;
}
status = gps_close(&gps_measure);
if(status != 1)
{
return_value = THREADS_LINUX_FAILURE;
}
status = calibration_close(&calibration_local_coordinate_system_data, &calibration_local_fields_data, &calibration_altimeter_data);
if(status != CALIBRATION_SUCCESS)
{
return_value = THREADS_LINUX_FAILURE;
}
status = sensors_close();
if(status != SENSORS_SUCCESS)
{
return_value = THREADS_LINUX_FAILURE;
}
return return_value;
}
void rt_init_thread_entry(void* parameter)
{
rt_components_init();
LED_init();
Motor_Init();
rt_kprintf("start device init\n");
//rt_hw_i2c1_init();
i2cInit();
rt_hw_spi2_init();
rt_hw_spi3_init();
rt_event_init(&ahrs_event, "ahrs", RT_IPC_FLAG_FIFO);
dmp_init();
sonar_init();
HMC5983_Init();
adns3080_Init();
//config_bt();
rt_thread_init(&led_thread,
"led",
led_thread_entry,
RT_NULL,
led_stack,
256, 16, 1);
rt_thread_startup(&led_thread);
spi_flash_init();
// bmp085_init("i2c1");
rt_kprintf("device init succeed\n");
if (dfs_mount("flash0", "/", "elm", 0, 0) == 0)
{
rt_kprintf("flash0 mount to /.\n");
}
else
{
rt_kprintf("flash0 mount to / failed.\n");
}
//default settings
PID_Init(&p_rate_pid, 0, 0, 0);
PID_Init(&r_rate_pid, 0, 0, 0);
PID_Init(&y_rate_pid, 0, 0, 0);
PID_Init(&p_angle_pid, 0, 0, 0);
PID_Init(&r_angle_pid, 0, 0, 0);
PID_Init(&y_angle_pid, 0, 0, 0);
PID_Init(&x_v_pid, 0, 0, 0);
PID_Init(&y_v_pid, 0, 0, 0);
PID_Init(&x_d_pid, 0, 0, 0);
PID_Init(&y_d_pid, 0, 0, 0);
PID_Init(&h_pid, 0, 0, 0);
load_settings(&settings, "/setting", &p_angle_pid, &p_rate_pid
, &r_angle_pid, &r_rate_pid
, &y_angle_pid, &y_rate_pid
, &x_d_pid, &x_v_pid
, &y_d_pid, &y_v_pid
, &h_pid);
settings.roll_min = settings.pitch_min = settings.yaw_min = 1000;
settings.th_min = 1000;
settings.roll_max = settings.pitch_max = settings.yaw_max = 2000;
settings.th_max = 2000;
// if(settings.pwm_init_mode)
// {
// Motor_Set(1000,1000,1000,1000);
//
// rt_thread_delay(RT_TICK_PER_SECOND*5);
//
// Motor_Set(0,0,0,0);
//
// settings.pwm_init_mode=0;
// save_settings(&settings,"/setting");
//
// rt_kprintf("pwm init finished!\n");
// }
get_pid();
PID_Set_Filt_Alpha(&p_rate_pid, 1.0 / 166.0, 20.0);
PID_Set_Filt_Alpha(&r_rate_pid, 1.0 / 166.0, 20.0);
PID_Set_Filt_Alpha(&y_rate_pid, 1.0 / 166.0, 20.0);
PID_Set_Filt_Alpha(&p_angle_pid, 1.0 / 166.0, 20.0);
PID_Set_Filt_Alpha(&r_angle_pid, 1.0 / 166.0, 20.0);
PID_Set_Filt_Alpha(&y_angle_pid, 1.0 / 75.0, 20.0);
PID_Set_Filt_Alpha(&x_v_pid, 1.0 / 100.0, 20.0);
PID_Set_Filt_Alpha(&y_v_pid, 1.0 / 100.0, 20.0);
PID_Set_Filt_Alpha(&x_d_pid, 1.0 / 100.0, 20.0);
PID_Set_Filt_Alpha(&y_d_pid, 1.0 / 100.0, 20.0);
PID_Set_Filt_Alpha(&h_pid, 1.0 / 60.0, 20.0);
rt_thread_init(&control_thread,
"control",
control_thread_entry,
RT_NULL,
control_stack,
1024, 3, 5);
rt_thread_startup(&control_thread);
rt_thread_init(&correct_thread,
"correct",
correct_thread_entry,
RT_NULL,
correct_stack,
1024, 12, 1);
rt_thread_startup(&correct_thread);
LED1(5);
}