/**
设置上台参数
**/
void setReady()
{
while(1) {
do {
uart1_put_str("\n\n\r上台参数：");
uart1_put_str("\n\r1.设置速度");
uart1_put_str("\n\r2.设置上台时延");
uart1_put_str("\n\r0.返回");
uart1_put_str("\n\r-> ");
select = uart1_get_char();
uart1_put_char(select);
} while( select != '1' && select != '2' && select != '3' && select != '0');
if(select == '0') break;
switch(select) {
case '1': // 设置左速度
uart1_put_str("\n\n\r设置上台速度：");
do {
uart1_put_str("\n\r速度左(-9999~09999): ");
for(i=0; i<5; i++) {
buf[i] = uart1_get_char();
uart1_put_char(buf[i]);
}
buf[5] = '\0';
READYSPEEDL = atoi(buf);
} while(READYSPEEDL < -9999 || READYSPEEDL > 9999);
do {
uart1_put_str("\n\r速度右(-9999~09999): ");
for(i=0; i<5; i++) {
buf[i] = uart1_get_char();
uart1_put_char(buf[i]);
}
buf[5] = '\0';
READYSPEEDR = atoi(buf);
} while(READYSPEEDR < -9999 || READYSPEEDR > 9999);
uart1_put_str("\n\r设置成功！");
break;
case '2': // 设置上台时延
do {
uart1_put_str("\n\r上台时延(0000~9999): ");
for(i=0; i<4; i++) {
buf[i] = uart1_get_char();
uart1_put_char(buf[i]);
}
buf[4] = '\0';
MSGOUP = atoi(buf);
} while( MSGOUP > 9999 ||MSGOUP < 0 );
uart1_put_str("\n\r设置成功！");
break;
default:
break;
}
}
}
/**
设置推棋子参数
**/
void setPush()
{
while(1) {
do {
uart1_put_str("\n\n\r推棋参数：");
uart1_put_str("\n\r1.设置速度");
uart1_put_str("\n\r2.设置记忆周期");
uart1_put_str("\n\r0.返回");
uart1_put_str("\n\r-> ");
select = uart1_get_char();
uart1_put_char(select);
} while( select != '1' && select != '2' && select != '3' && select != '0');
if(select == '0') break;
switch(select) {
case '1': // 设置推棋子速度
do{
uart1_put_str("\n\n\r设置推棋子速度(0000~9999)：");
for(i=0; i<4; i++) {
buf[i] = uart1_get_char();
uart1_put_char(buf[i]);
}
buf[4] = '\0';
SPEEDPUSH = atoi(buf);
} while(SPEEDPUSH < 0 || SPEEDPUSH > 9999);
uart1_put_str("\n\r设置成功！");
break;
case '2': // 设置推棋子记忆周期
do {
uart1_put_str("\n\r推棋子记忆周期(0000~9999): ");
for(i=0; i<4; i++) {
buf[i] = uart1_get_char();
uart1_put_char(buf[i]);
}
buf[4] = '\0';
MEMORYCYCLE_PUSH = atoi(buf);
} while( MEMORYCYCLE_PUSH > 9999 ||MEMORYCYCLE_PUSH < 0 );
uart1_put_str("\n\r设置成功！");
break;
default:
break;
}
}
}
/**
设置角落权值
**/
void setCornerRation()
{
do {
uart1_put_str("\n\n\r设置角落权值(0~9)：");
buf[0] = uart1_get_char();
uart1_put_char(buf[0]);
buf[1] = '\0';
CORNERRATION = atoi(buf);
} while( CORNERRATION < 0 || CORNERRATION > 9);
uart1_put_str("\n\r设置成功！");
}
/**
设置记忆周期
**/
void setMemoryCycle()
{
do {
uart1_put_str("\n\n\r设置记忆周期(0000~9999ms)：");
for(i=0; i<4; i++) {
buf[i] = uart1_get_char();
uart1_put_char(buf[i]);
}
buf[4] = '\0';
MEMORYCYCLE_INIT = atoi(buf);
} while( MEMORYCYCLE_INIT < 0 || MEMORYCYCLE_INIT > 9999);
uart1_put_str("\n\r设置成功！");
}
/**
设置状态周期
**/
void setStateCycle()
{
do {
uart1_put_str("\n\n\r设置状态周期(000~999ms)：");
for(i=0; i<3; i++) {
buf[i] = uart1_get_char();
uart1_put_char(buf[i]);
}
buf[3] = '\0';
STATECYCLE = atoi(buf);
} while( STATECYCLE < 0 || STATECYCLE > 999);
uart1_put_str("\n\r设置成功！");
}
void calibration_exit(void)
{
// Go back to old state
sys_set_mode(calibration_prev_mode);
sys_set_state(calibration_prev_state);
// Clear debug message buffers
for (int i = 0; i < DEBUG_COUNT; i++)
{
debug_message_send_one();
}
// Clear UART buffers
while (uart0_char_available())
{uart0_get_char();}
while (uart1_char_available())
{uart1_get_char();}
debug_message_buffer("Calibration finished. UART buffers cleared.");
}
/**
* @brief Receive communication packets and handle them
*
* This function decodes packets on the protocol level and also handles
* their value by calling the appropriate functions.
*/
void communication_receive(void)
{
mavlink_message_t msg;
mavlink_status_t status =
{ 0 };
status.packet_rx_drop_count = 0;
// COMMUNICATION WITH ONBOARD COMPUTER
while (uart0_char_available())
{
uint8_t c = uart0_get_char();
if (global_data.state.uart0mode == UART_MODE_MAVLINK)
{
// Try to get a new message
if (mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status))
{
// Handle message
handle_mavlink_message(MAVLINK_COMM_0, &msg);
}
}
else if (global_data.state.uart0mode == UART_MODE_BYTE_FORWARD)
{
uart1_transmit(c);
}
// And get the next one
}
// Update global packet drops counter
global_data.comm.uart0_rx_drop_count += status.packet_rx_drop_count;
global_data.comm.uart0_rx_success_count += status.packet_rx_success_count;
status.packet_rx_drop_count = 0;
// COMMUNICATION WITH EXTERNAL COMPUTER
while (uart1_char_available())
{
uint8_t c = uart1_get_char();
// Check if this link is used for MAVLink or GPS
if (global_data.state.uart1mode == UART_MODE_MAVLINK)
{
//uart0_transmit((unsigned char)c);
// Try to get a new message
if (mavlink_parse_char(MAVLINK_COMM_1, c, &msg, &status))
{
// Handle message
handle_mavlink_message(MAVLINK_COMM_1, &msg);
}
}
else if (global_data.state.uart1mode == UART_MODE_GPS)
{
if (global_data.state.gps_mode == 10)
{
static uint8_t gps_i = 0;
static char gps_chars[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN];
if (c == '$' || gps_i == MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN
- 1)
{
gps_i = 0;
char gps_chars_buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN];
strncpy(gps_chars_buf, gps_chars,
MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN);
debug_message_buffer(gps_chars_buf);
}
gps_chars[gps_i++] = c;
}
if (gps_parse(c))
{
// New GPS data received
//debug_message_buffer("RECEIVED NEW GPS DATA");
parse_gps_msg();
if (gps_lat == 0)
{
global_data.state.gps_ok = 0;
//debug_message_buffer("GPS Signal Lost");
}
else
{
global_data.state.gps_ok = 1;
mavlink_msg_gps_raw_send(
global_data.param[PARAM_SEND_DEBUGCHAN],
sys_time_clock_get_unix_time(), gps_mode, gps_lat
/ 1e7f, gps_lon / 1e7f, gps_alt / 100.0f,
0.0f, 0.0f, gps_gspeed / 100.0f, gps_course / 10.0f);
}
// // Output satellite info
// for (int i = 0; i < gps_nb_channels; i++)
// {
// mavlink_msg_gps_status_send(global_data.param[PARAM_SEND_DEBUGCHAN], gps_numSV, gps_svinfos[i].svid, gps_satellite_used(gps_svinfos[i].qi), gps_svinfos[i].elev, ((gps_svinfos[i].azim/360.0f)*255.0f), gps_svinfos[i].cno);
// }
}
}
else if (global_data.state.uart1mode == UART_MODE_BYTE_FORWARD)
{
uart0_transmit(c);
led_toggle(LED_YELLOW);
}
// And get the next one
}
// Update global packet drops counter
global_data.comm.uart0_rx_drop_count += status.packet_rx_drop_count;
global_data.comm.uart0_rx_success_count += status.packet_rx_success_count;
status.packet_rx_drop_count = 0;
}
/**
设置电机
**/
void setMotor()
{
while(1) {
do {
uart1_put_str("\n\n\r电机选项：");
uart1_put_str("\n\r1.设置电机ID");
uart1_put_str("\n\r2.设置最大速度");
uart1_put_str("\n\r3.设置转弯速度");
uart1_put_str("\n\r4.设置安全参数");
uart1_put_str("\n\r5.设置攻击参数");
uart1_put_str("\n\r0.返回");
uart1_put_str("\n\r-> ");
select = uart1_get_char();
uart1_put_char(select);
} while( select != '1' && select != '2' && select != '3' && select != '4'
&& select != '5' && select != '0');
if( select == '0') break;
switch(select) {
case '1': // 设置电机ID
uart1_put_str("\n\n\r设置电机ID：");
do {
uart1_put_str("\n\r电机左(00~99): ");
for(i=0; i<2; i++) {
buf[i] = uart1_get_char();
uart1_put_char(buf[i]);
}
buf[2] = '\0';
NODE_L = atoi(buf);
} while( NODE_L < 0 || NODE_L > 99);
do {
uart1_put_str("\n\r电机右(00~99): ");
for(i=0; i<2; i++) {
buf[i] = uart1_get_char();
uart1_put_char(buf[i]);
}
buf[2] = '\0';
NODE_R = atoi(buf);
} while(NODE_R < 0 || NODE_R > 99);
uart1_put_str("\n\r设置成功！");
break;
case '2': // 设置最大速度
uart1_put_str("\n\n\r设置最大速度(00000 ~ 20000)：");
do {
for(i=0; i<5; i++) {
buf[i] = uart1_get_char();
uart1_put_char(buf[i]);
}
buf[5] = '\0';
TOPSPEED = atoi(buf);
} while(TOPSPEED < 0 || TOPSPEED > 20000);
uart1_put_str("\n\r设置成功！");
break;
case '3': // 设置转弯速度
uart1_put_str("\n\n\r设置转弯速度(00000 ~ 20000)：");
do {
for(i=0; i<5; i++) {
buf[i] = uart1_get_char();
uart1_put_char(buf[i]);
}
buf[5] = '\0';
TURNSPEED = atoi(buf);
} while(TURNSPEED < 0 || TURNSPEED > 20000);
uart1_put_str("\n\r设置成功！");
break;
case '4': // 设置安全参数
uart1_put_str("\n\n\r设置安全参数：");
uart1_put_str("\n\r安全速度(00000~20000)：");
do {
for(i=0; i<5; i++) {
buf[i] = uart1_get_char();
uart1_put_char(buf[i]);
}
buf[5] = '\0';
SAFESPEED = atoi(buf);
} while(SAFESPEED < 0 || SAFESPEED > 20000);
uart1_put_str("\n\r安全加、减速度(00000~10000)：");
do {
for(i=0; i<5; i++) {
buf[i] = uart1_get_char();
uart1_put_char(buf[i]);
}
buf[5] = '\0';
SAFERATION = atoi(buf);
} while(SAFERATION < 0 || SAFERATION > 10000);
uart1_put_str("\n\r设置成功！");
break;
case '5': // 设置攻击参数
uart1_put_str("\n\n\r设置攻击参数：");
uart1_put_str("\n\r一档速度(00000~20000)：");
do {
for(i=0; i<5; i++) {
buf[i] = uart1_get_char();
uart1_put_char(buf[i]);
}
buf[5] = '\0';
ATTACKGEAR1 = atoi(buf);
} while(ATTACKGEAR1 < 0 || ATTACKGEAR1 > 20000);
uart1_put_str("\n\r二档速度(00000~20000)：");
do {
for(i=0; i<5; i++) {
buf[i] = uart1_get_char();
uart1_put_char(buf[i]);
}
buf[5] = '\0';
ATTACKGEAR2 = atoi(buf);
} while(ATTACKGEAR2 < 0 || ATTACKGEAR2 > 20000);
uart1_put_str("\n\r三档速度(00000~20000)：");
do {
for(i=0; i<5; i++) {
buf[i] = uart1_get_char();
uart1_put_char(buf[i]);
}
buf[5] = '\0';
ATTACKGEAR3 = atoi(buf);
} while(ATTACKGEAR3 < 0 || ATTACKGEAR3 > 20000);
uart1_put_str("\n\r四档速度(00000~20000)：");
do {
for(i=0; i<5; i++) {
buf[i] = uart1_get_char();
uart1_put_char(buf[i]);
}
buf[5] = '\0';
ATTACKGEAR4 = atoi(buf);
} while(ATTACKGEAR4 < 0 || ATTACKGEAR4 > 20000);
uart1_put_str("\n\r攻击加、减速度(00000~50000)：");
do {
for(i=0; i<5; i++) {
buf[i] = uart1_get_char();
uart1_put_char(buf[i]);
}
buf[5] = '\0';
ATTACKRATION = atoi(buf);
} while(ATTACKRATION < 0 || ATTACKRATION > 50000);
uart1_put_str("\n\r设置成功！");
break;
}
}
}
void menu()
{
myuart_init();
_delay_ms(1000);
uart1_put_str("\n\n\r设置模式启动，正在读取信息...");
//从EEPROM中读取参数
eeprom_rOw_par(EEP_R);
uart1_put_str("\n\r读取成功！");
while(1) {
buf[0] = 'n'; buf[1] = 'n'; buf[2] = 'n'; buf[3] = 'n'; buf[4] = 'n';
do {
uart1_put_str("\n\n\r您要进行哪项操作?");
uart1_put_str("\n\r1.打印当前信息");
uart1_put_str("\t2.设置推棋参数");
uart1_put_str("\t3.进行灰度采样");
uart1_put_str("\t4.设置角落权值");
uart1_put_str("\n\r5.设置状态周期");
uart1_put_str("\t6.设置记忆周期");
uart1_put_str("\t7.设置上台参数");
uart1_put_str("\t8.设置电机参数");
uart1_put_str("\n\rl.重读");
uart1_put_str("\tw.保存");
uart1_put_str("\n\r-> ");
select = uart1_get_char();
uart1_put_char(select);
} while( select != '1' && select != '2' && select != '3' && select != '4' && select != '5'
&& select != '6' && select != '7' && select != '8' && select != 'l' && select != 'w'
);
switch(select) {
case '1': // 打印当前信息
printMessage();
break;
case '2': // 设置推棋子参数
setPush();
break;
case '3': // 灰度采样
refresh_stage_AD();
break;
case '4': // 设置角落权值
setCornerRation();
break;
case '5': // 设置状态周期
setStateCycle();
break;
case '6': // 设置记忆周期
setMemoryCycle();
break;
case '7': // 设置上台参数
setReady();
break;
case '8': // 设置电机参数
setMotor();
break;
case 'w': // 保存
uart1_put_str("\n\n\r正在保存...");
eeprom_rOw_par(EEP_W);
uart1_put_str("\n\r保存成功!");
break;
case 'l': // 重新读取
uart1_put_str("\n\n\r正在重新读取信息...");
eeprom_rOw_par(EEP_R);
uart1_put_str("\n\r读取成功！");
break;
}
}
}