Preston Ernst
-data logging revision
Revision 2:92c25cb669f4, committed 2021-08-24
- Comitter:
- ernstpre
- Date:
- Tue Aug 24 08:51:13 2021 +0000
- Parent:
- 1:25a2b47ca291
- Commit message:
- Publish Commit 24/8/21
Changed in this revision
diff -r 25a2b47ca291 -r 92c25cb669f4 ControllerLoop.cpp
--- a/ControllerLoop.cpp Thu Aug 05 08:27:51 2021 +0000
+++ b/ControllerLoop.cpp Tue Aug 24 08:51:13 2021 +0000
@@ -11,101 +11,137 @@
ti.reset();
ti.start();
data.laser_on = false;
- }
+}
// decontructor for controller loop
ControllerLoop::~ControllerLoop() {}
// ----------------------------------------------------------------------------
// this is the main loop called every Ts with high priority
-void ControllerLoop::loop(void){
- float w01=2*3.1415927 * 8;
+void ControllerLoop::loop(void)
+{
+ float w01=2*3.1415927 * 2;
float xy[2];
float exc = 0;
PID_Cntrl v_cntrl_1(0.0153f, 3.06,0,0,Ts,-0.8,0.8);
PID_Cntrl v_cntrl_2(0.0153f, 3.06,0,0,Ts,-0.8,0.8);
- while(1)
- {
+
+ bool stop_rec = false;
+ int k=0;
+ float Logg[2000][4]; //float datal[2000][6];
+ //int vel1 = 5;
+ //int vel2 =10;
+ printf("Starting Controller \r\n");
+ while(1) {
ThisThread::flags_wait_any(threadFlag);
// THE LOOP ------------------------------------------------------------
short c1 = counter1 - index1.positionAtIndexPulse - mk.inc_offset[0]- mk.inc_additional_offset[0]; // get counts from Encoder
short c2 = counter2 - index2.positionAtIndexPulse - mk.inc_offset[1]- mk.inc_additional_offset[1]; // get counts from Encoder
data.sens_phi[0] = uw2pi1(2.0f*3.1415927f/4000.0f*(float)c1);
- data.sens_Vphi[0] = diff1(c1); // motor velocity
+ data.sens_Vphi[0] = diff1(c1); // motor velocity
data.sens_phi[1] = uw2pi2(2.0f*3.1415927f/4000.0f*(float)c2);
data.sens_Vphi[1] = diff2(c2); // motor velocity
// -------------------------------------------------------------
// at very beginning: move system slowly to find the zero pulse
// set "if(0)" if you like to ommit at beginning
- if(!is_initialized)
- {
+ if(!is_initialized) {
find_index();
- if(index1.positionAtIndexPulse != 0 && index2.positionAtIndexPulse != 0)
+ if(index1.positionAtIndexPulse != 0 && index2.positionAtIndexPulse != 0)
is_initialized=true;
+ } else {
+ if(k==0)
+ {
+ printf("Starting else loop \r\n");
}
- else
- {
// float Kp = 0.005;
// data.i_des[0] = 0.1f + Kp*(exc+50.0f - data.sens_Vphi[0]);
// ------------------------ do the control first
- // calculate desired currents here, you can do "anything" here,
+ // calculate desired currents here, you can do "anything" here,
// if you like to refer to values e.g. from the gui or from the trafo,
// please use data.xxx values, they are calculated 30 lines below
- //float e1 = 50 - data.sens_Vphi[0];
- //float e2 = 50 - data.sens_Vphi[1];
- //float v_des1 = exc;
- //float v_des2 = 0;
- float phi1_des = 0.025f*sinf(2.0f* 3.14159f*2.0f*ti.read());
- float phi2_des = 0.025f*cosf(2.0f* 3.14159f*2.0f*ti.read());
- float Kv = 123;
- float v_des1 = Kv*(phi1_des - data.sens_phi[0]);
- float v_des2 = Kv*(phi2_des - data.sens_phi[1]);
- data.i_des[0] = v_cntrl_1(v_des1 - data.sens_Vphi[0]);
- data.i_des[1] = v_cntrl_2(v_des2 - data.sens_Vphi[1]);
-
- //data.i_des[1] =0.0;
-
+ //float e1 = 50 - data.sens_Vphi[0];
+ //float e2 = 50 - data.sens_Vphi[1];
+ //float v_des1 = exc;
+ //float v_des2 = 0;
+ float phi1_des = 0.3f*sinf(2.0f* 3.14159f*2.0f*ti.read());
+ float phi2_des = 0.3f*cosf(2.0f* 3.14159f*2.0f*ti.read());
+ float Kv = 123;
+ float v_des1 = Kv*(phi1_des - data.sens_phi[0]);
+ float v_des2 = Kv*(phi2_des - data.sens_phi[1]);
+ data.i_des[0] = v_cntrl_1(v_des1 - data.sens_Vphi[0]);
+ data.i_des[1] = v_cntrl_2(v_des2 - data.sens_Vphi[1]);
+
+ //data.i_des[1] =0.0;
+
// ------------------------ write outputs
i_des1.write(i2u(data.i_des[0]));
i_des2.write(i2u(data.i_des[1]));
// GPA: if you want to use the GPA, uncomment and improve following line:
//exc = myGPA(data.i_des[0],data.sens_Vphi[0]);
exc = myGPA(v_des1, data.sens_phi[0]);
-
+ //
+
+ /*if(k%10000==0) {
+ printf("yes \n");
+ //printf("c1: %d c2: %d i2: %f\r\n",counts1,counts2,i2);
+ //printf("p1: %f p2: %f pd1: %f pd2: %f id1: %f id2: %f\r\n",data.sens_phi[0],data.sens_phi[1],phi1_des,phi2_des,data.i_des[0],data.i_des[1]);
+ }*/
+
+ if(k==200 && !stop_rec)
+ {
+ stop_rec = true;
+ k=0;
+
+ for(int k1=0; k1<2000; k1++)
+ {
+ for(int k2=0; k2<4; k2++)
+ {
+ //printf("k1 = %d k2 = %d \r\n", k1, k2);
+ printf("%3.4f ",Logg[k1][k2]);
+ }
+ printf("\r\n");
+ }
+ }
+
+ if(k<2000 && !stop_rec)
+ {
+ Logg[k][0]=data.sens_phi[0];
+ Logg[k][1]=data.sens_phi[1];
+ Logg[k][2]=data.i_des[0];
+ Logg[k][3]=data.i_des[1];
+ }
+ k++;
+ //
+
// now do trafos etc
- if(mk.external_control) // get desired values from external source (GUI)
- {
+ if(mk.external_control) { // get desired values from external source (GUI)
if(mk.trafo_is_on) // use desired xy values from xternal source and transform
- // otherwise external source delivers phi1, phi2 values directly
- {
+ // otherwise external source delivers phi1, phi2 values directly
+ {
bool dum = mk.X2P(data.cntrl_xy_des,data.cntrl_phi_des);
- }
}
- else // this is called, when desired values are calculated here internally (e.g. pathplanner)
- {
- if(mk.trafo_is_on)
- {
+ } else { // this is called, when desired values are calculated here internally (e.g. pathplanner)
+ if(mk.trafo_is_on) {
data.cntrl_xy_des[0] = 30.0f*cosf(w01*glob_ti.read()); // make a circle in xy-co-ordinates
data.cntrl_xy_des[1] = 30.0f*sinf(w01*glob_ti.read());
bool dum = mk.X2P(data.cntrl_xy_des,data.cntrl_phi_des);
- }
- else
- {
+ } else {
data.cntrl_phi_des[0] = .250f*cosf(w01*glob_ti.read()); // make some harmonic movements directly on phi1/phi2
data.cntrl_phi_des[1] = .250f*sinf(w01*glob_ti.read());
- }
}
+ }
bool dum = mk.P2X(data.sens_phi,data.est_xy); // calculate actual xy-values, uncomment this if there are timing issues
//current_path->get_x_v(glob_ti.read(),&phi_des,&v_des);
- } // else(..)
+ } // else(..)
laser_on = data.laser_on;
i_enable = big_button;
- }// endof the main loop
+ }// endof the main loop
}
-void ControllerLoop::sendSignal() {
+void ControllerLoop::sendSignal()
+{
thread.flags_set(threadFlag);
}
void ControllerLoop::start_loop(void)
@@ -116,16 +152,16 @@
float ControllerLoop::pos_cntrl(float d_phi)
{
-
- // write position controller here
- return 0.0;
- }
+
+ // write position controller here
+ return 0.0;
+}
void ControllerLoop::init_controllers(void)
{
// set values for your velocity and position controller here!
-
-
+
+
}
// find_index: move axis slowly to detect the zero-pulse
void ControllerLoop::find_index(void)
@@ -136,4 +172,4 @@
float i2 = 0.2f + Kp*(50.0f - data.sens_Vphi[1]) ;
i_des1.write(i2u(i1));
i_des2.write(i2u(i2));
- }
\ No newline at end of file
+}
\ No newline at end of file
diff -r 25a2b47ca291 -r 92c25cb669f4 main.cpp
--- a/main.cpp Thu Aug 05 08:27:51 2021 +0000
+++ b/main.cpp Tue Aug 24 08:51:13 2021 +0000
@@ -24,6 +24,8 @@
float Ts=.0002f; // sampling time
void pressed(void);
void released(void);
+
+
//------------- DEFINE FILTERS ----------------
// missing
//------------- Define In/Out -----------------
@@ -90,12 +92,13 @@
glob_ti.start();
glob_ti.reset();
loop.init_controllers();
- uart_com.start_uart();
+ //uart_com.start_uart(); // communication with MATLAB
+ printf("hello world \n");
loop.start_loop();
i_des1.write(i2u(0));
i_des2.write(i2u(0));
ThisThread::sleep_for(200);
- uart_com.send_text((char *)"Start Mirroractuator 1.1");
+ //uart_com.send_text((char *)"Start Mirroractuator 1.1"); // communciation with MATLAB
/* p1.initialize(300,10,A,0,0,0);
p2.initialize(300,10,-A,0,0,A);*/
laser_on = 0;
@@ -120,4 +123,19 @@
{
ThisThread::sleep_for(200);
}
+ // *** create CSV file of data
+
+ /*FILE *f = fopen ("test.csv", "a");
+
+ // inform user
+ if (!f) {
+ printf ("failed\n");
+ } else {
+ printf ("success\n");
+ }
+
+ fprintf (f, " test\n");
+ fclose (f);
+ return 0;*/
+
} // END OF main
diff -r 25a2b47ca291 -r 92c25cb669f4 sw_fifo.cpp
--- a/sw_fifo.cpp Thu Aug 05 08:27:51 2021 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,285 +0,0 @@
-////////////////////////////////////////////////////////////////////////////////////////
-/* enter necessary header files for proper interrupt vector and UART/USART visibility */
-////////////////////////////////////////////////////////////////////////////////////////
-
-#include <sw_fifo.h>
-#include "uart_comm_thread.h"
-
-typedef struct {
- uint8_t data_buf[FIFO_BUFFER_SIZE]; // FIFO buffer
- uint16_t i_first; // index of oldest data byte in buffer
- uint16_t i_last; // index of newest data byte in buffer
- uint16_t num_bytes; // number of bytes currently in buffer
-}sw_fifo_typedef;
-
-sw_fifo_typedef rx_fifo = { {0}, 0, 0, 0 }; // declare a receive software buffer
-sw_fifo_typedef tx_fifo = { {0}, 0, 0, 0 }; // declare a transmit software buffer
-
-
-/***************************************************************************************************************/
-// UART receive interrupt sub-routine
-// - interrupts when valid data exists in rx hardware buffer
-// - checks if there's room in the rx software buffer
-// - if there's room, it transfers the received data into the sw buffer
-// - automatically handles "uart_rx_buffer_full_flag"
-// - sets overflow flag upon software buffer overflow (doesn't overwrite existing data)
-//////////////////////////////////////////////
-/* enter name of UART RX IRQ Handler here */ {
-//////////////////////////////////////////////
-
- /* Explicitly clear the source of interrupt if necessary */
-
- if(rx_fifo.num_bytes == FIFO_BUFFER_SIZE) { // if the sw buffer is full
- uart_rx_fifo_ovf_flag = 1; // set the overflow flag
- }else if(rx_fifo.num_bytes < FIFO_BUFFER_SIZE) { // if there's room in the sw buffer
-
- ///////////////////////////////////////////////////
- /* read error/status reg here if desired */
- /* handle any hardware RX errors here if desired */
- ///////////////////////////////////////////////////
-
- /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
- rx_fifo.data_buf[rx_fifo.i_last] = /* enter pointer to UART rx hardware buffer here */ // store the received data as the newest data element in the sw buffer
- /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-
- rx_fifo.i_last++; // increment the index of the most recently added element
- rx_fifo.num_bytes++; // increment the bytes counter
- }
- if(rx_fifo.num_bytes == FIFO_BUFFER_SIZE) { // if sw buffer just filled up
- uart_rx_fifo_full_flag = 1; // set the RX FIFO full flag
- }
- if(rx_fifo.i_last == FIFO_BUFFER_SIZE) { // if the index has reached the end of the buffer,
- rx_fifo.i_last = 0; // roll over the index counter
- }
- uart_rx_fifo_not_empty_flag = 1; // set received-data flag
-} // end UART RX IRQ handler
-/***************************************************************************************************************/
-
-
-/***************************************************************************************************************/
-// UART transmit interrupt sub-routine
-// - interrupts when the tx hardware buffer is empty
-// - checks if data exists in the tx software buffer
-// - if data exists, it places the oldest element of the sw buffer into the tx hardware buffer
-// - if the sw buffer is emptied, it disables the "hw buffer empty" interrupt
-// - automatically handles "uart_tx_buffer_full_flag"
-//////////////////////////////////////////////
-/* enter name of UART TX IRQ Handler here */ {
-//////////////////////////////////////////////
-
- /* Explicitly clear the source of interrupt if necessary */
-
- if(tx_fifo.num_bytes == FIFO_BUFFER_SIZE) { // if the sw buffer is full
- uart_tx_fifo_full_flag = 0; // clear the buffer full flag because we are about to make room
- }
- if(tx_fifo.num_bytes > 0) { // if data exists in the sw buffer
-
- ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
- /* enter pointer to UART tx hardware buffer here */ = tx_fifo.data_buf[tx_fifo.i_first]; // place oldest data element in the TX hardware buffer
- ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-
- tx_fifo.i_first++; // increment the index of the oldest element
- tx_fifo.num_bytes--; // decrement the bytes counter
- }
- if(tx_fifo.i_first == FIFO_BUFFER_SIZE) { // if the index has reached the end of the buffer,
- tx_fifo.i_first = 0; // roll over the index counter
- }
- if(tx_fifo.num_bytes == 0) { // if no more data exists
-
- uart_tx_fifo_not_empty_flag = 0; // clear flag
-
- //////////////////////////////////////////////////////////////////////////
- /* disable UART "TX hw buffer empty" interrupt here */
- /* if using shared RX/TX hardware buffer, enable RX data interrupt here */
- //////////////////////////////////////////////////////////////////////////
-
- }
-}// end UART TX IRQ handler
-/***************************************************************************************************************/
-
-
-/***************************************************************************************************************/
-// UART data transmit function
-// - checks if there's room in the transmit sw buffer
-// - if there's room, it transfers data byte to sw buffer
-// - automatically handles "uart_tx_buffer_full_flag"
-// - sets the overflow flag upon software buffer overflow (doesn't overwrite existing data)
-// - if this is the first data byte in the buffer, it enables the "hw buffer empty" interrupt
-void uart_send_byte(uint8_t byte) {
-
- ///////////////////////////////////////////////////////////
- /* disable interrupts while manipulating buffer pointers */
- ///////////////////////////////////////////////////////////
-
- if(tx_fifo.num_bytes == FIFO_BUFFER_SIZE) { // no room in the sw buffer
- uart_tx_fifo_ovf_flag = 1; // set the overflow flag
- }else if(tx_fifo.num_bytes < FIFO_BUFFER_SIZE) { // if there's room in the sw buffer
- tx_fifo.data_buf[tx_fifo.i_last] = byte; // transfer data byte to sw buffer
- tx_fifo.i_last++; // increment the index of the most recently added element
- tx_fifo.num_bytes++; // increment the bytes counter
- }
- if(tx_fifo.num_bytes == FIFO_BUFFER_SIZE) { // if sw buffer is full
- uart_tx_fifo_full_flag = 1; // set the TX FIFO full flag
- }
- if(tx_fifo.i_last == FIFO_BUFFER_SIZE) { // if the "new data" index has reached the end of the buffer,
- tx_fifo.i_last = 0; // roll over the index counter
- }
-
- ///////////////////////
- /* enable interrupts */
- ///////////////////////
-
- if(tx_fifo.num_bytes > 0) { // if there is data in the buffer
-
- uart_tx_fifo_not_empty_flag = 1; // set flag
-
- ///////////////////////////////////////////////////////////////////////////
- /* if using shared RX/TX hardware buffer, disable RX data interrupt here */
- /* enable UART "TX hw buffer empty" interrupt here */
- ///////////////////////////////////////////////////////////////////////////
-
- }
-}
-/***************************************************************************************************************/
-
-
-/***************************************************************************************************************/
-// UART data receive function
-// - checks if data exists in the receive sw buffer
-// - if data exists, it returns the oldest element contained in the buffer
-// - automatically handles "uart_rx_buffer_full_flag"
-// - if no data exists, it clears the uart_rx_flag
-uint8_t uart_get_byte(void) {
-
- ///////////////////////////////////////////////////////////
- /* disable interrupts while manipulating buffer pointers */
- ///////////////////////////////////////////////////////////
-
- uint8_t byte = 0;
- if(rx_fifo.num_bytes == FIFO_BUFFER_SIZE) { // if the sw buffer is full
- uart_rx_fifo_full_flag = 0; // clear the buffer full flag because we are about to make room
- }
- if(rx_fifo.num_bytes > 0) { // if data exists in the sw buffer
- byte = rx_fifo.data_buf[rx_fifo.i_first]; // grab the oldest element in the buffer
- rx_fifo.i_first++; // increment the index of the oldest element
- rx_fifo.num_bytes--; // decrement the bytes counter
- }else{ // RX sw buffer is empty
- uart_rx_fifo_not_empty_flag = 0; // clear the rx flag
- }
- if(rx_fifo.i_first == FIFO_BUFFER_SIZE) { // if the index has reached the end of the buffer,
- rx_fifo.i_first = 0; // roll over the index counter
- }
-
- ///////////////////////
- /* enable interrupts */
- ///////////////////////
-
- return byte; // return the data byte
-}
-/***************************************************************************************************************/
-sw_fifo.h
-sw_fifo.h (1.5 KB)
-#define FIFO_BUFFER_SIZE 128 // software buffer size (in bytes)
-
-// UART data transmit function
-// - checks if there's room in the transmit sw buffer
-// - if there's room, it transfers data byte to sw buffer
-// - automatically handles "uart_tx_buffer_full_flag"
-// - sets the overflow flag upon software buffer overflow (doesn't overwrite existing data)
-// - if this is the first data byte in the buffer, it enables the "hw buffer empty" interrupt
-void uart_send_byte(uint8_t byte);
-
-
-// UART data receive function
-// - checks if data exists in the receive sw buffer
-// - if data exists, it returns the oldest element contained in the buffer
-// - automatically handles "uart_rx_buffer_full_flag"
-// - if no data exists, it clears the uart_rx_flag
-uint8_t uart_get_byte(void);
-
-volatile extern uint8_t uart_rx_fifo_not_empty_flag; // this flag is automatically set and cleared by the software buffer
-volatile extern uint8_t uart_rx_fifo_full_flag; // this flag is automatically set and cleared by the software buffer
-volatile extern uint8_t uart_rx_fifo_ovf_flag; // this flag is not automatically cleared by the software buffer
-volatile extern uint8_t uart_tx_fifo_full_flag; // this flag is automatically set and cleared by the software buffer
-volatile extern uint8_t uart_tx_fifo_ovf_flag; // this flag is not automatically cleared by the software buffer
-volatile extern uint8_t uart_tx_fifo_not_empty_flag; // this flag is automatically set and cleared by the software buffer
-
-// What I should post into the main:
-/////////////////////////////////////////////////////////////////////////
-/////////////////////////////////////////////////////////////////////////
-
-/*
-#include <sw_fifo.h> // make software buffer visible to this file
-
-volatile uint8_t uart_rx_fifo_not_empty_flag = 0;
-volatile uint8_t uart_rx_fifo_full_flag = 0;
-volatile uint8_t uart_rx_fifo_ovf_flag = 0;
-volatile uint8_t uart_tx_fifo_full_flag = 0;
-volatile uint8_t uart_tx_fifo_ovf_flag = 0;
-volatile uint8_t uart_tx_fifo_not_empty_flag = 0;
-
-int main (void) {
- uint8_t i = 0;
- uint8_t rx_data = 0;
-
- // initialize clocks
- // disable global interrupts
- // initialize gpio
- // initialize uart/usart
-
- // enable "UART RX" interrupt and "TX hardware buffer empty" interrupt
- // enable global interrupts
-
- while(uart_tx_fifo_full_flag); // wait for room to open up in the software buffer
- uart_send_byte('A'); // transmit ASCII character 'A'
- while(uart_tx_fifo_full_flag);
- uart_send_byte(0x41); // transmit ASCII character 'A'
-
- // transmit ASCII characters 1-5
- for(i=0; i<5; i++) {
- while(uart_tx_fifo_full_flag);
- uart_send_byte(i+48);
- }
-
- while(1) {
- // enter sleep mode if supported (wake from UART Rx)
-
- while(uart_rx_fifo_not_empty_flag) { // if data exists in software buffer
- rx_data = uart_get_byte(); // grab first data byte from software buffer
-
- /* handle received byte as desired */
-
-
- uart_send_byte(rx_data); // example of how to echo received ASCII characters
- }
-
- // check for rx overflow condition
- if(uart_rx_fifo_ovf_flag) {
-
- /* handle rx overflow condition as desired */
-
- uart_rx_fifo_ovf_flag = 0; // clear the rx overflow flag
- }
-
- // check for tx overflow condition
- if(uart_tx_fifo_ovf_flag) {
-
- /* handle tx overflow condition as desired */
-
- uart_tx_fifo_ovf_flag = 0; // clear the tx overflow flag
- }
-
- // if you need to disable global interrupts, you should wait until the tx fifo is empty
- while(uart_tx_fifo_not_empty_flag);
- /* Disable global interrupts */
-
- /* Do something */
-
- /* Re-enable global interrupts */
- } // end while
-} // end main
-*/
-
-
-
-
diff -r 25a2b47ca291 -r 92c25cb669f4 sw_fifo.hpp --- a/sw_fifo.hpp Thu Aug 05 08:27:51 2021 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,24 +0,0 @@ -#define FIFO_BUFFER_SIZE 128 // software buffer size (in bytes) - -// UART data transmit function -// - checks if there's room in the transmit sw buffer -// - if there's room, it transfers data byte to sw buffer -// - automatically handles "uart_tx_buffer_full_flag" -// - sets the overflow flag upon software buffer overflow (doesn't overwrite existing data) -// - if this is the first data byte in the buffer, it enables the "hw buffer empty" interrupt -void uart_send_byte(uint8_t byte); - - -// UART data receive function -// - checks if data exists in the receive sw buffer -// - if data exists, it returns the oldest element contained in the buffer -// - automatically handles "uart_rx_buffer_full_flag" -// - if no data exists, it clears the uart_rx_flag -uint8_t uart_get_byte(void); - -volatile extern uint8_t uart_rx_fifo_not_empty_flag; // this flag is automatically set and cleared by the software buffer -volatile extern uint8_t uart_rx_fifo_full_flag; // this flag is automatically set and cleared by the software buffer -volatile extern uint8_t uart_rx_fifo_ovf_flag; // this flag is not automatically cleared by the software buffer -volatile extern uint8_t uart_tx_fifo_full_flag; // this flag is automatically set and cleared by the software buffer -volatile extern uint8_t uart_tx_fifo_ovf_flag; // this flag is not automatically cleared by the software buffer -volatile extern uint8_t uart_tx_fifo_not_empty_flag; // this flag is automatically set and cleared by the software buffer