void Time_Pause(int pausa) {
long long tend, t, diff;
t = Time_GetTime();
tend = t + pausa;
do {
diff = tend - t;
if (diff > 1000) {
Sleep(diff / 1000);
} else {
Sleep(0);
}
t = Time_GetTime();
} while (tend >= t);
}
void Draw_LoopIterationAux() {
Draw_LoopIteration();
// Update time
_procTime2 = Time_GetTime();
_accTime += _procTime2 - _procTime1;
_procTime1 = _procTime2;
}
/////////////////////////////
// Draw_Loop
//
// Loops updating the game window.
void Draw_Loop(void (*proc)(void *data), void (*draw)(void *data, float f),
void *data) {
_proc_func = proc;
_draw_func = draw;
_data = data;
if (_draw_looping) {
return;
}
_draw_looping = 1;
#ifndef EMSCRIPTEN
long long procTime1, procTime2, drawTime1, drawTime2;
_accTime = proc_t_frame;
procTime1 = drawTime1 = Time_GetTime();
while (Draw_LoopIteration()) {
// Wait to round draw_t_frame
drawTime2 = Time_GetTime();
Time_Pause(draw_t_frame - (drawTime2 - drawTime1));
drawTime2 = Time_GetTime();
drawTime1 = drawTime2;
// Update time
procTime2 = Time_GetTime();
_accTime += procTime2 - procTime1;
procTime1 = procTime2;
}
#else
_accTime = proc_t_frame;
_procTime1 = Time_GetTime();
if (_fps <= 50) {
emscripten_set_main_loop(Draw_LoopIterationAux, _fps, 1);
} else {
emscripten_set_main_loop(Draw_LoopIterationAux, 0, 1);
}
#endif
}
/*{{{ void ccsp_safe_pause_timeout (sched_t *sched)*/
void ccsp_safe_pause_timeout (sched_t *sched)
{
unsigned int sync;
Time now;
#ifdef DEBUG_RTS
fprintf(stderr, "USERPROC: ccsp_safe_pause_timeout() entered\n");
#endif
if (sched->tq_fptr == NULL) {
return;
} else if (Time_PastTimeout (sched)) {
return;
}
now = Time_GetTime(sched);
if (Time_AFTER (sched->tq_fptr->time, now)) {
unsigned int usecs = Time_MINUS (sched->tq_fptr->time, now);
if (usecs < min_sleep) {
while (!(sync = att_safe_swap (&(sched->sync), 0))) {
int i = 10;
while (i--) {
idle_cpu ();
}
if (Time_PastTimeout (sched)) {
break;
}
serialise ();
}
if (sync) {
/* restore detected flags */
att_safe_or (&(sched->sync), sync);
}
} else {
ccsp_set_next_alarm (sched, usecs);
ccsp_safe_pause (sched);
}
}
#ifdef DEBUG_RTS
fprintf(stderr, "USERPROC: ccsp_safe_pause_timeout() about to exit (return 0)\n");
#endif
}
task Gyro()
{
HTGYROstartCal(sensor_gyro);
float vel_curr = 0.0;
float vel_prev = 0.0;
float dt = 0.0;
int timer_gyro = 0;
Time_ClearTimer(timer_gyro);
while (true) {
vel_prev = vel_curr;
dt = (float)Time_GetTime(timer_gyro)/(float)1000.0; // msec to sec
Time_ClearTimer(timer_gyro);
vel_curr = (float)HTGYROreadRot(sensor_gyro);
heading += ((float)vel_prev+(float)vel_curr)*(float)0.5*(float)dt;
Time_Wait(1);
}
}
char write_string_to_files(uchar * data)
{
int waitCnt=0;
int maxWaitCnt = 100000;
NORMALTIME time;
uchar fileName[30];//本来只有13长度，怕越界了
time = Time_GetTime();
sprintf(fileName,"%02d%02d%02d%02d.txt",time.tm_year-2000,time.tm_mon,time.tm_mday,time.tm_hour);
//①获取系统状态
//----------------------------------------------------------------------------
waitCnt=0;
while(IS_SD_BUSY)
{
waitCnt++;
if(waitCnt>maxWaitCnt)
{
return 7;
}
};//先判断SD是否忙
SD_read_system();//上电后串口第一个字节发不出去
SD_read_system();
//等待状态
F_wait_response = 0x01;
SD_cmd_return_byte = 0;
waitCnt = 0;
while(F_wait_response)
{
waitCnt++;
if(waitCnt>maxWaitCnt)
{
return 1;
}
};
if((SD_cmd_return_byte & 0xCF) == 0x0D)//没有文件打开
{
}
else if((SD_cmd_return_byte & 0xCF) == 0x1D)//已经有文件打开 则关闭
{
SD_close_file();
while(IS_SD_BUSY);
}
else
{
return SD_cmd_return_byte;
}
//②创建文件
//----------------------------------------------------------------------------
SD_send_cmd(SD_CREATE,13,fileName);
F_wait_response = 0x01;
SD_cmd_return_byte = 0;
waitCnt=0;
while(F_wait_response)
{
waitCnt++;
if(waitCnt>maxWaitCnt)
{
return 2;
}
};
while(IS_SD_BUSY);
//③打开文件
//----------------------------------------------------------------------------
SD_send_cmd(SD_OPEN,13,fileName);
F_wait_response = 0x01;
SD_cmd_return_byte = 0;
waitCnt=0;
while(F_wait_response)
{
waitCnt++;
if(waitCnt>maxWaitCnt)
{
return 3;
}
};
while(IS_SD_BUSY);
//④写入文件
//----------------------------------------------------------------------------
SD_send_data(SD_WRITE,strlen(data)+4,0xffffffff,data);
F_wait_response = 0x01;
SD_cmd_return_byte = 0;
waitCnt=0;
while(F_wait_response)
{
waitCnt++;
if(waitCnt>maxWaitCnt)
{
return 4;
}
};
while(IS_SD_BUSY);
//⑤保存文件
//----------------------------------------------------------------------------
SD_save_file();//保存文件
F_wait_response = 0x01;
SD_cmd_return_byte = 0;
waitCnt=0;
while(F_wait_response)
{
waitCnt++;
if(waitCnt>maxWaitCnt)
{
return 5;
}
};
while(IS_SD_BUSY);
//⑥关闭文件
//----------------------------------------------------------------------------
SD_close_file();//关闭文件
F_wait_response = 0x01;
SD_cmd_return_byte = 0;
waitCnt=0;
while(F_wait_response)
{
waitCnt++;
if(waitCnt>maxWaitCnt)
{
return 6;
}
};
while(IS_SD_BUSY);
return 0;
}
task PID()
{
const float kP_up = 0.082;
const float kI_up = 0.017;
const float kD_up = 0.00;
const float kP_down = 0.002;
const float kI_down = 0.005;
const float kD_down = 0.00;
const float I_term_decay_rate = 0.87;
const int I_term_threshold = 500;
int timer_loop = 0;
Time_ClearTimer(timer_loop);
int dt = Time_GetTime(timer_loop);
lift_pos = Motor_GetEncoder(encoder_lift);
float error = 0.0;
float error_prev = 0.0;
float error_sum = 0.0;
float error_rate = 0.0;
Joystick_WaitForStart();
Time_ClearTimer(timer_loop);
while (true) {
dt = Time_GetTime(timer_loop);
Time_ClearTimer(timer_loop);
error_prev = error;
lift_pos = Motor_GetEncoder(encoder_lift);
if (is_lift_manual == false) {
if (lift_target < pos_lift_bottom) {
lift_target = pos_lift_bottom;
}
if (lift_target > pos_lift_top) {
lift_target = pos_lift_top;
}
error = lift_target - lift_pos;
if (error > 0) {
isDown = false;
} else {
isDown = true;
}
error_sum *= I_term_decay_rate;
if (error < I_term_threshold) {
error_sum += error * (float)dt;
}
error_rate = (error - error_prev) / (float)dt;
term_P_lift = error;
term_I_lift = error_sum;
term_D_lift = error_rate;
switch (isDown) {
case true :
term_P_lift *= kP_down;
term_I_lift *= kI_down;
term_D_lift *= kD_down;
break;
case false :
term_P_lift *= kP_up;
term_I_lift *= kI_up;
term_D_lift *= kD_up;
break;
}
power_lift = term_P_lift + term_I_lift + term_D_lift;
} else {
lift_target = lift_pos;
power_lift = power_lift_temp;
}
if (abs(power_lift)<20) {
power_lift = 0;
}
if (isLiftFrozen) {
power_lift = 0;
}
if (isReset == false) {
if (power_lift>0 && lift_pos>pos_lift_top) {
power_lift = 0;
} else if (power_lift<0 && lift_pos<pos_lift_bottom) {
power_lift = 0;
}
} else {
Motor_ResetEncoder(encoder_lift);
}
Motor_SetPower(power_lift, motor_lift_A);
Motor_SetPower(power_lift, motor_lift_B);
Time_Wait(2);
}
}
int main(void)
{
u16 times=0,i=0;
NORMALTIME cur_time; //设置RTC的时间用
//Flash_Write(0x08041000,(u8 *)IndCalib,sizeof(IndCalib));
Flash_Read(0x08041000,(u8 *)IndCalib,sizeof(IndCalib));
delay_init(); //延时函数初始化
NVIC_Configuration(); //设置NVIC中断分组2:2位抢占优先级，2位响应优先级
uart1_init(115200); //上位机通信模块
uart2_init(9600); //风速传感器模块 A2 A3 要转为232
//uart3_init(19200); //SD卡模块 （兼职风向-因为3有重映射功能）
uart4_init(9600); //称重模块
uart5_init(9600); //AD模块
LED_Init(); //LED端口初始化
KEY_Init(); //初始化与按键连接的硬件接口
SD_init();
OPEN_SD_POWER;
CLOSE_SD_POWER;
OPEN_SD_POWER;
CLOSE_SD_POWER;
OPEN_SD_POWER;
cur_time.tm_year = 2016; //2016-1900
cur_time.tm_mon = 5;
cur_time.tm_mday = 29;
cur_time.tm_hour = 23;
cur_time.tm_min = 56;
cur_time.tm_sec = 2;
RTC_Init(cur_time);//
while(DS18B20_Init())//初始化DS18B20,兼检测18B20
{
printf("DS18B20 Check Failed!");
printf("Please Check! ");
}
printf("DS18B20 Ready! ");
while(1)
{
if(IS_BUT_DN)
{
BEEP = 0;
}
else
{
BEEP = 1;
g_PaOffset= -g_Pa;
g_IndValOffset[0]= -Volt2Distance(0,g_IndVal[0]);
g_IndValOffset[1]= -Volt2Distance(1,g_IndVal[1]);
g_IndValOffset[2]= -Volt2Distance(2,g_IndVal[2]);
g_IndValOffset[3]= -Volt2Distance(3,g_IndVal[3]);
}
if(times%60==0)
{
windSpeedDirFlag++;
sendWindSpeedCmd(); //串口2 带232
//printf("3\r\n");
if(5 == windSpeedDirFlag)
{
windSpeedDirFlag = 0;
}
//printf("4\r\n");
//风速的反应慢 一秒钟最多发一次指令 不然传感器要疯掉
if(0 == windSpeedDirFlag)
{
uart3_init(19200); //SD卡模块 （兼职风向-因为3有重映射功能）
g_uart3_used_for_SD = 1;
// 日期 时间 位移1, 2, 3, 4 |拉力| 温度 |SD|风速|风向
sprintf(CmdStr,"#01,%02d-%02d-%02d,%02d:%02d:%02d,%04d,%04d,%04d,%04d,%04d,%04d,%04d,%04d,%04d,%04d,%1d,%04d,%04d!\r\n",
cur_time.tm_year,cur_time.tm_mon,cur_time.tm_mday,
cur_time.tm_hour,cur_time.tm_min,cur_time.tm_sec,
dateSendtoPC[0],dateSendtoPC[1],dateSendtoPC[2],dateSendtoPC[3],
dateSendtoPC[4],
dateSendtoPC[6],dateSendtoPC[7],dateSendtoPC[8],dateSendtoPC[9],
g_Temper,lastSDerr,g_WindSpeed,g_WindDir);
//printf("7\r\n");
lastSDerr = write_string_to_files(CmdStr);
}
else
{
//printf("8\r\n");
uart3_init2(9600);
//SD卡的usart3 重映射到 PB10 和PB11口 来读风向
g_uart3_used_for_SD = 0;
send_byte_to_usart3(0x02);//初始化完后第一个字节会发不成功
sendWindDirCmd();//串口3
}
//printf("11\r\n");
cur_time = Time_GetTime();
}
if(times%10==0)
{
//sendWeightLoad1Cmd();
//称重标定代码
//printf("12\r\n");
switch(PC_Wcmd)
{
case 0x30:
//printf("13\r\n");
BEEP = 1;
sendWeightZeroCmd();
BEEP = 0;
PC_Wcmd = 0;
break;
case 0x40:
//printf("14\r\n");
BEEP = 1;
sendWeightLoad1Cmd();
PC_Wcmd = 0;
BEEP = 0;
break;
case 0x41:
//printf("15\r\n");
BEEP = 1;
sendWeightLoad2Cmd(g_weightCalib[0],g_weightCalib[1]);
PC_Wcmd = 0;
BEEP = 0;
break;
default:
//printf("16\r\n");
//sendWeightLoad2Cmd(0x27,0x10);
sendWeightCmd(); //串口4
break;
}
BLED3=1;
sendADCmd(); //串口5
BLED3=0;
dateSendtoPC[0] = g_IndVal[0];
dateSendtoPC[1] = g_IndVal[1];
dateSendtoPC[2] = g_IndVal[2];
dateSendtoPC[3] = g_IndVal[3];
dateSendtoPC[4] = g_PaOffset + g_Pa;
dateSendtoPC[6] = g_IndValOffset[0] + Volt2Distance(0,g_IndVal[0]);
dateSendtoPC[7] = g_IndValOffset[1] + Volt2Distance(1,g_IndVal[1]);
dateSendtoPC[8] = g_IndValOffset[2] + Volt2Distance(2,g_IndVal[2]);
dateSendtoPC[9] = g_IndValOffset[3] + Volt2Distance(3,g_IndVal[3]);
}
switch(PC_cmd)
{
case 1:
BLED2 = 1;
// 日期 时间 位移1, 2, 3, 4 |拉力| 温度 |SD|风速|风向
sprintf(CmdStr,"#01,%02d-%02d-%02d,%02d:%02d:%02d,%04d,%04d,%04d,%04d,%04d,%04d,%04d,%04d,%04d,%04d,%d,%04d,%04d!\r\n",
cur_time.tm_year,cur_time.tm_mon,cur_time.tm_mday,
cur_time.tm_hour,cur_time.tm_min,cur_time.tm_sec,
dateSendtoPC[0],dateSendtoPC[1],dateSendtoPC[2],dateSendtoPC[3],
dateSendtoPC[4],
dateSendtoPC[6],dateSendtoPC[7],dateSendtoPC[8],dateSendtoPC[9],
g_Temper,lastSDerr,g_WindSpeed,g_WindDir);
send_string_to_usart1(CmdStr);
BLED2 = 0;
PC_cmd = 0;
break;
case 0x10:
case 0x11:
case 0x12:
case 0x13:
case 0x14:
BEEP = 1;
sprintf(CmdStr,"#%02x",PC_cmd);
for(i=0;i<21;i++)
{
sprintf(CmdStr,"%s,%+05d",CmdStr,IndCalib[PC_cmd-0x10][i]);
}
sprintf(CmdStr,"%s!\r\n",CmdStr);
send_string_to_usart1(CmdStr);
PC_cmd = 0;
BEEP = 0;
break;
case 0x20:
case 0x21:
case 0x22:
case 0x23:
case 0x24:
sprintf(CmdStr,"#%02x",PC_cmd-0x10);
for(i=0;i<21;i++)
{
sprintf(CmdStr,"%s,%+05d",CmdStr,IndCalib[PC_cmd-0x20][i]);
}
sprintf(CmdStr,"%s!\r\n",CmdStr);
send_string_to_usart1(CmdStr);
PC_cmd = 0;
break;
}
times++;
delay_ms(10);
}
}
