/*-----------------------------------------------------------------------*/
SWORD read_rev_data(void)
{
u8_t status;
if (MEMS_SUCCESS == GetSatusReg(&status))
{
if (status & 80)
{
GetAngRateRaw(&rev);
rev.x/=1000;
rev.y/=500;
rev.z/=500;
rad.x+=rev.x;
rad.y+=rev.y;
rad.z+=rev.z;
#if 0
LCD_PrintoutInt(32, 0, rev.x);
LCD_PrintoutInt(32, 2, rev.y);
LCD_PrintoutInt(32, 4, rev.z);
#endif
return 0;
}
return 1;
}
return 2;
}
/*----------------------------------------------------------------------*/
void read_device_no()
{
LCD_P8x16Str(0, 4, (BYTE*)"DeviceNo=");
if (!read_device_no_from_TF())
{
if (g_device_NO!=0)
{
LCD_PrintoutInt(72, 4, g_device_NO);
}
else
{
suicide();
}
}
else
{
suicide();
}
}
/*-----------------------------------------------------------------------*/
void BalanceControl(void)//暂时用data_speed_pid
{
/*
//用电位器调节转速，试验用
int potential=1;
int sss=1;
potential=(int)ADC.CDR[33].B.CDATA;//PB9
if(potential<0)
{
potential=0-potential;
}
if(potential>SPEED_PWM_MAX)
{
potential=SPEED_PWM_MAX;
}
if(potential<200)
sss=200;
else
sss=500;
set_motor_pwm(100);
*/
float delta_angle_balance;
float delta_anglespeed_balance;
float temp_angle_balance, temp_anglespeed_balance;
static float currentanglespeed_balance, lastanglespeed_balance=0;
float last_angle_balance=0;
float temp_p,temp_d;
angle_calculate();
g_fCarAngle_balance= AngleCalculate[2];
g_fGyroscopeAngleSpeed_balance=-AngleCalculate[3];
temp_angle_balance=CarAngleInitial_balance - g_fCarAngle_balance;
temp_anglespeed_balance= CarAnglespeedInitial_balance - g_fGyroscopeAngleSpeed_balance;
if(g_fCarAngle_balance>7||g_fCarAngle_balance<-7)
{
temp_d=0;
temp_p=90;
}
else
{
temp_p=data_ROLL_angle_pid.p;
temp_d=data_ROLL_angle_pid.d;
}
// currentanglespeed_balance=g_fCarAngle_balance;
// delta_anglespeed_balance=currentanglespeed_balance-lastanglespeed_balance;
// lastanglespeed_balance=currentanglespeed_balance;
delta_angle_balance = temp_p*(CarAngleInitial_balance - g_fCarAngle_balance);
delta_angle_balance+=temp_d*(CarAnglespeedInitial_balance - g_fGyroscopeAngleSpeed_balance);
// delta_angle_balance+=data_speed_pid.d*0.4*delta_anglespeed_balance;
//angle_pwm_balance=dta_angle;
ROLL_angle_pwm=delta_angle_balance;
LCD_PrintoutInt(0, 6, ROLL_angle_pwm);
}
/*-----------------------------------------------------------------------*/
void init_all_and_POST(void)
{
int i = 0;
/* TF卡 */
TCHAR *path = "0:";
disable_watchdog();
init_modes_and_clock();
initEMIOS_0MotorAndSteer();
initEMIOS_0Image();/* 摄像头输入中断初始化 */
init_pit();
init_led();
init_DIP();
init_serial_port_0();
init_serial_port_1();
init_serial_port_2();
//init_ADC();
//init_serial_port_3();
init_supersonic_receive_0();
init_supersonic_receive_1();
// init_supersonic_receive_2();
// init_supersonic_receive_3();
init_supersonic_trigger_0();
init_supersonic_trigger_1();
// init_supersonic_trigger_2();
// init_supersonic_trigger_3();
// init_optical_encoder();
//init_DSPI_2();
//init_I2C();
init_choose_mode();
/* 初始化SPI总线 */
init_DSPI_1();
/* 开启外部总中断 */
enable_irq();
/* 初始化显示屏 */
initLCD();
//LCD_DISPLAY();
LCD_Fill(0xFF); /* 亮屏 */
delay_ms(50);
LCD_Fill(0x00); /* 黑屏 */
delay_ms(50);
#if 1
/* 初始化TF卡 */
LCD_P8x16Str(0,0, (BYTE*)"TF..");
if (!SD_init())
{
/* 挂载TF卡文件系统 */
if (FR_OK == f_mount(&fatfs1, path, 1))
{
/* 文件读写测试 */
if (!test_file_system())
{
g_devices_init_status.TFCard_is_OK = 1;
}
}
}
if (g_devices_init_status.TFCard_is_OK)
{
LCD_P8x16Str(0,0, (BYTE*)"TF..OK");
}
else
{
LCD_P8x16Str(0,0, (BYTE*)"TF..NOK");
suicide();
}
/* 读取设备号 */
LCD_P8x16Str(0, 4, (BYTE*)"DeviceNo=");
if (!read_device_no_from_TF())
{
if (WIFI_ADDRESS_WITHOUT_INIT != g_device_NO)
{
LCD_PrintoutInt(72, 4, g_device_NO);
}
else
{
suicide();
}
}
else
{
suicide();
}
/* 开启RFID读卡器主动模式 */
if (!init_RFID_modul_type())
{
g_devices_init_status.RFIDCard_energetic_mode_enable_is_OK = 1;
LCD_P8x16Str(0, 6, (BYTE*)"RFID..OK");
}
else
{
g_devices_init_status.RFIDCard_energetic_mode_enable_is_OK = 0;
LCD_P8x16Str(0, 6, (BYTE*)"RFID..NOK");
suicide();
}
delay_ms(1000);
/* 换屏 */
LCD_Fill(0x00);
/* 读取舵机参数 */
LCD_P8x16Str(0, 0, (BYTE*)"StH.L=");
if (read_steer_helm_data_from_TF())
{
suicide();
}
update_steer_helm_basement_to_steer_helm();
LCD_PrintoutInt(48, 0, data_steer_helm_basement.left_limit);
set_steer_helm_basement(data_steer_helm_basement.left_limit);
delay_ms(500);
LCD_P8x16Str(0, 2, (BYTE*)"StH.R=");
LCD_PrintoutInt(48, 2, data_steer_helm_basement.right_limit);
set_steer_helm_basement(data_steer_helm_basement.right_limit);
delay_ms(500);
LCD_P8x16Str(0, 4, (BYTE*)"StH.C=");
LCD_PrintoutInt(48, 4, data_steer_helm_basement.center);
set_steer_helm_basement(data_steer_helm_basement.center);
/* 读取mode号 */
LCD_P8x16Str(0, 6, (BYTE*)"MODE=");
LCD_PrintoutInt(40, 6, mode);
//set_pos_target();
delay_ms(1000);
/* 换屏 */
LCD_Fill(0x00);
/* 速度闭环测试 */
g_f_enable_speed_control = 1;
LCD_P8x16Str(0, 4, (BYTE*)"S.T=0");
set_speed_target(0);
delay_ms(2000);
/* 换屏 */
LCD_Fill(0x00);
#endif
}
/*----------------------------------------------------------------------*/
void read_DIP_mode()
{
LCD_P8x16Str(0, 6, (BYTE*)"MODE=");
LCD_PrintoutInt(40, 6, mode);
}
