#define PID_P 0.3
-#define PID_P_S 0.7
+#define PID_P_S 0.8 /*2.3 kmita*/ /*1.4 vhodne jen pro P regulator*/
+#define PID_I_S 0.01
#define PRIOR_KERN 50
#define PRIOR_HIGH 49
.pos_reg_ena=0,
.desired_spd=0,
.spd_reg_ena=0,
- .old_pos={0}
+ .old_pos={0},
+ .spd_err_sum=0,
+ .log_col_count=0, /* pocet radku zaznamu */
+ .log_col=0,
+ .doLogs=0
};
/**
return x;
}
+
+/*
+ * \brief
+ * Free logs
+ */
+void freeLogs(){
+ int r;
+ if (rps.log_col_count){
+ for (r=0;r<LOG_ROWS;r++){
+ free(rps.logs[r]);
+ }
+ }
+ rps.log_col_count=0;
+ rps.doLogs=0;
+}
+
+/*
+ * \brief
+ * Makes log.
+ */
+void makeLog(){
+ int r;
+ if (rps.log_col==MAX_LOGS-1){
+ rps.doLogs=0;
+ return;
+ }
+ rps.logs[0][rps.log_col]=(int)rps.tf_count;
+ rps.logs[1][rps.log_col]=(int)rps.spi_dat->pozice;
+
+ rps.logs[2][rps.log_col]=(int)rps.pwm1;
+ rps.logs[3][rps.log_col]=(int)rps.pwm2;
+ rps.logs[4][rps.log_col]=(int)rps.pwm3;
+ rps.logs[5][rps.log_col]=rps.duty;
+
+ rps.logs[6][rps.log_col]=rps.desired_spd;
+ rps.logs[7][rps.log_col]=rps.speed;
+
+ rps.logs[8][rps.log_col]=(int)(rps.spi_dat->ch1/rps.spi_dat->adc_m_count);
+ rps.logs[9][rps.log_col]=(int)(rps.spi_dat->ch2/rps.spi_dat->adc_m_count);
+ rps.logs[10][rps.log_col]=(int)(rps.spi_dat->ch0/rps.spi_dat->adc_m_count);
+
+ rps.log_col++;
+ /*
+ if (rps.log_col==rps.log_col_count-1){
+ rps.log_col_count*=2;
+ rps.log_col_count%=MAX_LOGS;
+ for (r=0;r<LOG_ROWS;r++){
+ rps.logs[r]=realloc(rps.logs[r],rps.log_col_count*sizeof(int));
+ if (rps.logs[r]==NULL){
+ rps.doLogs=0;
+ rps.error=1;
+ }
+ }
+ }
+ */
+}
+
/*
* \brief
* Pripravi psi buffer
spi_disable();
clk_disable();
+ freeLogs();
/*muzeme zavrit semafor*/
sem_destroy(&rps.thd_par_sem);
printf("\nprogram bezpecne ukoncen\n");
*/
inline void spd_pid(){
int duty_tmp;
- duty_tmp = PID_P_S*(rps.desired_spd - rps.speed);
+ int error;
+ error=rps.desired_spd - rps.speed;
+ rps.spd_err_sum+=error;
+ duty_tmp = PID_P_S*error+PID_I_S*rps.spd_err_sum;
if (duty_tmp>MAX_DUTY){
rps.duty=MAX_DUTY;
}else if (duty_tmp<-MAX_DUTY){
}else if(!rps.index_ok && rps.commutate){
simple_hall_commutator(rps.duty);
}
+
+ /*zalogujeme hodnoty*/
+ if (rps.doLogs && !(rps.tf_count%LOG_PERIOD)){
+ makeLog();
+ }
+
sem_post(&rps.thd_par_sem); /*--post semaphore---*/
/* calculate next shot */