X-Git-Url: https://rtime.felk.cvut.cz/gitweb/fpga/rpi-motor-control.git/blobdiff_plain/1a8d767b097d1536b264d8bae3e9d5c16617f835..9650ac29cdffe74d39e64da32a34384dcd9db72f:/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 4ae245b..8b65c74 100644
--- 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 */