*/
static void stop(struct rpi_state* state){
sem_wait(&state->thd_par_sem);
- state->test=0; /*konfiguracni byte*/
state->commutate=0;
state->pwm1=0;
state->pwm2=0;
struct rpi_state s; /*state*/
float cur0, cur1, cur2;
int i;
- signed long int speed;
/* copy the data */
sem_wait(&state->thd_par_sem);
data_p = *state->spi_dat;
s=*state;
sem_post(&state->thd_par_sem);
- speed=data_p.pozice-s.old_pos;
if (data_p.adc_m_count){
cur0=data_p.ch0/data_p.adc_m_count;
cur1=data_p.ch1/data_p.adc_m_count;
printf("%.2X ", data_p.debug_rx[i]);
}
puts("");
- printf("\npozice=%u\n",data_p.pozice);
- printf("rychlost=%ld\n",speed);
+ printf("\npozice=%ld\n",data_p.pozice);
+ printf("rychlost=%d\n",s.speed);
printf("chtena pozice=%d\n",s.desired_pos);
printf("transfer count=%u\n",s.tf_count);
printf("raw_pozice=%u\n",data_p.pozice_raw);
.desired_pos=0, /* desired position */
.pos_reg_ena=0,
.desired_spd=0,
- .spd_reg_ena=0
+ .spd_reg_ena=0,
+ .old_pos={0}
};
/**
}
void substractOffset(struct rpi_in* data, struct rpi_in* offset){
- data->pozice_raw=data->pozice;
- data->pozice-=offset->pozice;
+ data->pozice=data->pozice_raw-offset->pozice_raw;
return;
}
/*
*/
inline void spd_pid(){
int duty_tmp;
- signed long int speed=rps.spi_dat->pozice-rps.old_pos;
duty_tmp = PID_P*(rps.desired_pos - (int32_t)data.pozice);
if (duty_tmp>MAX_DUTY){
rps.duty=MAX_DUTY;
rps.duty = duty_tmp;
}
}
+
+/*
+ * \brief
+ * Computate speed.
+ */
+void compSpeed(){
+ signed long int spd;
+ spd=rps.spi_dat->pozice-rps.old_pos[rps.tf_count%OLD_POS_NUM];
+ rps.speed=(int32_t)spd;
+}
+
/*
* \brief
* Feedback loop.
clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &t, NULL);
sem_wait(&rps.thd_par_sem); /*---take semaphore---*/
- rps.old_pos=rps.spi_dat->pozice; /*old position*/
+ /*old positions*/
+ rps.old_pos[rps.tf_count%OLD_POS_NUM]=rps.spi_dat->pozice;
prepare_tx(tx); /*save the data to send*/
data = spi_read(tx); /*exchange data*/
/*subtract initiate postion */
rps.tf_count++;
substractOffset(&data,&pocatek);
+ compSpeed(); /*spocita rychlost*/
if (!rps.index_ok){
if (first){
}
-
/**
* \brief Main function.
*/
#include <stdint.h>
#include <semaphore.h>
+#define OLD_POS_NUM 50
+
struct rpi_in;
struct rpi_state{
int duty; /* duty cycle of pwm */
int desired_pos; /* desired position */
- int old_pos; /* one cycle old position */
+ int old_pos[OLD_POS_NUM]; /* old positions */
int desired_spd; /* desired speed */
+ int speed;
char commutate; /* zapina prepocet duty na jednotlive pwm */
char pos_reg_ena; /* position regulation enable */
*/
/*uprava endianity pozice*/
- uint8_p=(uint8_t*)&in.pozice;
+ uint8_p=(uint8_t*)&in.pozice_raw;
/* x86 je Little-Endian */
uint8_p[0]=rx[3]; /* LSB */
uint8_p[1]=rx[2];
* \brief Struktura pro prichozi data z fpga.
*/
struct rpi_in{
- uint32_t pozice; /*continue with normal size and test it..*/
+ signed long int pozice; /*continue with normal size and test it..*/
uint32_t pozice_raw; /*with offset*/
uint32_t ch0, ch1, ch2;
int8_t hal1,hal2,hal3; /* bool values */