#include <cvirte.h>
#include <userint.h>
#include <utility.h>
#include <ansi_c.h>
#include <rs232.h>
#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#include "motor.h"
#include <analysis.h>
#include <math.h>
//stepping motor busheng --> declaration 1step=1.8degrees
//experimantally: full rotation = 3200 steps (on setting 1step/mm ($100))
int pCOM1 = 6;
int pCOM2 = 7;
int run, graph, f_t, ji, vi, run1;
int left_1, right_1;
static int panel, tfID, tfID1;
static int poolHandle = 0;
#define TEXT_LENGTH 2000
#define MAX_THREADS 10
double volt_g[TEXT_LENGTH], volt_g1[TEXT_LENGTH*200];
int bytes_read0;
int bytes_read1;
char read_data0[TEXT_LENGTH];
char read_data1[TEXT_LENGTH];
void initialize()
{
// Open grbl serial port //("COM6",115200)
OpenComConfig (pCOM1, "COM6", 115200, 0, 8, 1, 512, 512);
/* Turn off Hardware handshaking (loopback test will not function with it on) */
SetCTSMode (pCOM1, LWRS_HWHANDSHAKE_OFF);
ComWrt (pCOM1, "\r\n\r\n", 5); //Writing on Port
Delay(2); //Wait for grbl to initialize
int strLen0 = GetInQLen (pCOM1); //Reading from Port
bytes_read0 = ComRd (pCOM1, read_data0, strLen0);
//Left in this form - can have more initializing commands if needeed
char init[1][10] = {"$RST=$\n"};//, "$100=20\n","$$\n","$100=100\n"
char *cmd;
for(int i=0; i<1; i++)
{
cmd = init + i; //Takes init[i]
ComWrt (pCOM1, cmd, len);
Delay(0.01); //Give it a little time to respond...
int strLen1 = GetInQLen (pCOM1);
bytes_read1 = ComRd (pCOM1, read_data1, strLen1);
printf(" : %s\n", read_data1
);
//printf("bytes_read = %d\n\n", bytes_read1);
}
FlushOutQ (pCOM1);
}
void asking_position()
{
while (1)
{
FlushInQ (pCOM1);
FlushOutQ (pCOM1);
ComWrt (pCOM1, "?\n", 2); //asks grbl his position
Delay(0.01);
int strLen0 = GetInQLen (pCOM1);
bytes_read0 = ComRd (pCOM1, read_data0, strLen0);
//printf(" : %s", read_data0);
//printf("bytes_read = %d\n\n", bytes_read0);
//taking subtring of a string to first appernace of character
char *read_data_0
= strtok(read_data0
, "\n");
//printf(" : %s\n", read_data_0);
//finding substring in string
char *substr
= strstr(read_data_0
,"Idle");
//printf("\n%s\n", substr);
if(substr>0) break;
if (!run)
{
ComWrt (pCOM1, "!\n", 2); //stops grbl
Delay(0.01);
ComWrt (pCOM1, "~\n", 2); //releases grbl
Delay(0.01);
int strLen = GetInQLen (pCOM1);
bytes_read0 = ComRd (pCOM1, read_data0, strLen);
//printf(" : %s", read_data0);
break;
}
}
}
void initialize_voltage()
{
// Open serial port for photodetector - reading is on chanel A1
OpenComConfig (pCOM2, "COM7", 115200, 0, 8, 1, 512, 512);
SetCTSMode (pCOM2, LWRS_HWHANDSHAKE_OFF);
Delay(0.5);
FlushInQ (pCOM2);
FlushOutQ (pCOM2);
}
int bytes_read2;
char read_data2[TEXT_LENGTH];
double read_voltage()
{
//int strLen2 = GetInQLen (pCOM2);
bytes_read2 = ComRdTerm (pCOM2, read_data2, 11, 10);
//IMPORTANT: 11 bytes is the size of one line
printf("Voltage: \n%s\n", read_data2
);
//printf("bytes_read = %d\n\n", bytes_read2);
//Delay(0.1);
double Volt;
sscanf(read_data2
, "%lf", &Volt
);
//printf("Volt = %lf\n", Volt);
return Volt;
}
int bytes_read3;
char read_data3[TEXT_LENGTH];
void movement(int steps)
{
//Move on the steps
char str[10], move[20] = "$J=X";
//Command must look like "X200\n" or "$J=X200F600"
//printf("len0 is %d ", len0);
printf("Moving on point %d\n", steps
);
ComWrt (pCOM1, move, len0);
Delay(0.01);
int strLen3 = GetInQLen (pCOM1);
bytes_read3 = ComRd (pCOM1, read_data3, strLen3);
//printf(" :\n%s", read_data3);
//printf("bytes_read = %d\n\n", bytes_read3);
FlushInQ (pCOM1);
}
static int CVICALLBACK daq_run(void *functionData)
{
int num_points;
int waittime;
double volt;
int j = 0; //for graph
GetCtrlVal(panel, p1_NUM_POINTS, &num_points);
GetCtrlVal(panel, p1_WAIT_TIME, &waittime);
//double wait = waittime/1000.0; //Wait time between each step
//printf("wait: %lf", wait);
movement(0);
Delay(0.1);
asking_position();
initialize_voltage();
if (!run) return 0;
FlushInQ (pCOM1);
FlushOutQ (pCOM1);
Delay(0.1);
movement(num_points);
while (1) // Continious reading
{
volt = read_voltage();
FlushInQ (pCOM2);
SetCtrlVal(panel, p1_VOLTAGE, volt);
//writing in tabel for graph
volt_g1[j] = volt;
if (volt>6) volt_g1[j]=volt_g1[j-1];
if (volt<-0.1) volt_g1[j]=volt_g1[j-1];
j++;
if (!run) // Stopping grbl if pressed STOP
{
ComWrt (pCOM1, "!\n", 2); //stops grbl
Delay(0.01);
ComWrt (pCOM1, "~\n", 2); //releases grbl
Delay(0.01);
int strLen = GetInQLen (pCOM1);
bytes_read0 = ComRd (pCOM1, read_data0, strLen);
//printf(" : %s", read_data0);
break;
}
FlushInQ (pCOM1);
FlushOutQ (pCOM1);
ComWrt (pCOM1, "?\n", 2); //asks grbl his position
Delay(0.01);
int strLen5 = GetInQLen (pCOM1);
bytes_read0 = ComRd (pCOM1, read_data0, strLen5);
char *read_data_0
= strtok(read_data0
, "\n");
char *substr2
= strstr(read_data_0
,"Idle");
//printf("\n%d\n", substr);
if(substr2>0) break;
}
if (run)
{
// Choosing 2^n points
int j_j = j - 500; //cut off first 300 and last - 200 - points
int j_jj = j_j/num_points;
//printf("j_j = %d, j_jj = %d\n",j_j,j_jj);
//IMPORTANT! On graph must be only num_points of points
for (int i=0; i<num_points; i++)
{
volt_g[i] = volt_g1[300+i*j_jj];
}
}
// Moving by steps
/*for(int i=(-num_points/2); i<(num_points/2); i++)
{
//printf("i is: %d\n", i);
movement(i);
asking_position();
Delay(0.01); // Little time has to be in between so that grbl can respond
volt = read_voltage();
Delay(wait);
FlushInQ (pCOM1);
FlushInQ (pCOM2);
SetCtrlVal(panel, p1_VOLTAGE, volt);
//writing in tabel for graph
volt_g[j] = volt;
j++;
if (!run) break;
}*/
if (run) //IMPORTANT!! SAME NUM. of POINTS FOR FOURIER
{
//for(int i=0;i<j;i++) printf("volt_g[%d] = %lf\n",i,volt_g[i]);
if (graph>0) DeleteGraphPlot (panel, p1_VOLTAGE_GRAPH, graph, VAL_DELAYED_DRAW);
graph = PlotY (panel, p1_VOLTAGE_GRAPH, &volt_g[0], num_points, VAL_DOUBLE,
VAL_CONNECTED_POINTS, VAL_SIMPLE_DOT, VAL_SOLID, 1, VAL_WHITE);
ji = num_points;
f_t = 1;
}
FlushInQ (pCOM1);
FlushOutQ (pCOM1);
FlushInQ (pCOM2);
FlushOutQ (pCOM2);
run = 0;
return 0;
}
static int CVICALLBACK voltage_run(void *functionData)
{
int fix_range;
double ymin,ymax,xmin,xmax;
double voltA,voltB=0;
//double volt1[TEXT_LENGTH];
initialize_voltage();
int chart = NewCtrl (panel, CTRL_GRAPH_LS, "RUN VOLTAGE", 100, 15);
SetCtrlAttribute(panel, chart, ATTR_HEIGHT, 600);
SetCtrlAttribute(panel, chart, ATTR_WIDTH , 1300);
while(1)
{
// Range
GetCtrlVal(panel, p1_RANGE, &fix_range);
if(fix_range)
{
GetAxisRange (panel, chart, VAL_MANUAL, &xmin, &xmax, VAL_MANUAL, &ymin, &ymax);
if (ymax<=2) SetAxisRange (panel, chart, VAL_NO_CHANGE, 0, 0, VAL_MANUAL, 0, 2);
else SetAxisRange (panel, chart, VAL_NO_CHANGE, 0, 0, VAL_MANUAL, 0, 5);
}
else SetAxisRange (panel, chart, VAL_NO_CHANGE, 0, 0, VAL_AUTOSCALE, 0, 0);
//volt1[time] = read_voltage();
voltA = read_voltage();
PlotPoint
(panel
, chart
, time, voltA
, VAL_SOLID_SQUARE
, VAL_WHITE
);
if (time
>0) PlotLine
(panel
, chart
, time, voltA
, time
-1, voltB
, VAL_WHITE
);
if (time
>50) SetAxisRange
(panel
, chart
, VAL_MANUAL
, time
-50, time, VAL_NO_CHANGE
, 0, 0);
SetCtrlVal(panel, p1_VOLTAGE, voltA);
Delay(0.05);
voltB = voltA;
//volt1[time] = voltA;
FlushInQ (pCOM2);
if (vi == 0) break;
}
DiscardCtrl (panel, chart);
// After some time (~4000 points) it becomes slower in writing out data...
//if (graph!=0) DeleteGraphPlot (panel, p1_VOLTAGE_GRAPH, graph, VAL_DELAYED_DRAW);
//graph = PlotY (panel, p1_VOLTAGE_GRAPH, &volt1[0], time+1, VAL_DOUBLE,
// VAL_CONNECTED_POINTS, VAL_SIMPLE_DOT, VAL_SOLID, 1, VAL_WHITE);
return 0;
}
static int CVICALLBACK free_run(void *functionData)
{
if (run == 1) return 0;
run1 = 1;
if (left_1) movement(-10000);
if (right_1) movement(10000);
left_1 = 0;
right_1 = 0;
while (1)
{
if (run1 == 0)
{
ComWrt (pCOM1, "!\n", 2); //stops grbl
Delay(0.01);
ComWrt (pCOM1, "~\n", 2); //releases grbl
Delay(0.01);
int strLen = GetInQLen (pCOM1);
bytes_read0 = ComRd (pCOM1, read_data0, strLen);
//printf(" : %s", read_data0);
break;
}
}
return 0;
}
int main()
{
SetStdioPort(HOST_SYSTEM_STDIO);
SetStdioWindowOptions(1000000, 0, 0);
SetStdioWindowVisibility(1);
initialize();
CmtNewThreadPool (MAX_THREADS, &poolHandle);
DisplayPanel (panel = LoadPanel (0, "motor.uir", p1));
RunUserInterface ();
CmtDiscardThreadPool (poolHandle);
return 0;
}
int CVICALLBACK StartCB (int panel, int control, int event,
void *callbackData, int eventData1, int eventData2)
{
int dummy;
switch (event)
{
case EVENT_COMMIT:
if (run == 1) break;
run = 1;
CmtScheduleThreadPoolFunction (poolHandle, daq_run, (void *)&dummy, &tfID);
}
return 0;
}
int CVICALLBACK StopCB (int panel, int control, int event,
void *callbackData, int eventData1, int eventData2)
{
switch (event)
{
case EVENT_COMMIT:
if (run == 0) break;
run = 0;
CmtWaitForThreadPoolFunctionCompletion (poolHandle, tfID,
OPT_TP_PROCESS_EVENTS_WHILE_WAITING);
CmtReleaseThreadPoolFunctionID (poolHandle, tfID);
break;
}
return 0;
}
int CVICALLBACK ExitCB (int panel, int control, int event,
void *callbackData, int eventData1, int eventData2)
{
switch (event)
{
case EVENT_COMMIT:
QuitUserInterface (0);
CloseCom(pCOM1);
CloseCom(pCOM2);
break;
}
return 0;
}
int CVICALLBACK Fourier_TransCB (int panel, int control, int event,
void *callbackData, int eventData1, int eventData2)
{
if (!ji) return 0;
const int ii = ji;
double fourier_t[ii], fourier_ti[ii];
char window_fun;
switch (event)
{
case EVENT_COMMIT:
for(int i=0; i<ii; i++)
{
fourier_t[i] = volt_g[i];
fourier_ti[i] = 0;
}
GetCtrlVal(panel, p1_WIN_FUN, &window_fun);
//printf("window_fun = %d\n", window_fun);
if (window_fun!=0)
{
//do something with window functions
if(window_fun==1) for(int i=0; i<ii; i++) fourier_t[i] = (2*fourier_t[i]);
if(window_fun
==2) for(int i
=0; i
<ii
; i
++) fourier_t
[i
] = sin(fourier_t
[i
]);
YGraphPopup ("HOW volt_g LOOKS NOW", fourier_t, ii, VAL_DOUBLE);
}
FFT (fourier_t, fourier_ti, ii);
for (int i
=0; i
<ii
; i
++) fourier_t
[i
] = fabs(fourier_t
[i
]);
fourier_t[0]=0;
YGraphPopup ("FOURIER", fourier_t, ii/2+1, VAL_DOUBLE);
if (f_t == 1)
{
FILE *fp,*fp1;
//file name has to have file extension
char fname[50];
GetCtrlVal(panel, p1_F_NAME, fname);
char fname1[50];
GetCtrlVal(panel, p1_F_NAME1, fname1);
fp1
= fopen(fname1
, "w");
for(int i=0; i<ii; i++)
{
fprintf(fp
, "%d\t%lf\n", i
+1, volt_g
[i
]);
}
for(int i=0; i<ii/2+1; i++)
{
fprintf(fp1
, "%d\t%lf\n", i
, fourier_t
[i
]); //0th component is from DC - background?
}
printf("Files %s and %s created.\n", fname
, fname1
);
}
f_t = 0; //so that we make only one file
break;
}
return 0;
}
int CVICALLBACK ExamplesCB (int panel, int control, int event,
void *callbackData, int eventData1, int eventData2)
{
const int ki=1024;
double ret[ki], ret1[ki];
double val = 3.14159265/180;
int x1, x2, a1, a2;
switch (event)
{
case EVENT_COMMIT:
GetCtrlVal(panel, p1_FREQ1, &x1);
GetCtrlVal(panel, p1_FREQ2, &x2);
GetCtrlVal(panel, p1_AMPL1, &a1);
GetCtrlVal(panel, p1_AMPL2, &a2);
for (int i=0; i<ki; i++)
{
ret
[i
] = a1
*sin(x1
*i
*val
)+a2
*sin(x2
*i
*val
);
ret1[i] = 0;
}
YGraphPopup ("example graph", ret, ki, VAL_DOUBLE);
FFT (ret, ret1, ki);
for (int i
=0; i
<ki
; i
++) ret
[i
] = fabs(ret
[i
]);
//for (int i=0;i<ki;i++) printf("basic: %lf, fourier: %lf\n", ret1[i], ret[i]);
YGraphPopup ("example fourier", ret, ki/2+1, VAL_DOUBLE);
break;
}
return 0;
}
int CVICALLBACK Only_voltageCB (int panel, int control, int event,
void *callbackData, int eventData1, int eventData2)
{
int dummy, volt_run;
switch (event)
{
case EVENT_COMMIT:
GetCtrlVal (panel, p1_VOLT_GRAPH, &volt_run);
if(volt_run)
{
vi = 1;
CmtScheduleThreadPoolFunction (poolHandle, voltage_run, (void *)&dummy, &tfID);
}
else
{
vi = 0;
CmtWaitForThreadPoolFunctionCompletion (poolHandle, tfID,
OPT_TP_PROCESS_EVENTS_WHILE_WAITING);
CmtReleaseThreadPoolFunctionID (poolHandle, tfID);
}
break;
}
return 0;
}
int CVICALLBACK RangeCB (int panel, int control, int event,
void *callbackData, int eventData1, int eventData2)
{
int fix_range;
double ymin,ymax,xmin,xmax;
switch (event)
{
case EVENT_COMMIT:
GetCtrlVal(panel, p1_RANGE, &fix_range);
if(fix_range)
{
GetAxisRange (panel, p1_VOLTAGE_GRAPH, VAL_MANUAL, &xmin, &xmax, VAL_MANUAL, &ymin, &ymax);
if (ymax<=2) SetAxisRange (panel, p1_VOLTAGE_GRAPH, VAL_NO_CHANGE, 0, 0, VAL_MANUAL, 0, 2);
else SetAxisRange (panel, p1_VOLTAGE_GRAPH, VAL_NO_CHANGE, 0, 0, VAL_MANUAL, 0, 5);
}
else SetAxisRange (panel, p1_VOLTAGE_GRAPH, VAL_NO_CHANGE, 0, 0, VAL_AUTOSCALE, 0, 0);
break;
}
return 0;
}
int CVICALLBACK FreeMoveCB (int panel, int control, int event,
void *callbackData, int eventData1, int eventData2)
{
int dummy;
switch (event)
{
case EVENT_COMMIT:
if (run == 1) break;
if (run1 == 1) break;
right_1 = 1;
CmtScheduleThreadPoolFunction (poolHandle, free_run, (void *)&dummy, &tfID1);
break;
}
return 0;
}
int CVICALLBACK FreeMove1CB (int panel, int control, int event,
void *callbackData, int eventData1, int eventData2)
{
int dummy;
switch (event)
{
case EVENT_COMMIT:
if (run == 1) break;
if (run1 == 1) break;
left_1 = 1;
CmtScheduleThreadPoolFunction (poolHandle, free_run, (void *)&dummy, &tfID1);
break;
}
return 0;
}
int CVICALLBACK FreeMoveStopCB (int panel, int control, int event,
void *callbackData, int eventData1, int eventData2)
{
switch (event)
{
case EVENT_COMMIT:
if (run == 1) break;
if (run1 == 0) break;
run1 = 0;
CmtWaitForThreadPoolFunctionCompletion (poolHandle, tfID1,
OPT_TP_PROCESS_EVENTS_WHILE_WAITING);
CmtReleaseThreadPoolFunctionID (poolHandle, tfID1);
break;
}
return 0;
}
int CVICALLBACK Pos0CB (int panel, int control, int event,
void *callbackData, int eventData1, int eventData2)
{
switch (event)
{
case EVENT_COMMIT:
if (run == 1) break;
if (run1 == 1) break;
FlushInQ (pCOM1);
FlushOutQ (pCOM1);
CloseCom(pCOM1);
initialize();
break;
}
return 0;
}