Modified program exit behavior.

diff --git a/pmsm-control/test_sw/cmd_proc.c b/pmsm-control/test_sw/cmd_proc.c
index 46ff07009a9a81638787ac206f7e12b78bddf29f..35ed9c29f54bbf0bec7acd1438959416aa8fb990 100644 (file)
--- 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 4ae245b099f497af63c331fea9c32431b20852bb..8b65c74d3e74254a67c2260d7ceffb41344455a9 100644 (file)
--- a/pmsm-control/test_sw/main_pmsm.c
+++ b/pmsm-control/test_sw/main_pmsm.c
@@ -16,6 +16,7 @@
 #include <unistd.h>    /*usleep*/
 #include <pthread.h>   /*threads*/
 #include <time.h>      /*nanosleep*/
+#include <string.h>
 
 #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 906e29c7ca4ea1bd12c26300e31c38a69be61223..046363047a4838da93fe170b3f6e0d7751e78767 100644 (file)
--- 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 b50b25010aeb1edbbd46ba520b96904f48928482..e028201678e7a3c512f52bd338052762227d8318 100644 (file)
--- 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 7be937398f107cf84116e5ecaf5fde88d95ba11f..28de88db6302f5f4ddbae997566a64993a664f15 100644 (file)
--- 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 */