From: Martin Prudek Date: Sat, 16 May 2015 12:03:04 +0000 (+0200) Subject: Modified program exit behavior. X-Git-Url: https://rtime.felk.cvut.cz/gitweb/fpga/rpi-motor-control.git/commitdiff_plain/9650ac29cdffe74d39e64da32a34384dcd9db72f Modified program exit behavior. --- diff --git a/pmsm-control/test_sw/cmd_proc.c b/pmsm-control/test_sw/cmd_proc.c index 46ff070..35ed9c2 100644 --- a/pmsm-control/test_sw/cmd_proc.c +++ b/pmsm-control/test_sw/cmd_proc.c @@ -16,8 +16,8 @@ static char doPrint = 1; static void printHelp(){ doPrint=0; puts("start - Pripravi rizeni, zapne enable bity pwm."); - puts("stop - Vypne pwm a rizeni. Enable bity na nulu."); - puts("0 - Vypne pwm a rizeni. Enable bity na nulu."); + puts("stop - Vypne komutaci, pwm a rizeni."); + puts("0 - Vypne komutaci, pwm a rizeni."); puts("ga:[hodnota] - Zapne rizeni na zvolenou absolutni pozici."); puts("print - Zapne nebo vypne pravidelne vypisovani hodnot."); puts("duty:[hodnota] - Nastavi pevnou sirku plneni."); @@ -58,7 +58,6 @@ static void stop(struct rpi_state* state){ 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; @@ -86,6 +85,7 @@ static void dutySet(struct rpi_state* state, int duty){ static void goAbsolute(struct rpi_state* state, int pos){ sem_wait(&state->thd_par_sem); state->pos_reg_ena=1; + state->commutate=1; state->desired_pos=pos; sem_post(&state->thd_par_sem); } @@ -168,12 +168,14 @@ void printData(struct rpi_state* state){ 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; @@ -185,7 +187,8 @@ void printData(struct rpi_state* state){ printf("%.2X ", data_p.debug_rx[i]); } puts(""); - printf("\npozice=%d\n",(int32_t)data_p.pozice); + printf("\npozice=%u\n",data_p.pozice); + printf("rychlost=%ld\n",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); diff --git a/pmsm-control/test_sw/main_pmsm.c b/pmsm-control/test_sw/main_pmsm.c index 4ae245b..8b65c74 100644 --- a/pmsm-control/test_sw/main_pmsm.c +++ b/pmsm-control/test_sw/main_pmsm.c @@ -16,6 +16,7 @@ #include /*usleep*/ #include /*threads*/ #include /*nanosleep*/ +#include #include "rpin.h" /*gpclk*/ #include "rp_spi.h" /*spi*/ @@ -26,6 +27,7 @@ #define MAX_DUTY 170 #define PID_P 0.3 +#define PID_P_S 0.3 #define PRIOR_KERN 50 #define PRIOR_HIGH 49 @@ -90,11 +92,86 @@ int32_t min(int32_t x, int32_t y, int32_t z){ return x; } +/* + * \brief + * Pripravi psi buffer + */ +void prepare_tx(uint8_t * tx){ + + /*Data format: + * tx[4] - bity 95 downto 88 - bits that are sent first + * tx[5] - bity 87 downto 80 + * tx[6] - bity 79 downto 72 + * tx[7] - bity 71 downto 64 + * tx[8] - bity 63 downto 56 + * tx[9] - bity 55 downto 48 + * tx[10] - bity 47 downto 40 + * tx[11] - bity 39 downto 32 + * tx[12] - bity 31 downto 24 + * tx[13] - bity 23 downto 16 + * tx[14] - bity 15 downto 8 + * tx[15] - 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 + * . + * . + * Unused + * . + * . + * bits 47 .. 32 - match PWM1 + * bits 31 .. 16 - match PWM2 + * bits 15 .. 0 - match PWM3 + */ + + + uint16_t tmp; + + /* keep the 11-bit cap*/ + + if (rps.pwm1>2047) rps.pwm1=2047; + if (rps.pwm2>2047) rps.pwm2=2047; + if (rps.pwm3>2047) rps.pwm3=2047; + + tx[0]=rps.test; /*bit 94 - enable PWM1*/ + + /*now we have to switch the bytes due to endianess */ + /* ARMv6 & ARMv7 instructions are little endian */ + /*pwm1*/ + tx[10]=((uint8_t*)&rps.pwm1)[1]; /*MSB*/ + tx[11]=((uint8_t*)&rps.pwm1)[0]; /*LSB*/ + + /*pwm2*/ + tx[12]=((uint8_t*)&rps.pwm2)[1]; /*MSB*/ + tx[13]=((uint8_t*)&rps.pwm2)[0]; /*LSB*/ + + /*pwm3*/ + tx[14]=((uint8_t*)&rps.pwm3)[1]; /*MSB*/ + tx[15]=((uint8_t*)&rps.pwm3)[0]; /*LSB*/ + + +} + /** * \brief Signal handler pro Ctrl+C */ 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; + prepare_tx(tx); /*save the data to send*/ + data=spi_read(tx); + spi_disable(); clk_disable(); /*muzeme zavrit semafor*/ @@ -200,66 +277,7 @@ void inv_trans_comm_2(int duty){ rps.pwm2=(uint16_t)u2; rps.pwm3=(uint16_t)u3; } -void prepare_tx(uint8_t * tx){ - /*Data format: - * tx[4] - bity 95 downto 88 - bits that are sent first - * tx[5] - bity 87 downto 80 - * tx[6] - bity 79 downto 72 - * tx[7] - bity 71 downto 64 - * tx[8] - bity 63 downto 56 - * tx[9] - bity 55 downto 48 - * tx[10] - bity 47 downto 40 - * tx[11] - bity 39 downto 32 - * tx[12] - bity 31 downto 24 - * tx[13] - bity 23 downto 16 - * tx[14] - bity 15 downto 8 - * tx[15] - 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 - * . - * . - * Unused - * . - * . - * bits 47 .. 32 - match PWM1 - * bits 31 .. 16 - match PWM2 - * bits 15 .. 0 - match PWM3 - */ - - - uint16_t tmp; - - /* keep the 11-bit cap*/ - - if (rps.pwm1>2047) rps.pwm1=2047; - if (rps.pwm2>2047) rps.pwm2=2047; - if (rps.pwm3>2047) rps.pwm3=2047; - - tx[0]=rps.test; /*bit 94 - enable PWM1*/ - - /*now we have to switch the bytes due to endianess */ - /* ARMv6 & ARMv7 instructions are little endian */ - /*pwm1*/ - tx[10]=((uint8_t*)&rps.pwm1)[1]; /*MSB*/ - tx[11]=((uint8_t*)&rps.pwm1)[0]; /*LSB*/ - - /*pwm2*/ - tx[12]=((uint8_t*)&rps.pwm2)[1]; /*MSB*/ - tx[13]=((uint8_t*)&rps.pwm2)[0]; /*LSB*/ - - /*pwm3*/ - tx[14]=((uint8_t*)&rps.pwm3)[1]; /*MSB*/ - tx[15]=((uint8_t*)&rps.pwm3)[0]; /*LSB*/ - - -} /** * Funkce pravidelne vypisuje posledni zjistenou pozici lokalniho motoru */ @@ -513,6 +531,24 @@ inline void pos_pid(){ rps.duty = duty_tmp; } } +/* + * \brief + * Very simple PID regulator. + * Now only with P-part so that the error doesnt go to zero. + * FIXME: make better + */ +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; + }else if (duty_tmp<-MAX_DUTY){ + rps.duty=-MAX_DUTY; + }else{ + rps.duty = duty_tmp; + } +} /* * \brief * Feedback loop. @@ -534,6 +570,8 @@ void * read_data(void* param){ /* wait until next shot */ 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*/ prepare_tx(tx); /*save the data to send*/ data = spi_read(tx); /*exchange data*/ /*subtract initiate postion */ diff --git a/pmsm-control/test_sw/misc.c b/pmsm-control/test_sw/misc.c index 906e29c..0463630 100644 --- a/pmsm-control/test_sw/misc.c +++ b/pmsm-control/test_sw/misc.c @@ -25,8 +25,7 @@ * Signal handler. */ void appl_sig_handler(int sig){ - appl_stop(); - exit(1); + exit(0); /*atexit is set*/ } /* diff --git a/pmsm-control/test_sw/pmsm_state.h b/pmsm-control/test_sw/pmsm_state.h index b50b250..e028201 100644 --- a/pmsm-control/test_sw/pmsm_state.h +++ b/pmsm-control/test_sw/pmsm_state.h @@ -20,6 +20,7 @@ struct rpi_state{ int duty; /* duty cycle of pwm */ int desired_pos; /* desired position */ + int old_pos; /* one cycle old position */ int desired_spd; /* desired speed */ char commutate; /* zapina prepocet duty na jednotlive pwm */ diff --git a/pmsm-control/test_sw/rp_spi.h b/pmsm-control/test_sw/rp_spi.h index 7be9373..28de88d 100644 --- a/pmsm-control/test_sw/rp_spi.h +++ b/pmsm-control/test_sw/rp_spi.h @@ -15,7 +15,6 @@ struct rpi_in{ int8_t hal1,hal2,hal3; /* bool values */ int8_t en1, en2, en3; /*(bool)last read pwm-enable values - !they are changed after reading ! */ int8_t shdn1,shdn2,shdn3; /*(bool)last read shutdown values - !they are changed after reading ! */ - int8_t b54, b53, b52, b51, b50, b49, b48, b47, b46, b45, b44, b43, b42, b41, b40, b39, b38, b37, b36; /*bits for debug*/ uint16_t adc_m_count; /*current measurments count*/ uint8_t debug_rx[16]; uint16_t index_position; /* raw position of irc_i */