#include "rpin.h" /*gpclk*/
#include "rp_spi.h" /*spi*/
#include "misc.h" /*structure for priorities*/
+#include "pxmc_sin_fixed.h" /*to test sin commutation */
+
#define PRUM_PROUD 2061
#define THREAD_SHARED 0
#define INIT_VALUE 0 /*init value for semaphor*/
+
+#define PXMC_SIN_FIX_TAB_BITS 9
+#define PXMC_SIN_FIX_IDX_SLR 23
+#define PXMC_SIN_FIX_XD_MASK 0x007fffff
+#define PXMC_SIN_FIX_XD_SLR 8
+#define PXMC_SIN_FIX_A_MASK 0xffffc000
+#define PXMC_SIN_FIX_B_SLL 19
+#define PXMC_SIN_FIX_B_SAR 16
+#define PXMC_SIN_FIX_B_XD_SAR 6
+#define PXMC_SIN_FIX_ZIC_MASK 0x00002000
+#define PXMC_SIN_FIX_ZIC_BIT 13
+
+#define PXMC_SIN_FIX_PI2 0x40000000
+#define PXMC_SIN_FIX_2PI3 0x55555555
+
struct sigaction sighnd; /*struktura pro signal handler*/
struct rpi_in data;
struct rpi_state{
uint8_t test;
uint16_t pwm1, pwm2, pwm3;
+ uint16_t t_pwm1, t_pwm2, t_pwm3;
char commutate;
int duty; /* duty cycle of pwm */
uint16_t index_dist; /* distance to index position */
printf("raw_pozice=%d\n",(int32_t)data_p.pozice_raw);
printf("raw_pozice last12=%u\n",(data_p.pozice_raw&0x0FFF));
printf("index position=%u\n",data_p.index_position);
- printf("distance to index=%u\n",s.index_dist);
printf("hal1=%d, hal2=%d, hal3=%d\n",data_p.hal1,data_p.hal2,data_p.hal3);
printf("en1=%d, en2=%d, en3=%d (Last sent)\n",!!(0x40&s.test),!!(0x20&s.test),!!(0x10&s.test));
printf("shdn1=%d, shdn2=%d, shdn3=%d (L.s.)\n",!!(0x08&s.test),!!(0x04&s.test),!!(0x02&s.test));
printf("PWM1=%u(L.s.)\n",s.pwm1);
printf("PWM2=%u(L.s.)\n",s.pwm2);
printf("PWM3=%u(L.s.)\n",s.pwm3);
+ printf("distance to index=%u\n",s.index_dist);
+ printf("T_PWM1=%u T_PWM2=%u T_PWM3=%u\n",s.t_pwm1,s.t_pwm2, s.t_pwm3);
printf("Pocet namerenych proudu=%u\n",data_p.adc_m_count);
printf("(pwm1) (ch1)=%d (avg=%4.0f) (%2.2f%%)\n",data_p.ch1,cur1,diff_p(cur1));
printf("(pwm2) (ch2)=%d (avg=%4.0f)(%2.2f%%)\n",data_p.ch2,cur2,diff_p(cur2));
uint16_t tmp;
/* keep the cap*/
+
if (rps.pwm1>2047) rps.pwm1=2047;
if (rps.pwm2>2047) rps.pwm2=2047;
if (rps.pwm3>2047) rps.pwm3=2047;
}
return (void*)0;
}
+
+inline
+int sin_commutator(int duty){
+ #define DEGREE_60 715827883
+ #define DEGREE_120 1431655765
+ #define DEGREE_180 2147483648
+ #define DEGREE_240 2863311531
+ #define DEGREE_300 3579139413
+ uint32_t j,pos;
+ int32_t sin;
+ uint16_t pwm;
+ pos=rps.index_dist*4294967;
+ if (duty>=0){ /*clockwise rotation*/
+ /* 1st phase */
+ sin = pxmc_sin_fixed_inline(pos+DEGREE_240,10); /*10+1 bity*/ /*-120*/
+ pwm=0;
+ for(j=0;j!=11;j++){
+ /* multiplicate as if maximum sinus value was unity */
+ pwm+=(!(sin & 0x10000000))*(((1 << j) & sin)>>j)*(duty>>(10-j));
+ }
+ rps.pwm1=pwm;
+
+ /* 2nd phase */
+ sin = pxmc_sin_fixed_inline(pos+DEGREE_120,10); /*10+1 bity*/ /*-240*/
+ pwm=0;
+ for(j=0;j!=11;j++){
+ /* multiplicate as if maximum sinus value was unity */
+ pwm+=(!(sin & 0x10000000))*(((1 << j) & sin)>>j)*(duty>>(10-j));
+ }
+ rps.pwm2=pwm;
+
+ /* 3rd phase */
+ sin = pxmc_sin_fixed_inline(pos,10); /*10+1 bity*/
+ pwm=0;
+ for(j=0;j!=11;j++){
+ /* multiplicate as if maximum sinus value was unity */
+ pwm+=(!(sin & 0x10000000))*(((1 << j) & sin)>>j)*(duty>>(10-j));
+ }
+ rps.pwm3=pwm;
+ }else{
+ duty=-duty;
+
+ /* 1st phase */
+ sin = pxmc_sin_fixed_inline(pos+DEGREE_60,10); /*10+1 bity*/ /*-300*/
+ pwm=0;
+ for(j=0;j!=11;j++){
+ /* multiplicate as if maximum sinus value was unity */
+ pwm+=(!(sin & 0x10000000))*(((1 << j) & sin)>>j)*(duty>>(10-j));
+ }
+ rps.pwm1=pwm;
+
+ /* 2nd phase */
+ sin = pxmc_sin_fixed_inline(pos+DEGREE_300,10); /*10+1 bity*/ /*-60-*/
+ pwm=0;
+ for(j=0;j!=11;j++){
+ /* multiplicate as if maximum sinus value was unity */
+ pwm+=(!(sin & 0x10000000))*(((1 << j) & sin)>>j)*(duty>>(10-j));
+ }
+ rps.pwm2=pwm;
+
+ /* 3rd phase */
+ sin = pxmc_sin_fixed_inline(pos+DEGREE_180,10); /*10+1 bity*/ /*-180*/
+ pwm=0;
+ for(j=0;j!=11;j++){
+ /* multiplicate as if maximum sinus value was unity */
+ pwm+=(!(sin & 0x10000000))*(((1 << j) & sin)>>j)*(duty>>(10-j));
+ }
+ rps.pwm3=pwm;
+ }
+ return 0;
+}
/*
* \brief
* Test function to be placed in controll loop.
* Switches PWM's at point where they produce same force.
* This points are found thanks to IRC position,
*/
-inline void simple_ind_dist_commutator(int duty){
+inline
+void simple_ind_dist_commutator(int duty){
if (duty>=0){ /* clockwise - so that position increase */
/* pwm3 */
if ((rps.index_dist>=45 && rps.index_dist<=373) ||
}
pid();
if (rps.index_ok && rps.commutate){
- simple_ind_dist_commutator(rps.duty);
+ /*simple_ind_dist_commutator(rps.duty);*/
+ sin_commutator(rps.duty);
}else if(!rps.index_ok && rps.commutate){
simple_hall_commutator(rps.duty);
}
--- /dev/null
+/*******************************************************************
+ Components for embedded applications builded for
+ laboratory and medical instruments firmware
+
+ pxmc_sin_fixed.h - generic multi axis motion controller
+ fixed sine approximation
+
+ (C) 2001-2015 by Pavel Pisa pisa@cmp.felk.cvut.cz
+ (C) 2002-2015 by PiKRON Ltd. http://www.pikron.com
+
+ This file can be used and copied according to next
+ license alternatives
+ - GPL - GNU Public License
+ - other license provided by project originators
+
+ *******************************************************************/
+
+#include <stdint.h>
+
+#define PXMC_SIN_FIX_TAB_BITS 9
+#define PXMC_SIN_FIX_IDX_SLR 23
+#define PXMC_SIN_FIX_XD_MASK 0x007fffff
+#define PXMC_SIN_FIX_XD_SLR 8
+#define PXMC_SIN_FIX_A_MASK 0xffffc000
+#define PXMC_SIN_FIX_B_SLL 19
+#define PXMC_SIN_FIX_B_SAR 16
+#define PXMC_SIN_FIX_B_XD_SAR 6
+#define PXMC_SIN_FIX_ZIC_MASK 0x00002000
+#define PXMC_SIN_FIX_ZIC_BIT 13
+
+#define PXMC_SIN_FIX_PI2 0x40000000
+#define PXMC_SIN_FIX_2PI3 0x55555555
+
+extern const uint32_t pxmc_sin_fixed_table[];
+
+/**
+ * pxmc_sin_fixed_inline - fixed math sine computation
+ * @x: The argument value - 2^32 corresponds to 2 * PI or 360 deg
+ * @res_unit_bits: number of fraction bits of the result (default 16)
+ *
+ * Returns sine of value @x argument where unit is 2^16, i.e.
+ * return value 0x10000 is equivalent to +1.0 and -0x10000 to -1.0
+ */
+static inline
+int32_t pxmc_sin_fixed_inline(uint32_t x, int res_unit_bits)
+{
+ uint32_t tabval;
+ unsigned int ti;
+ int32_t a;
+ int b, xd;
+ int32_t yl;
+
+ if (!res_unit_bits)
+ res_unit_bits = 16;
+
+ ti = x >> PXMC_SIN_FIX_IDX_SLR;
+ xd = (x & PXMC_SIN_FIX_XD_MASK) >> PXMC_SIN_FIX_XD_SLR;
+
+ tabval = pxmc_sin_fixed_table[ti];
+ a = tabval & PXMC_SIN_FIX_A_MASK;
+ b = (int32_t)(tabval << PXMC_SIN_FIX_B_SLL) >> PXMC_SIN_FIX_B_SAR;
+
+ yl = a;
+ yl += ((int32_t)b * xd) >> PXMC_SIN_FIX_B_XD_SAR;
+
+ yl += yl & (1 << (29 - res_unit_bits));
+ yl >>= 30 - res_unit_bits;
+
+ return yl;
+}
+
+/**
+ * pxmc_sincos_fixed_inline - fixed math sine computation
+ * @pysin: pointer to location where sine value is returned
+ * @pycos: pointer to location where cosine value is returned
+ * @x: The argument value - 2^32 corresponds to 2 * PI or 360 deg
+ * @res_unit_bits: number of fraction bits of the result (default 16)
+ *
+ * Returns sine and cosine of value @x argument where unit is 2^16, i.e.
+ * return value 0x10000 is equivalent to +1.0 and -0x10000 to -1.0
+ */
+static inline
+void pxmc_sincos_fixed_inline(int32_t *pysin, int32_t *pycos,
+ uint32_t x, int res_unit_bits)
+{
+ uint32_t tabval;
+ unsigned int ti;
+ int32_t a;
+ int b, xd;
+ int32_t ysin;
+ int32_t ycos;
+
+ if (!res_unit_bits)
+ res_unit_bits = 16;
+
+ ti = x >> PXMC_SIN_FIX_IDX_SLR;
+ xd = (x & PXMC_SIN_FIX_XD_MASK) >> PXMC_SIN_FIX_XD_SLR;
+
+ tabval = pxmc_sin_fixed_table[ti];
+ a = tabval & PXMC_SIN_FIX_A_MASK;
+ b = (int32_t)(tabval << PXMC_SIN_FIX_B_SLL) >> PXMC_SIN_FIX_B_SAR;
+
+ ysin = a;
+ ysin += ((int32_t)b * xd) >> PXMC_SIN_FIX_B_XD_SAR;
+
+ ysin += ysin & (1 << (29 - res_unit_bits));
+ ysin >>= 30 - res_unit_bits;
+
+ ti = (uint32_t)(x + PXMC_SIN_FIX_PI2) >> PXMC_SIN_FIX_IDX_SLR;
+
+ tabval = pxmc_sin_fixed_table[ti];
+ a = tabval & PXMC_SIN_FIX_A_MASK;
+ b = (int32_t)(tabval << PXMC_SIN_FIX_B_SLL) >> PXMC_SIN_FIX_B_SAR;
+
+ ycos = a;
+ ycos += ((int32_t)b * xd) >> PXMC_SIN_FIX_B_XD_SAR;
+
+ ycos += ycos & (1 << (29 - res_unit_bits));
+ ycos >>= 30 - res_unit_bits;
+
+ *pysin = ysin;
+ *pycos = ycos;
+}
+
+/**
+ * pxmc_sin_fixed_zic_inline - indicate range for +/-10 degree zero current correction
+ * @x: The argument value - 2^32 corresponds to 2 * PI or 360 deg
+ *
+ * Returns value one if input is in the 0 or PI +/- 10 deg range.
+ */
+static inline
+int pxmc_sin_fixed_zic_inline(uint32_t x)
+{
+ unsigned int ti;
+
+ ti = x >> PXMC_SIN_FIX_IDX_SLR;
+
+ return (pxmc_sin_fixed_table[ti] >> PXMC_SIN_FIX_ZIC_BIT) & 1;
+}