/**
* Set an Actuator value
* @param a - The Actuator
* @param value - The value to set
*/
void Actuator_SetValue(Actuator * a, double value, bool record)
{
if (a->sanity != NULL && !a->sanity(a->user_id, value))
{
//ARE YOU INSANE?
Log(LOGERR,"Insane value %lf for actuator %s", value, a->name);
return;
}
if (!(a->set(a->user_id, value)))
{
Fatal("Failed to set actuator %s to %lf", a->name, value);
}
// Set time stamp
struct timespec t;
clock_gettime(CLOCK_MONOTONIC, &t);
DataPoint d = {TIMEVAL_DIFF(t, *Control_GetStartTime()), a->last_setting.value};
// Record value change
if (record)
{
d.time_stamp -= 1e-6;
Data_Save(&(a->data_file), &d, 1);
d.value = value;
d.time_stamp += 1e-6;
Data_Save(&(a->data_file), &d, 1);
}
a->last_setting = d;
}
// void Gyro_NonLinear(void)
// {
// Gyro_Radian_Total
// Gyro_Multiplier=PPoly_Eval(fabs(1000*Gyro_Radian_Step/time_interval),GREENGYRO);
// delay(1);
// }
void Gyro_CheckFloat(void)
{
LED2_on;
Gyro_Clear();
Gyro_Float_Flag=1;
Gyro_Float_Flag1=1;
Gyro_Float_Flag2=1;
while(Gyro_Float_Flag)
delay(1);
if(Gyro_Float_Total2-Gyro_Float_Total1==0)
Float_State=CF_ERROR;
else if(Int64Abs(Gyro_Float_Total2-Gyro_Float_Total1-Gyro_Float*1024)>40960)
{
Gyro_Float=(Gyro_Float_Total2-Gyro_Float_Total1)/1024;
Float_State=CF_CHANGED;
Data_Save();
}
else
{
Float_State=CF_NORMAL;
}
Gyro_Clear();
LED2_off;
}
void GYRO_INIT_STATE(void)
{
/*
s_gi 陀螺仪初始化状态时的状态标志
0--初始化还未开始
1--校准正传系数
2--校准反正系数
3--告知主控陀螺仪标定完成
*/
static uint8_t s_gi = 0;
switch(s_gi)
{
case 0 : s_gi = 1;
Gyro_Clear();
break;
case 1 : s_gi = 2;
Gyro_Init_P();
Gyro_Clear();
S_GYRO_C1_FUNC();
break;
case 2 : s_gi = 0;
Gyro_Init_N();
S_GYRO_C2_FUNC();
Data_Save();
SS_H = 0;//复位状态标志
break;
default : s_gi = 1;
Gyro_Clear();
}
}
void ENC_R_INIT_STATE(void)
{
/*
s_er 码盘旋转半径初始化状态时的状态标志
0--初始化还未开始
1--校准正传系数
2--校准反正系数
3--告知主控陀螺仪标定完成
*/
static uint8_t s_er = 0;
switch(s_er)
{
case 0 : s_er = 1;
Encoder_Clear(0);
Encoder_Clear(1);
Gyro_Clear();
break;
case 1 : s_er = 0;
Encoder_InitR();
Data_Save();
SS_H = 0; //复位状态标志
break;
default : s_er = 1;
Encoder_Clear(0);
Encoder_Clear(1);
}
}
void load_defaults (void)
{
while (1)
{
LCD_Clear();
LCD_SetXY(0,0);
LCD_WriteString("------NOTICE------");
LCD_SetXY(0,1);
LCD_WriteString("Are you sure to load defaults?Press OK to continue,or back to cancel");
refresh();
delay(LCD_DELAY);
if(KeyReady==1)
{
KeyReady=0;
switch(KeyValue)
{
case 18://ok
flash_save[0].u16_data[0]=800;//AGV_speed
flash_save[0].u16_data[1]=800;//delta_x
flash_save[0].u16_data[2]=800;//delta_y
flash_save[0].u16_data[3]=200;//Stop_time
Data_Save();
init_parameter();
return;
case keyback:
return;
}
}
}
}
void parameter_setting (void)
{
while(1)
{
LCD_Clear();
LCD_SetXY(0,0);
LCD_WriteString("PARAMETER SETTING");
LCD_SetXY(0,1);
LCD_WriteString("1.SPEED:");
LCD_WriteDouble(AGV_speed,1);
LCD_SetXY(0,2);
LCD_WriteString("2.DELTA:");
LCD_WriteDouble(delta_x,0);
LCD_WriteString("/");
LCD_WriteDouble(delta_y,0);
LCD_SetXY(0,3);
LCD_WriteString("3.STOPTIME:");
LCD_WriteInt(Stop_time);
refresh();
if (KeyReady==1)
{
KeyReady=0;
switch (KeyValue)
{
case 1:
Input_FloatValue(&AGV_speed,"SPEED");
flash_save[0].u16_data[0]=AGV_speed;
break;
case 2:
Input_FloatValue(&delta_x,"DELTA_X");
flash_save[0].u16_data[1]=delta_x;
Input_FloatValue(&delta_y,"DELTA_Y");
flash_save[0].u16_data[2]=delta_y;
break;
case 3:
Input_IntValue(&Stop_time,"STOPTIME");
flash_save[0].u16_data[3]=Stop_time;
break;
case 5:
return;
}
Data_Save();
init_parameter();
}
delay(LCD_DELAY);
}
}
/**
* Record data from a single Sensor; to be run in a seperate thread
* @param arg - Cast to Sensor* - Sensor that the thread will handle
* @returns NULL (void* required to use the function with pthreads)
*/
void * Sensor_Loop(void * arg)
{
Sensor * s = (Sensor*)(arg);
Log(LOGDEBUG, "Sensor %d starts", s->id);
// Until the sensor is stopped, record data points
while (s->activated)
{
bool success = s->read(s->user_id, &(s->current_data.value));
struct timespec t;
clock_gettime(CLOCK_MONOTONIC, &t);
s->current_data.time_stamp = TIMEVAL_DIFF(t, *Control_GetStartTime());
if (success)
{
if (s->sanity != NULL)
{
if (!s->sanity(s->user_id, s->current_data.value))
{
Fatal("Sensor %s (%d,%d) reads unsafe value", s->name, s->id, s->user_id);
}
}
s->averaged_data.time_stamp += s->current_data.time_stamp;
s->averaged_data.value = s->current_data.value;
if (++(s->num_read) >= s->averages)
{
s->averaged_data.time_stamp /= s->averages;
s->averaged_data.value /= s->averages;
Data_Save(&(s->data_file), &(s->averaged_data), 1); // Record it
s->num_read = 0;
s->averaged_data.time_stamp = 0;
s->averaged_data.value = 0;
}
}
else
{
// Silence because strain sensors fail ~50% of the time :S
//Log(LOGWARN, "Failed to read sensor %s (%d,%d)", s->name, s->id,s->user_id);
}
clock_nanosleep(CLOCK_MONOTONIC, 0, &(s->sample_time), NULL);
}
// Needed to keep pthreads happy
Log(LOGDEBUG, "Sensor %s (%d,%d) finished", s->name,s->id,s->user_id);
return NULL;
}
void W_ENC_COEFF_FUNC(void)
{
uint8_t EncID;
uint8_t CoeffID;
EncID = MOSI_ENC_COEFF.u8_data[0]/4;
CoeffID = MOSI_ENC_COEFF.u8_data[0]-4*EncID;
if(CoeffID == 0)
Encoders[EncID].Convert1 = MOSI_ENC_COEFF.f64_data;
if(CoeffID == 1)
Encoders[EncID].Convert2 = MOSI_ENC_COEFF.f64_data;
if(CoeffID == 2)
Encoders[EncID].Radius = MOSI_ENC_COEFF.f64_data;
if(CoeffID == 3)
Encoders[EncID].radian = MOSI_ENC_COEFF.f64_data;
Data_Save();
}
void ENC_L_INIT_STATE(void)
{
/*
s_el 码盘正反转初始化状态时的状态标志
0--初始化还未开始
1--校准正传系数
2--校准反正系数
3--告知主控陀螺仪标定完成
*/
static uint8_t s_el = 0;
switch(s_el)
{
case 0:
s_el = 1;
Encoder_Clear(0);
Encoder_Clear(1);
break;
case 1:
s_el = 2;
Encoder_InitXY(0);
break;
case 2:
s_el = 3;
Encoder_Clear(0);
Encoder_Clear(1);
break;
case 3:
s_el = 0;
Encoder_InitXY(1);
Encoder_Init();
Data_Save();
SS_H = 0; //复位状态标志
break;
default : s_el = 1;
Encoder_Clear(0);
Encoder_Clear(1);
break;
}
}
void Data_Receive_Anl(u8 *data_buf, u8 num)
{
vs16 rc_value_temp;
u8 sum = 0;
//Sys_sPrintf(Printf_USART, data_buf, num);
for (u8 i = 0; i < (num - 1); i++)
sum += *(data_buf + i);
if (!(sum == *(data_buf + num - 1))) return; //sum
if (!(*(data_buf) == 0xAA && *(data_buf + 1) == 0xAF)) return; //
/////////////////////////////////////////////////////////////////////////////////////
if (*(data_buf + 2) == 0X01)
{
if (*(data_buf + 4) == 0X01)
MPU6050_CalOff_Acc();
if (*(data_buf + 4) == 0X02)
MPU6050_CalOff_Gyr();
if (*(data_buf + 4) == 0X03)
{
MPU6050_CalOff_Acc();
MPU6050_CalOff_Gyr();
}
if (*(data_buf + 4) == 0X04)
;//Cal_Compass();
if (*(data_buf + 4) == 0X05)
;//MS5611_CalOffset();
}
if (*(data_buf + 2) == 0X02)
{
if (*(data_buf + 4) == 0X01)
{
Send.PID1 = 1;
Send.PID2 = 1;
Send.PID3 = 1;
Send.PID4 = 1;
Send.PID5 = 1;
Send.PID6 = 1;
}
if (*(data_buf + 4) == 0X02)
Send.Offset = 1;
}
if (*(data_buf + 2) == 0X10) //PID1
{
PID_ROL.P = (float)((vs16)(*(data_buf + 4 ) << 8) | *(data_buf + 5 )) / PID_ROL_P_MULTIPLYING;
PID_ROL.I = (float)((vs16)(*(data_buf + 6 ) << 8) | *(data_buf + 7 )) / PID_ROL_I_MULTIPLYING;
PID_ROL.D = (float)((vs16)(*(data_buf + 8 ) << 8) | *(data_buf + 9 )) / PID_ROL_D_MULTIPLYING;
PID_PIT.P = (float)((vs16)(*(data_buf + 10) << 8) | *(data_buf + 11)) / PID_PIT_P_MULTIPLYING;
PID_PIT.I = (float)((vs16)(*(data_buf + 12) << 8) | *(data_buf + 13)) / PID_PIT_I_MULTIPLYING;
PID_PIT.D = (float)((vs16)(*(data_buf + 14) << 8) | *(data_buf + 15)) / PID_PIT_D_MULTIPLYING;
PID_YAW.P = (float)((vs16)(*(data_buf + 16) << 8) | *(data_buf + 17)) / PID_YAW_P_MULTIPLYING;
PID_YAW.I = (float)((vs16)(*(data_buf + 18) << 8) | *(data_buf + 19)) / PID_YAW_I_MULTIPLYING;
PID_YAW.D = (float)((vs16)(*(data_buf + 20) << 8) | *(data_buf + 21)) / PID_YAW_D_MULTIPLYING;
Data_Send_Check(sum);//
Data_Save(1);
Send.PID1 = 1;
}
if (*(data_buf + 2) == 0X11) //PID2
{
PID_ALT.P = (float)((vs16)(*(data_buf + 4 ) << 8) | *(data_buf + 5 )) / PID_ALT_P_MULTIPLYING ;
PID_ALT.I = (float)((vs16)(*(data_buf + 6 ) << 8) | *(data_buf + 7 )) / PID_ALT_I_MULTIPLYING ;
PID_ALT.D = (float)((vs16)(*(data_buf + 8 ) << 8) | *(data_buf + 9 )) / PID_ALT_D_MULTIPLYING ;
PID_POS.P = (float)((vs16)(*(data_buf + 10) << 8) | *(data_buf + 11)) / PID_POS_P_MULTIPLYING ;
PID_POS.I = (float)((vs16)(*(data_buf + 12) << 8) | *(data_buf + 13)) / PID_POS_I_MULTIPLYING ;
PID_POS.D = (float)((vs16)(*(data_buf + 14) << 8) | *(data_buf + 15)) / PID_POS_D_MULTIPLYING ;
PID_PID_1.P = (float)((vs16)(*(data_buf + 16) << 8) | *(data_buf + 17)) / PID_PID_1_P_MULTIPLYING;
PID_PID_1.I = (float)((vs16)(*(data_buf + 18) << 8) | *(data_buf + 19)) / PID_PID_1_I_MULTIPLYING;
PID_PID_1.D = (float)((vs16)(*(data_buf + 20) << 8) | *(data_buf + 21)) / PID_PID_1_D_MULTIPLYING;
Send.PID2 = 1;
Data_Send_Check(sum);
}
if (*(data_buf + 2) == 0X12) //PID3
{
PID_PID_2.P = (float)((vs16)(*(data_buf + 4 ) << 8) | *(data_buf + 5 )) / PID_PID_2_P_MULTIPLYING;
PID_PID_2.I = (float)((vs16)(*(data_buf + 6 ) << 8) | *(data_buf + 7 )) / PID_PID_2_I_MULTIPLYING;
PID_PID_2.D = (float)((vs16)(*(data_buf + 8 ) << 8) | *(data_buf + 9 )) / PID_PID_2_D_MULTIPLYING;
PID_PID_3.P = (float)((vs16)(*(data_buf + 10) << 8) | *(data_buf + 11)) / PID_PID_3_P_MULTIPLYING;
PID_PID_3.I = (float)((vs16)(*(data_buf + 12) << 8) | *(data_buf + 13)) / PID_PID_3_I_MULTIPLYING;
PID_PID_3.D = (float)((vs16)(*(data_buf + 14) << 8) | *(data_buf + 15)) / PID_PID_3_D_MULTIPLYING;
PID_PID_4.P = (float)((vs16)(*(data_buf + 16) << 8) | *(data_buf + 17)) / PID_PID_4_P_MULTIPLYING;
PID_PID_4.I = (float)((vs16)(*(data_buf + 18) << 8) | *(data_buf + 19)) / PID_PID_4_I_MULTIPLYING;
PID_PID_4.D = (float)((vs16)(*(data_buf + 20) << 8) | *(data_buf + 21)) / PID_PID_4_D_MULTIPLYING;
Send.PID3 = 1;
Data_Send_Check(sum);
Data_Save(1);
}
if (*(data_buf + 2) == 0X13) //PID4
{
PID_PID_5.P = (float)((vs16)(*(data_buf + 4 ) << 8) | *(data_buf + 5 )) / PID_PID_5_P_MULTIPLYING;
PID_PID_5.I = (float)((vs16)(*(data_buf + 6 ) << 8) | *(data_buf + 7 )) / PID_PID_5_I_MULTIPLYING;
PID_PID_5.D = (float)((vs16)(*(data_buf + 8 ) << 8) | *(data_buf + 9 )) / PID_PID_5_D_MULTIPLYING;
PID_PID_6.P = (float)((vs16)(*(data_buf + 10) << 8) | *(data_buf + 11)) / PID_PID_6_P_MULTIPLYING;
PID_PID_6.I = (float)((vs16)(*(data_buf + 12) << 8) | *(data_buf + 13)) / PID_PID_6_I_MULTIPLYING;
PID_PID_6.D = (float)((vs16)(*(data_buf + 14) << 8) | *(data_buf + 15)) / PID_PID_6_D_MULTIPLYING;
PID_PID_7.P = (float)((vs16)(*(data_buf + 16) << 8) | *(data_buf + 17)) / PID_PID_7_P_MULTIPLYING;
PID_PID_7.I = (float)((vs16)(*(data_buf + 18) << 8) | *(data_buf + 19)) / PID_PID_7_I_MULTIPLYING;
PID_PID_7.D = (float)((vs16)(*(data_buf + 20) << 8) | *(data_buf + 21)) / PID_PID_7_D_MULTIPLYING;
Data_Send_Check(sum);
}
if (*(data_buf + 2) == 0X14) //PID5
{
PID_PID_8.P = (float)((vs16)(*(data_buf + 4) << 8 ) | *(data_buf + 5 )) / PID_PID_8_P_MULTIPLYING ;
PID_PID_8.I = (float)((vs16)(*(data_buf + 6) << 8 ) | *(data_buf + 7 )) / PID_PID_8_I_MULTIPLYING ;
PID_PID_8.D = (float)((vs16)(*(data_buf + 8) << 8 ) | *(data_buf + 9 )) / PID_PID_8_D_MULTIPLYING ;
PID_PID_9.P = (float)((vs16)(*(data_buf + 10) << 8) | *(data_buf + 11)) / PID_PID_9_P_MULTIPLYING ;
PID_PID_9.I = (float)((vs16)(*(data_buf + 12) << 8) | *(data_buf + 13)) / PID_PID_9_I_MULTIPLYING ;
PID_PID_9.D = (float)((vs16)(*(data_buf + 14) << 8) | *(data_buf + 15)) / PID_PID_9_D_MULTIPLYING ;
PID_PID_10.P = (float)((vs16)(*(data_buf + 16) << 8) | *(data_buf + 17)) / PID_PID_10_P_MULTIPLYING;
PID_PID_10.I = (float)((vs16)(*(data_buf + 18) << 8) | *(data_buf + 19)) / PID_PID_10_I_MULTIPLYING;
PID_PID_10.D = (float)((vs16)(*(data_buf + 20) << 8) | *(data_buf + 21)) / PID_PID_10_D_MULTIPLYING;
Data_Send_Check(sum);
}
if (*(data_buf + 2) == 0X15) //PID6
{
PID_PID_11.P = (float)((vs16)(*(data_buf + 4) << 8) | *(data_buf + 5)) / PID_PID_11_P_MULTIPLYING;
PID_PID_11.I = (float)((vs16)(*(data_buf + 6) << 8) | *(data_buf + 7)) / PID_PID_11_I_MULTIPLYING;
PID_PID_11.D = (float)((vs16)(*(data_buf + 8) << 8) | *(data_buf + 9)) / PID_PID_11_D_MULTIPLYING;
PID_PID_12.P = (float)((vs16)(*(data_buf + 10) << 8) | *(data_buf + 11)) / PID_PID_12_P_MULTIPLYING;
PID_PID_12.I = (float)((vs16)(*(data_buf + 12) << 8) | *(data_buf + 13)) / PID_PID_12_I_MULTIPLYING;
PID_PID_12.D = (float)((vs16)(*(data_buf + 14) << 8) | *(data_buf + 15)) / PID_PID_12_D_MULTIPLYING;
Data_Send_Check(sum);
}
if (*(data_buf + 2) == 0X16) //OFFSET
{
AngleOffset_Rol = (float)((vs16)(*(data_buf + 4) << 8) | *(data_buf + 5)) / 1000;
AngleOffset_Pit = (float)((vs16)(*(data_buf + 6) << 8) | *(data_buf + 7)) / 1000;
// Data_Save();
}
/////////////////////////////////////////////////////////////////////////////////////////////////
if (*(data_buf + 2) == 0x03) //
{
}
Ex_Anl();
}
void W_GYRO_C2_FUNC(void)
{
Gyro_Convert2=MOSI_GYRO_C2.f64_data;
Data_Save();
}
void W_GYRO_C1_FUNC(void)
{
Gyro_Convert1=MOSI_GYRO_C1.f64_data;
Data_Save();
}
