X-Git-Url: https://rtime.felk.cvut.cz/gitweb/fpga/rpi-motor-control.git/blobdiff_plain/59b72ac21a7189e402f90a288889174af4fe3015..1cf729421e63f859e426a429f9ce935a86afd167:/pmsm-control/test_sw/main_pmsm.c diff --git a/pmsm-control/test_sw/main_pmsm.c b/pmsm-control/test_sw/main_pmsm.c index ec33728..2eb9262 100644 --- a/pmsm-control/test_sw/main_pmsm.c +++ b/pmsm-control/test_sw/main_pmsm.c @@ -15,27 +15,53 @@ #include /*sheduler*/ #include /*usleep*/ #include /*threads*/ +#include /*nanosleep*/ +#include #include "rpin.h" /*gpclk*/ #include "rp_spi.h" /*spi*/ #include "misc.h" /*structure for priorities*/ - - -#define PRUM_PROUD 2061 -#define PRUM_SOUC 6183 +#include "pmsm_state.h" +#include "cmd_proc.h" +#include "controllers.h" +#include "commutators.h" +#include "comp.h" +#include "logs.h" #define PRIOR_KERN 50 #define PRIOR_HIGH 49 #define PRIOR_LOW 20 #define THREAD_SHARED 0 -#define INIT_VALUE 0 /*init value for semaphor*/ +#define INIT_VALUE 1 /*init value for semaphor*/ -struct sigaction sighnd; /*struktura pro signal handler*/ -struct rpi_in data; -uint8_t test; -uint16_t pwm1, pwm2, pwm3; +#define NSEC_PER_SEC (1000000000) /* The number of nsecs per sec. */ + + +struct rpi_in data; +struct rpi_state rps={ + //.MAX_DUTY=170, + .spi_dat=&data, + .test=0, + .pwm1=0,.pwm2=0, .pwm3=0, + .pwm1=0, .t_pwm2=0, .t_pwm3=0, + .commutate=0, + .duty=0, /* duty cycle of pwm */ + .index_dist=0, /* distance to index position */ + .index_ok=0, + .tf_count=0, /*number of transfer*/ + .desired_pos=0, /* desired position */ + .pos_reg_ena=0, + .desired_spd=0, + .spd_reg_ena=0, + .old_pos={0}, + .spd_err_sum=0, + .log_col_count=0, /* pocet radku zaznamu */ + .log_col=0, + .doLogs=0, + .alpha_offset=960 +}; /** @@ -48,204 +74,142 @@ int clk_init() gpioSetMode(4, FSEL_ALT0); return 0; } -/* - * \brief Terminates GPCLK. - */ - -inline void clk_disable(){ - termClock(0); -} /** * \brief Signal handler pro Ctrl+C */ -void sighnd_fnc(){ +void appl_stop(){ + uint8_t tx[16]; + sem_wait(&rps.thd_par_sem); + + memset(tx,0,16*sizeof(int)); + rps.pwm1=0; + rps.pwm2=0; + rps.pwm3=0; + spi_read(&rps); + spi_disable(); - clk_disable(); + termClock(0); + freeLogs(&rps); + /*muzeme zavrit semafor*/ + sem_destroy(&rps.thd_par_sem); printf("\nprogram bezpecne ukoncen\n"); - exit(0); } -void substractOffset(struct rpi_in* data, struct rpi_in* offset){ - data->pozice-=offset->pozice; - return; -} -/* - * pocita procentualni odchylku od prumerneho proudu - */ -float diff_p(int16_t value){ - return ((float)value-PRUM_PROUD)*100/PRUM_PROUD; -} -/* - * pocita procentualni odchylku od prumerneho souctu proudu - */ -float diff_s(int16_t value){ - return ((float)value-PRUM_SOUC)*100/PRUM_SOUC; -} -/* - * tiskne potrebna data - */ -void printData(struct rpi_in data){ - printf("\npozice=%d\n",(int32_t)data.pozice); - printf("hal1=%d, hal2=%d, hal3=%d\n",data.hal1,data.hal2,data.hal3); - printf("en1=%d, en2=%d, en3=%d (Predchozi hodnoty)\n",data.en1,data.en2,data.en3); - printf("shdn1=%d, shdn2=%d, shdn3=%d (P.h.)\n",data.shdn1,data.shdn2,data.shdn3); - printf("PWM1(10d5)b54=%d %d %d %d %d %d=b49(P.h.)\n",data.b54,data.b53,data.b52,data.b51,data.b50,data.b49); - printf("PWM2(10d4)b48=%d %d %d %d %d %d %d=b42(P.h.)\n",data.b48,data.b47,data.b46,data.b45,data.b44,data.b43,data.b42); - printf("PWM3(10d5)b41=%d %d %d %d %d %d=b36(P.h.)\n",data.b41,data.b40,data.b39,data.b38,data.b37,data.b36); - printf("(pwm1)proud1 (ch1)=%d (%2.2f%%) %x\n",data.ch1,diff_p(data.ch1),(uint16_t)data.ch1); - printf("(pwm2)proud2 (ch2)=%d (%2.2f%%) %x\n",data.ch2,diff_p(data.ch2),(uint16_t)data.ch2); - printf("(pwm3)proud3 (ch0)=%d (%2.2f%%) %x\n",data.ch0,diff_p(data.ch0),(uint16_t)data.ch0); - printf("soucet proudu=%d (%2.2f%%)\n",data.ch0+data.ch1+data.ch2,diff_s(data.ch0+data.ch1+data.ch2)); -} -void prepare_tx(uint8_t * tx){ - - /*Data format: - * tx[0] - bity 95 downto 88 - bits that are sent first - * tx[1] - bity 87 downto 80 - * tx[2] - bity 79 downto 72 - * tx[3] - bity 71 downto 64 - * tx[4] - bity 63 downto 56 - * tx[5] - bity 55 downto 48 - * tx[6] - bity 47 downto 40 - * tx[7] - bity 39 downto 32 - * tx[8] - bity 31 downto 24 - * tx[9] - bity 23 downto 16 - * tx[10] - bity 15 downto 8 - * tx[11] - bity 7 downto 0 - * - * bit 95 - ADC reset - * bit 94 - enable PWM1 - * bit 93 - enable PWM2 - * bit 92 - enable PWM3 - * bit 91 - shutdown1 - * bit 90 - shutdown2 - * bit 89 - shutdown3 - * . - * . - * . - * bits 34 .. 24 - match PWM1 - * bits 23 .. 13 - match PWM2 - * bit 11,12 - Unused - * bits 10 .. 0 - match PWM3 - */ - - - uint16_t tmp; - - /* keep the cap*/ - if (pwm1>2047) pwm1=2047; - if (pwm2>2047) pwm2=2047; - if (pwm3>2047) pwm3=2047; - - tx[0]=test; /*bit 94 - enable PWM1*/ - - /*pwm1*/ - tx[7]=(tx[7] & 0xF8) | (0x07 & ((uint8_t*)&pwm1)[1]); /*MSB*/ - tx[8]=((uint8_t*)&pwm1)[0]; /*LSB*/ - - /*pwm2*/ - tmp=pwm2; - tmp<<=5; - tx[9]=((uint8_t*)&tmp)[1]; /*MSB*/ - tx[10]=(tx[10] & 0x1F) | (0xE0 & ((uint8_t*)&tmp)[0]); /*LSB*/ - - /*pwm3*/ - tx[10]=(tx[10] & 0xF8) | (0x07 & ((uint8_t*)&pwm3)[1]); /*MSB*/ - tx[11]=((uint8_t*)&pwm3)[0]; /*LSB*/ - - -} /** * Funkce pravidelne vypisuje posledni zjistenou pozici lokalniho motoru */ void * pos_monitor(void* param){ - set_priority(param); /*set priority*/ while(1){ - printData(data); + printData(&rps); usleep(1000000); /*1 Hz*/ } return (void*)0; } -/** - * Funkce pravidelne vycita data z motoru +/* + * \brief + * Feedback loop. + * TODO: replace bunch of 'IFs' with Object-like pattern */ void * read_data(void* param){ int i; struct rpi_in pocatek; - uint8_t tx[]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0} ; - set_priority(param); /*set priority*/ - pocatek = spi_read(tx); + struct rpi_state poc={ + .spi_dat=&pocatek, + .test=0, + .pwm1=0, .pwm1=0, .pwm3=0 + }; + struct timespec t; + int interval = 1000000; /* 1ms ~ 1kHz*/ + char first=1; + uint16_t last_index; /*we have index up-to date*/ + spi_read(&poc); /*pocatecni informace*/ + clock_gettime(CLOCK_MONOTONIC ,&t); + /* start after one second */ + t.tv_sec++; while(1){ - prepare_tx(tx); - data = spi_read(tx); - substractOffset(&data,&pocatek); - usleep(1000); /*1kHz*/ + /* wait until next shot */ + clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &t, NULL); + sem_wait(&rps.thd_par_sem); /*---take semaphore---*/ + + /*old positions*/ + rps.old_pos[rps.tf_count%OLD_POS_NUM]=rps.spi_dat->pozice; + spi_read(&rps); /*exchange data*/ + /*subtract initiate postion */ + rps.tf_count++; + substractOffset(&data,poc.spi_dat); + compSpeed(&rps); /*spocita rychlost*/ + + 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(&rps); /*vypocet vzdalenosti indexu*/ + } + }else{ /*index je v poradku*/ + comIndDist(&rps); /*vypocet vzdalenosti indexu*/ + } + + /* pocitame sirku plneni podle potreb rizeni*/ + if (rps.pos_reg_ena){ /*pozicni rizeni*/ + pos_pid(&rps); + }else if(rps.spd_reg_ena){ /*rizeni na rychlost*/ + spd_pid(&rps); + } + + /* sirka plneni prepoctena na jednotlive pwm */ + if (rps.index_ok && rps.commutate){ + /*simple_ind_dist_commutator(rps.duty);*/ + /*sin_commutator(rps.duty);*/ + inv_trans_comm(&rps); + inv_trans_comm_2(&rps); + }else if(!rps.index_ok && rps.commutate){ + simple_hall_commutator(&rps); + } + + /*zalogujeme hodnoty*/ + if (rps.doLogs && !(rps.tf_count%LOG_PERIOD)){ + makeLog(&rps); + } + + sem_post(&rps.thd_par_sem); /*--post semaphore---*/ + + /* calculate next shot */ + t.tv_nsec += interval; + + while (t.tv_nsec >= NSEC_PER_SEC) { + t.tv_nsec -= NSEC_PER_SEC; + t.tv_sec++; + } + } } /** * \brief Main function. */ - int main(){ - uint16_t tmp; - - /*nastaveni priorit vlaken*/ - struct thread_param tsp; - tsp.sch_policy = SCHED_FIFO; - - /*nastaveni signalu pro vypnuti pomoci Ctrl+C*/ - sighnd.sa_handler=&sighnd_fnc; - sigaction(SIGINT, &sighnd, NULL ); - + pthread_t base_thread_id; clk_init(); /* inicializace gpio hodin */ spi_init(); /* iniicializace spi*/ /*semafor pro detekci zpracovani parametru vlaken*/ - sem_init(&thd_par_sem,THREAD_SHARED,INIT_VALUE); - - /*vlakna*/ - pthread_t tid; /*identifikator vlakna*/ - pthread_attr_t attr; /*atributy vlakna*/ - pthread_attr_init(&attr); /*inicializuj implicitni atributy*/ + sem_init(&rps.thd_par_sem,THREAD_SHARED,INIT_VALUE); + setup_environment(); + base_thread_id=pthread_self(); + /*main control loop*/ + create_rt_task(&base_thread_id,PRIOR_HIGH,read_data,NULL); - /*ziskavani dat z motoru*//*vysoka priorita*/ - tsp.sch_prior = PRIOR_HIGH; - pthread_create(&tid, &attr, read_data, (void*)&tsp); + /*monitor of current state*/ + create_rt_task(&base_thread_id,PRIOR_LOW,pos_monitor,NULL); - /*vypisovani lokalni pozice*//*nizka priorita*/ - tsp.sch_prior = PRIOR_LOW; - pthread_create(&tid, &attr, pos_monitor, (void*)&tsp); + /*wait for commands*/ + poll_cmd(&rps); - - /*muzeme zavrit semafor*/ - sem_destroy(&thd_par_sem); - - while (1){ - scanf("%u",&tmp); - printf("volba=%x\n",tmp); - switch (tmp){ - case 1: - scanf("%u",&pwm1); - break; - case 2: - scanf("%u",&pwm2); - break; - case 3: - scanf("%u",&pwm3); - break; - case 4: - scanf("%u",&test); - break; - - default: - break; - } - - } return 0; }