puts("0 - Vypne pwm a rizeni. Enable bity na nulu.");
puts("ga:[hodnota] - Zapne rizeni na zvolenou absolutni pozici.");
puts("print - Zapne nebo vypne pravidelne vypisovani hodnot.");
+ puts("duty:[hodnota] - Nastavi pevnou sirku plneni.");
+
puts("help - Vypne vypisovani hodnot a zobrazi tuto napovedu.");
puts("exit - Bezpecne ukonci program.");
}
static void start(struct rpi_state* state){
sem_wait(&state->thd_par_sem);
state->test=0x70; /*konfiguracni byte*/
- state->commutate=1;
sem_post(&state->thd_par_sem);
}
sem_wait(&state->thd_par_sem);
state->test=0; /*konfiguracni byte*/
state->commutate=0;
+ state->pos_reg_ena=0;
state->pwm1=0;
state->pwm2=0;
state->pwm3=0;
* Nastavi pevnou sirku plneni
*/
static void dutySet(struct rpi_state* state, int duty){
-
+ sem_wait(&state->thd_par_sem);
+ if (duty>512) duty=512;
+ if (duty<-512) duty=-512;/*paranoia*/
+ state->duty=duty;
+ state->commutate=1;
+ state->pos_reg_ena=0;
+ sem_post(&state->thd_par_sem);
}
/*
*/
static void goAbsolute(struct rpi_state* state, int pos){
sem_wait(&state->thd_par_sem);
+ state->pos_reg_ena=1;
state->desired_pos=pos;
sem_post(&state->thd_par_sem);
}
.index_dist=0, /* distance to index position */
.index_ok=0,
.tf_count=0, /*number of transfer*/
- .desired_pos=0 /* desired position */
+ .desired_pos=0, /* desired position */
+ .pos_reg_ena=0,
+ .desired_spd=0,
+ .spd_reg_ena=0
};
/**
* Now only with P-part so that the error doesnt go to zero.
* TODO: add anti-wind up and I and D parts
*/
-inline void pid(){
+inline void pos_pid(){
int duty_tmp;
duty_tmp = PID_P*(rps.desired_pos - (int32_t)data.pozice);
if (duty_tmp>MAX_DUTY){
/*
* \brief
* Feedback loop.
+ * TODO: replace bunch of 'IFs' with Object-like pattern
*/
void * read_data(void* param){
int i;
/*subtract initiate postion */
rps.tf_count++;
substractOffset(&data,&pocatek);
- comIndDist();
+
if (!rps.index_ok){
if (first){
last_index=data.index_position;
first=0;
}else if (last_index!=data.index_position){
rps.index_ok=1;
+ comIndDist(); /*vypocet vzdalenosti indexu*/
}
+ }else{ /*index je v poradku*/
+ comIndDist(); /*vypocet vzdalenosti indexu*/
+ }
+ /* pocitame sirku plneni podle potreb rizeni*/
+ if (rps.pos_reg_ena){
+ pos_pid();
}
- pid();
+ /* sirka plneni prepoctena na jednotlive pwm */
if (rps.index_ok && rps.commutate){
/*simple_ind_dist_commutator(rps.duty);*/
/*sin_commutator(rps.duty);*/
struct rpi_in;
struct rpi_state{
- struct rpi_in* spi_dat;
- sem_t thd_par_sem;
- uint8_t test;
- uint16_t pwm1, pwm2, pwm3;
- uint16_t t_pwm1, t_pwm2, t_pwm3;
- char commutate;
- int duty; /* duty cycle of pwm */
+ struct rpi_in* spi_dat; /* spi data */
+ sem_t thd_par_sem; /* data metual exlusion access */
+ uint8_t test; /* configuratin byte - pwm enabl. bits etc. */
+ uint32_t tf_count; /* number of transfer*/
+
uint16_t index_dist; /* distance to index position */
- unsigned char index_ok;
- uint32_t tf_count; /*number of transfer*/
+ unsigned char index_ok; /* we have actual index position */
+
+ uint16_t pwm1, pwm2, pwm3; /* pwm duty cycles*/
+ uint16_t t_pwm1, t_pwm2, t_pwm3;/* debug*/
+
+ int duty; /* duty cycle of pwm */
int desired_pos; /* desired position */
+ int desired_spd; /* desired speed */
+
+ char commutate; /* zapina prepocet duty na jednotlive pwm */
+ char pos_reg_ena; /* position regulation enable */
+ char spd_reg_ena; /* speed rugulation enable */
};
#endif /*PMSM_STATE*/