void waypointspath_control::waypoints_autopath(){
commcount = coilchange_track = jiggle_track= 0;
qDebug() << "Waypoints control thread running";
//qDebug() << " Command entered : " << command;
command = start_command;
command_x = command_y = start_command;
TimeDelay(1000);
if(w->clicks)
{
CString comport = "\\\\.\\COM4";
w->serialHandle = CreateFile(comport, GENERIC_READ | GENERIC_WRITE, 0, 0, OPEN_EXISTING, FILE_ATTRIBUTE_NORMAL, 0);
if(w->serialHandle == INVALID_HANDLE_VALUE)
emit autopath_finished();
DCB serialParams = { 0 };
serialParams.DCBlength = sizeof(serialParams);
GetCommState(w->serialHandle, &serialParams);
serialParams.BaudRate = 9600;
serialParams.ByteSize = 8;
serialParams.StopBits = ONESTOPBIT;
serialParams.Parity = PARITY_NONE;
GetCommState(w->serialHandle, &serialParams);
// Set timeouts
COMMTIMEOUTS timeout = { 0 };
timeout.ReadIntervalTimeout = 50;
timeout.ReadTotalTimeoutConstant = 50;
timeout.ReadTotalTimeoutMultiplier = 50;
timeout.WriteTotalTimeoutConstant = 50;
timeout.WriteTotalTimeoutMultiplier = 10;
SetCommTimeouts(w->serialHandle, &timeout);
COM = w->COM;
for(int l = 1; l <= cycles_no; l++){
for(wayptno = 0; wayptno < w->clicks; wayptno++){
prevdist = calc_distance(w->waypoints[wayptno]);
xdist = std::abs(COM.x - w->waypoints[wayptno].x * conv_factor[0]);
ydist = std::abs(COM.y - w->waypoints[wayptno].y * conv_factor[1]);
prevangle = w->angle;
COMprev = w->COM;
while(wayptno < w->clicks && sqrt(pow(std::abs(w->COM.x - w->waypoints[wayptno].x),2) + pow(std::abs(w->COM.y - w->waypoints[wayptno].y),2)) > tolerance)
{
//data_command = determine_command(COM,waypoints,l);
commcount++;
//data_command[3] = 0xaa;
determine_command(wayptno);
//WriteFile(w->serialHandle,(LPCVOID)data_command, toBeWritten, written,NULL);
qDebug() << "Check state waypoint control : "<< WriteFile(w->serialHandle,(LPVOID)data_command, toBeWritten, written,NULL);
TimeDelay(1000);
if(!WriteFile(w->serialHandle,(LPCVOID)w->prevsent,toBeWritten, written, NULL));
emit autopath_finished();
TimeDelay(100);
//for(int i = 0; i < 7; i++)
//data_command[i] = 0x00;
}
}
}
}
CloseHandle(w->serialHandle);
emit autopath_finished();
}
// Called before every servo Sample
void CallBack(void)
{
// Compute Error and Time Delay it
ch0->DestOffset = TimeDelay(ch0->Dest-ch0->Position)*GAIN;
// Force no Damping if Stopped (Integrator off) or User on/off
if (ch0->I < 1e-6f || persist.UserData[100]==0.0) ch0->DestOffset=0.0;
// maintain Dither mode
T1=Time_sec(); // returns current time
DoDither(ch0, P_original_0, P_low_0, I_original_0, &T0_0, T1);
}
/* Output message to process, with "ms" milliseconds of delay
between each character. This is needed when sending the logon
script to ICC, which for some reason doesn't like the
instantaneous send. */
int
OutputToProcessDelayed (ProcRef pr, char *message, int count, int *outError, long msdelay)
{
ChildProc *cp = (ChildProc *) pr;
int outCount = 0;
int r;
while (count--) {
r = write(cp->fdTo, message++, 1);
if (r == -1) {
*outError = errno;
return outCount;
}
++outCount;
if (msdelay >= 0)
TimeDelay(msdelay);
}
return outCount;
}
int main() {
TimeDelay td1 = TimeDelay(); /* OOP: Automatic object instantiation (using stack) */
#ifdef NO_EXCEPTIONS
TimeDelay *td2 = new TimeDelay(); /* OOP: Dynamic object instantiation (using heap) */
#else
TimeDelay *td2 = NULL;
try { /* OOP: Exception handling */
td2 = new TimeDelay();
} catch(int e) {
while(true);
}
#endif
while (1)
{
/* PD12 to be toggled */
myled1 = myled1 ^ 1;
/* Insert delay */
wait(td1.get()/0x8FFFFF+0.25);
/* PD13 to be toggled */
myled2 = myled2 ^ 1;
/* Insert delay */
wait(td1.get()/0x8FFFFF+0.25);
/* PD14 to be toggled */
myled3 = myled3 ^ 1;
/* Insert delay */
wait(td1.get()/0x8FFFFF+0.25);
/* PD15 to be toggled */
myled4 = myled4 ^ 1;
/* Insert delay */
wait(td2->get()/0x8FFFFF+0.25);
}
}
void micro_sleep(unsigned int n) {
TimeDelay(UNIT_MICROHZ, n / 1000000, n % 1000000);
return;
}
unsigned sleep(unsigned n) {
TimeDelay(UNIT_MICROHZ, 0, n*1000);
return 0;
}
