static const struct sigevent* timerIntHandler(void* data, int id) {
struct timespec tp;
TimerIntData* myData = (TimerIntData*)data;
uint64_t new_tsc = ClockCycles();
clock_gettime(CLOCK_REALTIME, &tp);
if(new_tsc > myData->prev_tsc) {
myData->cur_delta = new_tsc - myData->prev_tsc;
/* when hell freezeth over, thy TSC shall roll over */
} else {
myData->cur_delta = myData->prev_delta;
}
/* 4/6 weighted average */
myData->filtered_delta = (40 * myData->cur_delta + 60 * myData->prev_delta) / 100;
myData->prev_delta = myData->cur_delta;
myData->prev_tsc = new_tsc;
if(myData->counter < 2) {
myData->counter++;
}
myData->last_clock = timespec2nsec(&tp);
return NULL;
}
int main ()
{
struct timespec clockval, prevclockval;
uint64_t delta;
uint64_t cycs [NumSamples];
int i;
setvbuf (stdout, NULL, _IOLBF, 0);
printf ("\nClockTime deltas:\n");
/*
* capture some clock changes
*/
for (i = 0; i < 10; i++) {
clock_gettime (CLOCK_REALTIME, &prevclockval);
do {
clock_gettime (CLOCK_REALTIME, &clockval);
} while (clockval.tv_sec == prevclockval.tv_sec &&
clockval.tv_nsec == prevclockval.tv_nsec);
delta = ((uint64_t) clockval.tv_sec * BILLION + (uint64_t) clockval.tv_nsec) -
((uint64_t) prevclockval.tv_sec * BILLION + (uint64_t) prevclockval.tv_nsec);
printf ("prev %u.%09lu, new %u.%09lu, delta %llu\n",
prevclockval.tv_sec, prevclockval.tv_nsec,
clockval.tv_sec, clockval.tv_nsec,
delta);
}
/*
* next, we very quickly snapshot some values of the ClockCycles
* call. We don't do any printf's, as we want to see the speed
* with which the "for" loop executes.
*/
printf ("%d ClockCycles values:\n", NumSamples);
for (i = 0; i < NumSamples; i++) {
cycs [i] = ClockCycles ();
}
/*
* now, print out the delta's, so we can easily compute how much
* time each "for" loop iteration took.
*/
printf ("%llu\n", cycs [0]);
for (i = 1; i < NumSamples; i++) {
printf ("%llu, delta %llu (decimal)\n",
cycs [i], cycs [i] - cycs [i - 1]);
}
printf ("%s: main, exiting\n", progname);
return EXIT_SUCCESS;
}
double get_tick_count_ha()
{
uint64_t cps, cycle;
double sec;
cycle=ClockCycles( );
cps = SYSPAGE_ENTRY(qtime)->cycles_per_sec;
sec=(double)cycle/cps;
return(sec);
}
static void *timer_thread( void *p )
{
int pool_id;
int ret;
uint64_t clk;
uint64_t rdata;
int timeout;
ret = yarrow_add_source( Yarrow, &pool_id );
if( ret != 0 )
{
slogf( _SLOGC_CHAR, _SLOG_CRITICAL,
"random: Unable to get pool_id for timer thread." );
return NULL;
}
timeout = 100;
rdata = 0;
while( 1 )
{
if( Yarrow )
{
yarrow_output( Yarrow, (uint8_t *)&rdata, sizeof( rdata ) );
/* Wait for between 10ms and 1033ms */
timeout = ( rdata & 0x3FF ) + 10;
}
delay( timeout );
clk = ClockCycles();
clk = clk ^ rdata;
if( Yarrow )
yarrow_input( Yarrow, (uint8_t *)&clk, sizeof( clk ), pool_id, 8 );
}
return NULL;
}
static Boolean
getTime (ClockDriver *self, TimeInternal *time) {
#ifdef __QNXNTO__
static TimerIntData tmpData;
int ret;
uint64_t delta;
double tick_delay;
uint64_t clock_offset;
struct timespec tp;
if(!tDataUpdated) {
memset(&tData, 0, sizeof(TimerIntData));
if(ThreadCtl(_NTO_TCTL_IO, 0) == -1) {
ERROR(THIS_COMPONENT"QNX: could not give process I/O privileges");
return FALSE;
}
tData.cps = SYSPAGE_ENTRY(qtime)->cycles_per_sec;
tData.ns_per_tick = 1000000000.0 / tData.cps;
tData.prev_tsc = ClockCycles();
clock_gettime(CLOCK_REALTIME, &tp);
tData.last_clock = timespec2nsec(&tp);
ret = InterruptAttach(0, timerIntHandler, &tData, sizeof(TimerIntData), _NTO_INTR_FLAGS_END | _NTO_INTR_FLAGS_TRK_MSK);
if(ret == -1) {
ERROR(THIS_COMPONENT"QNX: could not attach to timer interrupt");
return FALSE;
}
tDataUpdated = TRUE;
time->seconds = tp.tv_sec;
time->nanoseconds = tp.tv_nsec;
return;
}
memcpy(&tmpData, &tData, sizeof(TimerIntData));
delta = ClockCycles() - tmpData.prev_tsc;
/* compute time since last clock update */
tick_delay = (double)delta / (double)tmpData.filtered_delta;
clock_offset = (uint64_t)(tick_delay * tmpData.ns_per_tick * (double)tmpData.filtered_delta);
/* not filtered yet */
if(tData.counter < 2) {
clock_offset = 0;
}
DBGV("QNX getTime cps: %lld tick interval: %.09f, time since last tick: %lld\n",
tmpData.cps, tmpData.filtered_delta * tmpData.ns_per_tick, clock_offset);
nsec2timespec(&tp, tmpData.last_clock + clock_offset);
time->seconds = tp.tv_sec;
time->nanoseconds = tp.tv_nsec;
return TRUE;
#else
#if defined(_POSIX_TIMERS) && (_POSIX_TIMERS > 0)
struct timespec tp;
if (clock_gettime(CLOCK_REALTIME, &tp) < 0) {
PERROR(THIS_COMPONENT"clock_gettime() failed, exiting.");
exit(0);
}
time->seconds = tp.tv_sec;
time->nanoseconds = tp.tv_nsec;
#else
struct timeval tv;
gettimeofday(&tv, 0);
time->seconds = tv.tv_sec;
time->nanoseconds = tv.tv_usec * 1000;
#endif /* _POSIX_TIMERS */
#endif /* __QNXNTO__ */
return TRUE;
}
int main( int argc, char *argv[] )
{
db_clt_typ *pclt = NULL;
char hostname[MAXHOSTNAMELEN+1];
db_data_typ db_data;
posix_timer_typ *ptmr;
int recv_type;
int millisec = 5000;
trig_info_typ trig_info;
uint64_t ticksPerMilliSec = SYSPAGE_ENTRY(qtime)->cycles_per_sec / 1000000;
unsigned count = 0;
unsigned total_diff = 0;
/* Initialize the database. */
get_local_name(hostname, MAXHOSTNAMELEN);
if( (pclt = database_init(argv[0], hostname, DEFAULT_SERVICE,
COMM_QNX6_XPORT )) == NULL )
{
fprintf(stderr, "Database initialization error in ids_io\n");
veh_done( pclt );
exit( EXIT_FAILURE );
}
/* Initialize the timer. */
if ((ptmr = timer_init(millisec, DB_CHANNEL(pclt))) == NULL)
{
printf("timer_init failed\n");
exit( EXIT_FAILURE );
}
print_timer(ptmr);
if( setjmp( exit_env ) != 0 )
{
printf("average timediff = %u\n", total_diff / count);
veh_done( pclt );
exit( EXIT_SUCCESS );
}
else
sig_ign( sig_list, sig_hand );
for( ;; )
{
/* Now wait for a trigger. */
recv_type= clt_ipc_receive(pclt, &trig_info, sizeof(trig_info));
if (recv_type == DB_TIMER)
{
printf("received timer alarm\n");
}
else if(DB_TRIG_VAR(&trig_info) == 200)
{
fflush(stdout);
/* Read DB_DII_OUT_VAR and send DII control
* to the hardware. */
if( clt_read( pclt, 200, 200, &db_data ) == FALSE)
{
fprintf( stderr, "clt_read( DB_DII_OUT_VAR ).\n" );
}
else
{
uint64_t *incoming_time = (uint64_t*) db_data.value.user;
uint64_t timediff = ClockCycles() - *incoming_time;
timediff /= ticksPerMilliSec;
total_diff += timediff;
++count;
}
}
else
printf("Unknown trigger, recv_type %d\n", recv_type);
}
}
void SampleLoopTask::Task(){
int cnt = 0;
int region;
int attempt = 0;
static int dout = 1;
static double error = 0;
int last_status = 0;
double pos_prev = 0;
double pCmd_prev = 0;
double vCmd_prev = 0;
int obj_ang_index = 0; // to keep continuous angle value when acrossing pi(or -pi).
uint64_t cycle, cycle_prev;
// for acceleration calculation
double u = 0;
double alpha = 0;
double DuDeta1Bar =0;
double DuDeta2 =0;
double DalphaDeta1Bar =0;
double DalphaDeta2 =0;
double eta1Bar = 0;
double z = 0;
// to use eta2D in calculation of shD
double eta2_prev = 0;
double eta2D = 0;
double eta2D_prev = 0;
double elapsedSec = 0;
//test
double feedforwardVal = 0;
uint64_t cycle1, cycle2;
while(!lost){
TimerWait();
ncycles = ClockCycles();
sec=(double)(ncycles - ncycles_prev)/cps;
ncycles_prev = ncycles;
TimingProcess();
// Read Inputs
HW->ProcessInput(); // all digital & analog, encoders reading.
// Get status of camera
last_status = HW->ReadDigitalBit(IoHardware::FRAME_STATUS);
// Send out pulse to trigger camera
HW->WriteDigitalBit(IoHardware::CAMERA_TRIGGER, 1);
HW->WriteDigitalBit(IoHardware::CAMERA_TRIGGER, 0);
// test
//cycle1 = ClockCycles();
if(camera) {// && !lost){
// Wait for camera to process data, with timeout counter
while(HW->ReadDigitalBit(IoHardware::FRAME_STATUS) == last_status){
if(++attempt == (int)(6.0e5/SAMPLE_RATE)) { // 5.0e5 must be found out by experiments to give the smallest time to determine an error status
HW->SetAnalogOut(IoHardware::AMP_SIGNAL, 0.0);
HW->WriteDigitalBit(IoHardware::MOTOR_ENABLE, MOTOR_OFF);
HW->motorStatus = MOTOR_OFF;
lost = 1;
FATAL_ERROR("Frame not received -");
//strcpy(err_msg, "Frame not received -"); // // not work this way.
}
}
attempt = 0;
vnum[0] = -99;
int n = VNET->Recv();
region = (int)vnum[0];
switch(region){
case 0: //ROI_0:
obj.x0 = vnum[1]; //mm
obj.y0 = vnum[2]; //mm
break;
case 1: //ROI_1:
obj.x1 = vnum[1]; //mm
obj.y1 = vnum[2]; //mm
break;
default:
HW->SetAnalogOut(IoHardware::AMP_SIGNAL, 0.0);
HW->WriteDigitalBit(IoHardware::MOTOR_ENABLE, MOTOR_OFF);
HW->motorStatus = MOTOR_OFF;
printf("roi-vnum[0]:%d\n", region);
FATAL_ERROR("Object lost -");
lost = 1;
}
obj.x = (obj.x0 + obj.x1)/2/1000; // convert to m
obj.y = (obj.y0 + obj.y1)/2/1000;
double obj_raw_angle = atan2(obj.y1-obj.y0, obj.x1-obj.x0); // within -pi to pi
// to keep continuous angle value when acrossing pi(or -pi).
obj.theta = obj_raw_angle + obj_ang_index*2*3.141592; // converted angle
if ( fabs(obj.theta - obj.theta_prev) > 3.141592 ) {
if (obj.theta_prev > obj.theta) // pi to -pi region change
obj_ang_index++;
else
obj_ang_index--;
obj.theta = obj_raw_angle + obj_ang_index*2*3.141592; // newly converted angle
}
// calculation of the object angular velocity
obj.angVel = (obj.theta - obj.theta_prev) / sec; // the use of SAMPLE_RATE may not be a big difference.
// low pass filter
double alpha = 0.038; //0.02
obj.angVel = alpha*obj.angVel + (1-alpha)*obj.angVel_prev;
// calculation of the hand angular velocity
handTheta = -(HW->GetEncoderCount(IoHardware::ENC_0)) * ENC_RAD_PER_CNT / GR / 4; // just access the data previously read by GetEnc...()
handVel = (handTheta - handTheta_prev) /sec; //* SAMPLE_RATE;
handVel = alpha*handVel + (1-alpha)*handVel_prev;
// calculation of sh and \dot sh (=shD)
sh = (obj.theta - handTheta) / K_R;
eta2 = asin(-(obj.x-OBJ_X_OFFSET)/(RHO_H+RHO_O)); // alternative way to get eta2;
/// test, eta2 compensation
//eta2 = eta2 - 0.004*sin(obj.theta + 1.885);
// new method for shD, thus eta1 feedback
eta2D = (eta2 - eta2_prev) / sec;
eta2D = alpha*eta2D + (1-alpha)*eta2D_prev;
shD = RHO_H*(eta2D - handVel);
eta1 = M22*shD + M12*handVel;
/*************************************************************/
// calculation of the control input (acceleration command)
eta1Bar = eta1 - COEFF_ETA1_STAR;
z = handVel - TARGET_THETAHD;
if (eta2 == 0) {
u = -GAIN_K1*eta1Bar;
}
else {
u = -GAIN_K1*eta1Bar*sin(eta2)/eta2 - GAIN_K2*eta2;
}
alpha = (u - COEFF_sig2*eta1Bar)/COEFF_sig3;
if (eta2 == 0) {
DuDeta1Bar = -GAIN_K1;
DuDeta2 = -GAIN_K2;
}
else {
DuDeta1Bar = -GAIN_K1*sin(eta2)/eta2;
DuDeta2 = -GAIN_K1*eta1Bar*( (cos(eta2)*eta2 - sin(eta2))/(eta2*eta2) ) - GAIN_K2;
}
DalphaDeta1Bar = (DuDeta1Bar - COEFF_sig2)/COEFF_sig3;
DalphaDeta2 = DuDeta2/COEFF_sig3;
vInp = DalphaDeta1Bar*COEFF_sig1*sin(eta2) + DalphaDeta2*(COEFF_sig2*eta1Bar+COEFF_sig3*z) - eta2*COEFF_sig3 - GAIN_C*(z - alpha);
/*************************************************************/
aCmd = vInp; // calculated acceleration command
obj.theta_prev = obj.theta;
obj.angVel_prev = obj.angVel;
handTheta_prev = handTheta;
handVel_prev = handVel;
eta2D_prev = eta2D;
eta2_prev = eta2;
}
else { // Because the vision system keeps sending the data, QNX must read them, otherwise the vision system will get a send error.
VNET->Process();
}
// test
if (fabs(handVel) > 7.0 ) {//rad/sec
HW->SetAnalogOut(IoHardware::AMP_SIGNAL, 0.0);
HW->WriteDigitalBit(IoHardware::MOTOR_ENABLE, MOTOR_OFF);
HW->motorStatus = MOTOR_OFF;
//VNET->Process();
FATAL_ERROR("MOTOR RUNS TOO FAST");
lost = 1;
}
//**************************************************************
// reading & calculating output
//
// feedback
cycle = ClockCycles();
elapsedSec = (double)(cycle - cycle_prev)/cps;
cycle_prev = cycle;
//HW->ProcessInput(); // because of the encoder reading. Do not use this again. This takes a long time.
enc = -(HW->ReadEncoder(IoHardware::ENC_0)); // direct read. Not working?
pos = (enc * ENC_RAD_PER_CNT) / GR / 4; // convert to rad. GR:gear ratio (50). 4?
vel = (pos - pos_prev) / elapsedSec; // to calculate more exact velocity.
// This is necessary for not having motor run unexpectedly when swtiching from motor off to motor on.
if(HW->motorStatus == MOTOR_ON) {
cnt++;
// acceleration inner loop
// Notice that using no filtering velocity
vCmd = vCmd_prev + aCmd / SAMPLE_RATE;
pCmd = pCmd_prev + vCmd_prev / SAMPLE_RATE + 0.5 * aCmd / (SAMPLE_RATE*SAMPLE_RATE);
iCmd = calcI(vCmd, aCmd); // feedforward control.
feedforwardVal = iCmd;
//iCmd += (150* (pCmd - pos) + 0.6 * (vCmd - vel) + Ki * error); // Ki 0
//iCmd += (200* (pCmd - pos) + 0.6 * (vCmd - vel) + Ki * error); // Ki 0
//iCmd += (350* (pCmd - pos) + 1.0 * (vCmd - vel) + Ki * error); // Ki 0
iCmd += (150* (pCmd - pos) + 0.9 * (vCmd - vel) + Ki * error); // Ki 0
pos_prev = pos;
pCmd_prev = pCmd;
vCmd_prev = vCmd;
}
else {
iCmd = 0;
}
//**************************************************
//**************************************************
// control output
//
//limit current based on motor specs
if(iCmd > (MAX_CURRENT_MA) ){ // 1.6 mA
HW->SetAnalogOut(IoHardware::AMP_SIGNAL, 0.0);
HW->WriteDigitalBit(IoHardware::MOTOR_ENABLE, MOTOR_OFF);
HW->motorStatus = MOTOR_OFF;
//VNET->Process();
printf("iCmd:%f, pCmd:%f, pos:%f, vCmd:%f, vel:%f, aCmd:%f feedforwardVal:%f cnt:%d\n", iCmd, pCmd, pos, vCmd, vel, aCmd, feedforwardVal, cnt);
FATAL_ERROR("MOTOR CURRENT TOO HIGH");
lost = 1;
iCmd = MAX_CURRENT_MA;
}
else if(iCmd < (-MAX_CURRENT_MA) ){
HW->SetAnalogOut(IoHardware::AMP_SIGNAL, 0.0);
HW->WriteDigitalBit(IoHardware::MOTOR_ENABLE, MOTOR_OFF);
HW->motorStatus = MOTOR_OFF;
//VNET->Process();
printf("iCmd:%f, pCmd:%f, pos:%f, vCmd:%f, vel:%f, aCmd:%f feedforwardVal:%f cnt:%d%f\n", iCmd, pCmd, pos, vCmd, vel, aCmd, feedforwardVal, cnt);
FATAL_ERROR("MOTOR CURRENT TOO HIGH");
lost = 1;
iCmd = -MAX_CURRENT_MA;
}
ampVCmd =-iCmd * AMP_GAIN; // convert to analog signal. +-10 V. -0.86 for 4.5 rad/s. for test use -0.8
// sign change to agree with camera
// output signal to amp
if(!done){
HW->WriteAnalogCh(IoHardware::AMP_SIGNAL, ampVCmd);
//HW->WriteAnalogCh(IoHardware::AMP_SIGNAL, 0.0);
}
else{
HW->WriteAnalogCh(IoHardware::AMP_SIGNAL, 0.0);
}
//*******************************************************
//**************************************************
HW->ProcessOutput(); // all digital & analog writing.
// set outputs
/*num[0] = obj.theta;
num[1] = obj.angVel;
*/
num[0] = vInp;//obj.theta; //vInp; //correctedAcc; //eta1; //handVel; //pos; //handTheta; //pos; // angle in rad
num[1] = eta1;//obj.angVel; //eta1; //feedforwardVal; //pCmd; //eta2; //obj.theta; //vnum[0]; //obj.theta; //vel; //obj.theta;
num[2] = pos; //feedbackVal; //vCmd; //obj.angVel; //obj.x; //vnum[1]; //obj.x1; //ampVCmd; //obj.x1; // position in m, *10 for cm
num[3] = eta2; //handTheta; //iCmd; //sec; //elapsedSec; //sec; //vnum[2]; //obj.y1;
MNET->Process();
} // while()
// for some reason, this does not work.
HW->SetAnalogOut(IoHardware::AMP_SIGNAL, 0.0);
HW->WriteDigitalBit(IoHardware::MOTOR_ENABLE, MOTOR_OFF);
HW->motorStatus = MOTOR_OFF;
delay(10);
FATAL_ERROR(err_msg);
}
/**
* The start routine that is executed when the client calls start().
*/
void* Task::startRoutine()
{
int result;
uint64_t preStartCycleTime = 0;
uint64_t preEndCycleTime = 0;
uint64_t startCycleTime = 0;
uint64_t endCycleTime = 0;
uint64_t postEndCycleTime = 0;
// Set up some flags used to control task execution
bool firstRun = true;
computeComplete = 1;
testRunning = true;
preempted = false;
firstTimerRun = true;
// Reset the current compute time for this test
currentComputeTime = 0;
// Wait until we are released (a test begins)
sem_wait(&sem);
// Create the timer and kick it off
timer_create(CLOCK_REALTIME, &event, &timerID);
timer_settime(timerID, 0, &timerSpec, NULL);
sem_post(&proxySem);
// Intermittent wait that is used to make sure every task's timer is started
sem_wait(&sem);
// Jump into the test loop where the task will iteratively execute
// compute cycles when it is scheduled
while (testRunning)
{
// Block on execution semaphore (only after the first cycle)
if (!firstRun)
{
sem_wait(&sem);
preempted = false;
}
else
{
firstRun = false;
}
// Log pre-compute cycles
preEndCycleTime = 0;
preStartCycleTime = ClockCycles();
// Log the schedule event
TraceEvent(_NTO_TRACE_INSERTSUSEREVENT, EVENT_SCHEDULE, EVENT_SCHEDULE, uid);
scheduleList.push_back(uid);
// Begin/resume the compute cycle.
while (currentComputeTime < (computeTime * NS_PER_MS))
{
if (!preempted)
{
// Record start compute time jitter
if (preEndCycleTime == 0)
{
preEndCycleTime = ClockCycles();
}
// Burn and churn.
startCycleTime = ClockCycles();
result = nanospin(&burnTime);
endCycleTime = ClockCycles();
// Preemption means that nanospin took longer than expected, so handle that case.
if (!preempted)
{
realComputeTime += (endCycleTime - startCycleTime);
}
else
{
realComputeTime += TIME_QUANTUM; // unavoidable
}
// Check the nanospin return, just to be safe.
if (result == 0)
{
// We're okay - bump up the compute time.
currentComputeTime += TIME_QUANTUM;
totalComputationTime += TIME_QUANTUM;
}
else
{
cout << "Error: Task " << uid << " nanospin() returned: " << result << endl;
}
}
else
{
break; // Drop back to the execution semaphore.
}
}
// Check for deadline being hit
if (currentComputeTime >= (computeTime * NS_PER_MS))
{
// Update the new deadline and reset the compute time
currentComputeTime = 0;
computeComplete--;
totalComputationCycles++;
}
// Log post compute time cycles
postEndCycleTime = ClockCycles();
computeTransitionTime += ((postEndCycleTime - endCycleTime) +
(preEndCycleTime - preStartCycleTime));
// Give up the CPU for other tasks to execute
sched_yield();
}
// Suicide
kill();
// Delete the timer for the period - no longer needed
timer_delete(timerID);
}
int
main(int argc, char *argv[])
{
messip_channel_t *ch;
#if !defined(ONEWAY_MESSAGE)
char rec_buff[80];
#endif
char snd_buff[80];
int32_t answer;
int i;
double d;
#if defined(__QNXNTO__)
uint64_t cps, cycle1, cycle2, ncycles;
#else /*!__QNXNTO__ */
struct timespec before, after;
#endif /* __QNXNTO__ */
int q = 0;
#if 1
struct sched_param param;
if ((param.sched_priority=sched_get_priority_max(SCHED_FIFO)) == -1)
fprintf (stderr, "sched_get_priority_max(): %s\n", strerror (errno));
if (( sched_setscheduler(0, SCHED_FIFO, ¶m)) == -1)
fprintf (stderr, "sched_setscheduler(): %s\n", strerror (errno));
#endif
#if defined(__QNXNTO__)
/* find out how many cycles per second */
cps = SYSPAGE_ENTRY(qtime)->cycles_per_sec;
printf( "This system has %lld cycles/sec.\n",cps );
#endif /* __QNXNTO__ */
do {
// Locate the channel where to send message to
ch = messip_channel_connect(NULL, (argc < 2) ? "one" : argv[1], MESSIP_NOTIMEOUT);
assert(ch != NULL);
#if 1
// Send messages
for (i = 0; i < SEND_TIMES; i++) {
sprintf(snd_buff, "%d", i);
#if defined(__QNXNTO__)
cycle1=ClockCycles();
#else /*!__QNXNTO__ */
clock_gettime(CLOCK_REALTIME, &before);
#endif /* __QNXNTO__ */
#if defined(ONEWAY_MESSAGE)
messip_send(ch, 100, 200, (void *) snd_buff, strlen(snd_buff)+1, // Type=100 Subtype=200
&answer, NULL, -1, MESSIP_NOTIMEOUT);
#else
messip_send(ch, 100, 200, (void *) snd_buff, strlen(snd_buff)+1, // Type=100 Subtype=200
&answer, rec_buff, sizeof(rec_buff), MESSIP_NOTIMEOUT);
#endif
#if defined(__QNXNTO__)
cycle2=ClockCycles();
ncycles=cycle2-cycle1;
d=(double)ncycles/cps;
usleep(1000);
#else /*!__QNXNTO__ */
clock_gettime(CLOCK_REALTIME, &after);
d =
(after.tv_sec+after.tv_nsec/1e9)
-(before.tv_sec+before.tv_nsec/1e9);
#endif /* __QNXNTO__ */
#if defined(ONEWAY_MESSAGE)
printf("%.9f\n", d);
#else
printf("%s: %.9f\n", rec_buff, d);
#endif
}
if (i < SEND_TIMES) {
perror("messip_send()");
}
#endif
messip_channel_disconnect(ch, MESSIP_NOTIMEOUT);
} while(q++ < 3);
return 0;
}
void SampleLoopTask::Task() {
int cnt = 0;
int region;
int attempt;
static int dout = 1;
static double error = 0;
/*static int last_status = 0;
static double pos_prev = 0;
static double pCmd_prev = 0;
static double vCmd_prev = 0;
*/
int last_status = 0;
double pos_prev = 0;
double pCmd_prev = 0;
double vCmd_prev = 0;
int obj_ang_index = 0; // to keep continuous angle value when acrossing pi(or -pi).
double obj_x_offset = 0;
//double obj_y_offset = 0;
uint64_t cycle, cycle_prev;
// for acceleration calculation
double z = 0; //!
double h = 0; //! linearizing output
double hD = 0; //!
double hDD = 0; //!
double hDDD = 0; //!
double u =0; //
double eta2_prev = 0; //!
double eta2D = 0; //! to use eta2D in calculation of shD
double eta2D_prev = 0; //! to use eta2D in calculation of shD
double eta1_prev = 0; //!
double eta1D = 0; //! to use eta1D instead of sigma1 * sin(eta2)
double eta1D_prev = 0; //! to use eta1D instead of sigma1 * sin(eta2)
//double eta_a = 0.015;
//double eta_b = 0.02;
//double ETA2 = 0;
//double SIN_ETA2 = 0;
//int eta1D_flag = 0;
double err_sum = 0.0;
// for SMC
/*double s = 0;
double lambda = 3;
double eta = 0.1;
double F = 0;
double k = 0;
double fHat = 0;
double uHat = 0;
*/
//test
double feedforwardVal = 0;
double feedbackVal = 0;
//double test_v_prev = 0;
//double vCmd_prev = 0;
double elapsedSec = 0;
uint64_t cycle1, cycle2;
double elapsedSec1 = 0;
double elapsedSec2 = 0;
double obj_vel_uf = 0;
double vInp_prev = 0;
//double aCmd_sin = 0;
//int update_done = 0; // for automatic x_offset update
//double x_avg = 0; // for automatic x_offset update
//double x_max = 0;
//double x_min = 0;
while(!lost) {
TimerWait();
ncycles = ClockCycles();
sec=(double)(ncycles - ncycles_prev)/cps;
ncycles_prev = ncycles;
TimingProcess();
// Read Inputs
HW->ProcessInput(); // all digital & analog, encoders reading.
// Get status of camera
last_status = HW->ReadDigitalBit(IoHardware::FRAME_STATUS);
// Send out pulse to trigger camera
HW->WriteDigitalBit(IoHardware::CAMERA_TRIGGER, 1);
HW->WriteDigitalBit(IoHardware::CAMERA_TRIGGER, 0);
// test
//cycle1 = ClockCycles();
if(camera) {// && !lost){
// Wait for camera to process data, with timeout counter
while(HW->ReadDigitalBit(IoHardware::FRAME_STATUS) == last_status) {
if(++attempt == (int)(5.0e5/SAMPLE_RATE)) {
HW->SetAnalogOut(IoHardware::AMP_SIGNAL, 0.0);
HW->WriteDigitalBit(IoHardware::MOTOR_ENABLE, MOTOR_OFF);
HW->motorStatus = MOTOR_OFF;
delay(10);
FATAL_ERROR("Frame not received -");
//strcpy(err_msg, "Frame not received -");
lost = 1;
}
}
attempt = 0;
// test
//cycle2 = ClockCycles();
//elapsedSec1 = (double)(cycle2 - cycle1)/cps;
vnum[0] = -99;
int n = VNET->Recv();
//test
//cycle1 = ClockCycles();
//elapsedSec2 = (double)(cycle1 - cycle2)/cps;
region = (int)vnum[0];
switch(region) {
case 0: //ROI_0:
obj.x0 = vnum[1]; //mm
obj.y0 = vnum[2]; //mm
break;
case 1: //ROI_1:
obj.x1 = vnum[1]; //mm
obj.y1 = vnum[2]; //mm
break;
default:
//HW->SetAnalogOut(IoHardware::AMP_SIGNAL, 0.0);
//HW->WriteDigitalBit(IoHardware::MOTOR_ENABLE, MOTOR_OFF);
printf("roi-vnum[0]:%d\n", region);
FATAL_ERROR("Object lost -");
//strcpy(err_msg, "Object lost -");
lost = 1;
}
obj.x = (obj.x0 + obj.x1)/2/1000; // convert to m
obj.y = (obj.y0 + obj.y1)/2/1000;
double obj_raw_angle = atan2(obj.y1-obj.y0, obj.x1-obj.x0); // within -pi to pi
// compensating angle offset
// set only once at the beginning
/*if (obj_x_offset == 0 && obj.x0 != 0 && obj.x1 != 0) {
obj_x_offset = obj.x;
obj_y_offset = obj.y;
}*/
// object center position compensation
//obj.x = obj.x - 0.000250544*cos(obj_raw_angle + (-2.410447));
//uint64_t cycle1 = ClockCycles();
// to keep continuous angle value when acrossing pi(or -pi).
obj.theta = obj_raw_angle + obj_ang_index*2*3.141592; // converted angle
if ( fabs(obj.theta - obj.theta_prev) > 3.141592 ) {
if (obj.theta_prev > obj.theta) // pi to -pi region change
obj_ang_index++;
else
obj_ang_index--;
obj.theta = obj_raw_angle + obj_ang_index*2*3.141592; // newly converted angle
}
// calculation of the object angular velocity
//obj.angVel = (obj.theta - obj.theta_prev) * SAMPLE_RATE;
obj.angVel = (obj.theta - obj.theta_prev) / sec;
obj_vel_uf = obj.angVel;
// low pass filter
double alpha = 0.038; //0.02
obj.angVel = alpha*obj.angVel + (1-alpha)*obj.angVel_prev;
// calculation of the hand angular velocity
handTheta = -(HW->GetEncoderCount(IoHardware::ENC_0)) * ENC_RAD_PER_CNT / GR / 4; // just access the data previously read by GetEnc...()
handVel = (handTheta - handTheta_prev) /sec; //* SAMPLE_RATE;
//alpha = 0.1;
handVel = alpha*handVel + (1-alpha)*handVel_prev;
// calculation of sh and \dot sh (=shD)
sh = (obj.theta - handTheta) / K_R;
//shD = (obj.angVel - handVel) / K_R;
// coordinate transformation
//eta1 = M22*shD + M12*handVel;
// eta1D
//eta1D = (eta1 - eta1_prev) / sec;
//eta1D = alpha*eta1D + (1-alpha)*eta1D_prev;
//eta2 = handTheta + sh/RHO_H;
//obj_x_offset = OBJ_X_OFFSET + 0.0010775*sin(4*handTheta + 0.6422); // 0.0916609 or OBJ_X_OFFSET
//eta2 = asin(-(obj.x-obj_x_offset)/(RHO_H+RHO_O)); // alternative way to get eta2;
eta2 = asin(-(obj.x-OBJ_X_OFFSET)/(RHO_H+RHO_O)); // alternative way to get eta2;
//eta2 = atan(-(obj.x-OBJ_X_OFFSET)/(obj.y-OBJ_Y_OFFSET+RHO_H+RHO_O)); // alternative way to get eta2;
// xi = handVel;
//eta2 compensation
//eta2 = eta2 - 0.004*sin(obj.theta + 1.885);
// new method for shD, thus eta1 feedback
eta2D = (eta2 - eta2_prev) / sec;
eta2D = alpha*eta2D + (1-alpha)*eta2D_prev;
shD = RHO_H*(eta2D - handVel);
eta1 = M22*shD + M12*handVel;
// eta1D
// don't use this. this creates motor current too high
//eta1D = (eta1 - eta1_prev) / sec;
//eta1D = 0.005*eta1D + (1-0.005)*eta1D_prev;
//SIN_ETA2 = sin(eta2);
/*ETA2 = eta2;
if ( fabs(eta2) < eta_b ) {
if (eta2 > eta_a) {
//SIN_ETA2 = sin(2*eta2 - 0.02);
ETA2 = eta_b/(eta_b - eta_a)*(eta2 - eta_a);
}
else if (eta2 < -eta_a) {
ETA2 = eta_b/(eta_b - eta_a)*(eta2 + eta_a);
}
else {
ETA2 = 0;
}
}*/
// original
/*************************************************************/
// calculation of the control input (acceleration command)
z = handTheta - TARGET_THETAH;
h = eta2 - COEFF_sig3*z;
hD = eta2D - COEFF_sig3*handVel; // slightly better
//hD = COEFF_sig2*eta1;
hDD = COEFF_sig2*COEFF_sig1*sin(eta2);
//hDD = COEFF_sig2*eta1D; // use directly calculated (filtered) value for eta1D
/*if (eta1D_flag == 0) {
if ( fabs(eta1D - COEFF_sig2*sin(eta2)) < 0.02 ) {
eta1D_flag = 1;
}
else {
hDD = COEFF_sig2*COEFF_sig1*sin(eta2);
}
}*/
hDDD = COEFF_sig2*COEFF_sig1*cos(eta2)*eta2D; // slightly better
//hDDD = COEFF_sig2*COEFF_sig1*cos(eta2)*(COEFF_sig2*eta1 + COEFF_sig3*handVel);
/* For addition of integral control */
if(HW->motorStatus == MOTOR_ON) {
err_sum = err_sum + z*sec;
}
else {
err_sum = 0;
} // does not working successfully.
/// test
err_sum = 0;
u = -GAIN_K1*hDDD - GAIN_K2*hDD - GAIN_K3*hD - GAIN_K4*h + 100*err_sum;
vInp = ( u - COEFF_sig2*COEFF_sig1*sin(eta2)*(-eta2D + COEFF_sig1*COEFF_sig2*cos(eta2)) )/(COEFF_sig1*COEFF_sig2*COEFF_sig3*cos(eta2)); // slightly better
//vInp = ( u - hDD*(-eta2D + COEFF_sig1*COEFF_sig2*cos(eta2)) )/(COEFF_sig1*COEFF_sig2*COEFF_sig3*cos(eta2));
//vInp = ( u - COEFF_sig2*COEFF_sig1*sin(eta2)*(-COEFF_sig2*eta1-COEFF_sig3*handVel + COEFF_sig1*COEFF_sig2*cos(eta2)) )/(COEFF_sig1*COEFF_sig2*COEFF_sig3*cos(eta2));
//vInp = ( u - hDD*(-COEFF_sig2*eta1-COEFF_sig3*handVel + COEFF_sig1*COEFF_sig2*cos(eta2)) )/(COEFF_sig1*COEFF_sig2*COEFF_sig3*cos(eta2));
//vInp = ( u - COEFF_sig2*COEFF_sig1*sin(eta2)*(-COEFF_sig2*eta1-COEFF_sig3*handVel) - COEFF_sig1*COEFF_sig2*cos(eta2)*hDD)/(COEFF_sig1*COEFF_sig2*COEFF_sig3*cos(eta2));
/*************************************************************/
// Sliding Mode Control
/*s = hDDD + 3*lambda*hDD + 3*lambda*lambda*hD + lambda*lambda*lambda*h;
F = 0.02 * COEFF_sig1*COEFF_sig2*abs( -eta2D + COEFF_sig1*COEFF_sig2*cos(eta2) );
k = F + eta;
fHat = COEFF_sig1*COEFF_sig2*( -eta2D + COEFF_sig1*COEFF_sig2*cos(eta2) ) * eta2;
uHat = -fHat - 3*lambda*hDDD - 3*lambda*lambda*hDD - lambda*lambda*lambda*hD;
u = uHat - k*(s > 0 ? 1 : (s < 0 ? -1 : 0));
vInp = u /(COEFF_sig1*COEFF_sig2*COEFF_sig3*cos(eta2));
*/
//test input filter
//vInp = 0.5*vInp + (1-0.5)*vInp_prev;
aCmd = vInp; // calculated acceleration command
obj.theta_prev = obj.theta;
obj.angVel_prev = obj.angVel;
handTheta_prev = handTheta;
handVel_prev = handVel;
eta2D_prev = eta2D;
eta2_prev = eta2;
eta1_prev = eta1;
eta1D_prev = eta1D;
vInp_prev = vInp;
// For resetting of the upright eq. position
/*if(HW->motorStatus == MOTOR_ON && update_done == 0) {
x_avg += obj.x;
if (obj.x > x_max) {
x_max = obj.x;
}
if (obj.x < x_min) {
x_min = obj.x;
}
if ( (cnt%int(SAMPLE_RATE)) == 1 ) {
x_avg = x_avg / SAMPLE_RATE;
if ( (x_max - x_avg) < 0.001 && (x_avg - x_min) < 0.001 ) {
obj_x_offset = x_avg;
//x_offset_test = x_avg;
update_done = 1;
}
else {
x_avg = 0;
x_max = obj.x;
x_min = obj.x;
}
}
}*/
//uint64_t cycle2 = ClockCycles();
//elapsedSec=(double)(cycle2 - cycle1)/cps;
}
else { // Because the vision system keeps sending the data, QNX must read them, otherwise the vision system will get a send error.
VNET->Process();
//obj_ang_offset = 0.0; // reset the angle offset
//aCmd = 0;
//vCmd_prev = 0;
//pCmd_prev = 0;
}
// test
if (fabs(handVel) > 7.0 ) {//rad/sec
HW->SetAnalogOut(IoHardware::AMP_SIGNAL, 0.0);
HW->WriteDigitalBit(IoHardware::MOTOR_ENABLE, MOTOR_OFF);
HW->motorStatus = MOTOR_OFF;
delay(10);
FATAL_ERROR("MOTOR RUNS TOO FAST");
lost = 1;
}
//**************************************************************
// reading & calculating output
//
// feedback
cycle = ClockCycles();
elapsedSec = (double)(cycle - cycle_prev)/cps;
cycle_prev = cycle;
//HW->ProcessInput(); // because of the encoder reading. Do not use this again. This takes a long time.
//cycle2 = ClockCycles();
enc = -(HW->ReadEncoder(IoHardware::ENC_0)); // direct read. Not working?
//cycle1 = ClockCycles();
//elapsedSec2 = (double)(cycle1 - cycle2)/cps;
//enc = -(HW->GetEncoderCount(IoHardware::ENC_0)); // sign change to agree with camera
pos = (enc * ENC_RAD_PER_CNT) / GR / 4; // convert to rad. GR:gear ratio (50). 4?
//vel = (pos - pos_prev) * SAMPLE_RATE;
vel = (pos - pos_prev) / elapsedSec; // to calculate more exact velocity.
// This is necessary for not having motor run unexpectedly when swtiching from motor off to motor on.
if(HW->motorStatus == MOTOR_ON) {
cnt++;
/*double T_period = 0.2; //2;
// sinusoidal wave
aCmd = -2*DEG2RAD*(2*3.141592/T_period)*(2*3.141592/T_period)*sin(2*3.141592*(cnt)/SAMPLE_RATE/T_period);
//aCmd_sin = aCmd;
if (cnt == 1) { // at the beginning
// because of the sinusoidal wave
vCmd_prev = 2*DEG2RAD*(2*3.141592/T_period);
}
*/
// acceleration inner loop
// Notice that using no filtering velocity
vCmd = vCmd_prev + aCmd / SAMPLE_RATE;
pCmd = pCmd_prev + vCmd_prev / SAMPLE_RATE + 0.5 * aCmd / (SAMPLE_RATE*SAMPLE_RATE);
//pCmd = pCmd_prev + vCmd / SAMPLE_RATE;
//vCmd = vCmd_prev + aCmd * elapsedSec;
//pCmd = pCmd_prev + vCmd * elapsedSec;
iCmd = calcI(vCmd, aCmd); // feedforward control.
feedforwardVal = iCmd;
//iCmd += (150* (pCmd - pos) + 0.6 * (vCmd - vel) + Ki * error); // Ki 0
//iCmd += (200* (pCmd - pos) + 0.6 * (vCmd - vel) + Ki * error); // Ki 0
//iCmd += (350* (pCmd - pos) + 1.0 * (vCmd - vel) + Ki * error); // Ki 0
iCmd += (150* (pCmd - pos) + 0.9 * (vCmd - vel) + Ki * error); // Ki 0
pos_prev = pos;
pCmd_prev = pCmd;
vCmd_prev = vCmd;
}
else {
iCmd = 0;
}
//**************************************************
//**************************************************
// control output
//
//limit current based on motor specs
if(iCmd > (MAX_CURRENT_MA) ) { // 1.6 mA
HW->SetAnalogOut(IoHardware::AMP_SIGNAL, 0.0);
HW->WriteDigitalBit(IoHardware::MOTOR_ENABLE, MOTOR_OFF);
HW->motorStatus = MOTOR_OFF;
printf("iCmd:%f, pCmd:%f, pos:%f, vCmd:%f, vel:%f, aCmd:%f feedforwardVal:%f cnt:%d\n", iCmd, pCmd, pos, vCmd, vel, aCmd, feedforwardVal, cnt);
delay(10);
FATAL_ERROR("MOTOR CURRENT TOO HIGH");
lost = 1;
iCmd = MAX_CURRENT_MA;
}
else if(iCmd < (-MAX_CURRENT_MA) ) {
HW->SetAnalogOut(IoHardware::AMP_SIGNAL, 0.0);
HW->WriteDigitalBit(IoHardware::MOTOR_ENABLE, MOTOR_OFF);
HW->motorStatus = MOTOR_OFF;
printf("iCmd:%f, pCmd:%f, pos:%f, vCmd:%f, vel:%f, aCmd:%f feedforwardVal:%f cnt:%d%f\n", iCmd, pCmd, pos, vCmd, vel, aCmd, feedforwardVal, cnt);
delay(10);
FATAL_ERROR("MOTOR CURRENT TOO HIGH");
lost = 1;
iCmd = -MAX_CURRENT_MA;
}
ampVCmd =-iCmd * AMP_GAIN; // convert to analog signal. +-10 V. -0.86 for 4.5 rad/s. for test use -0.8
// sign change to agree with camera
// output signal to amp
if(!done) {
HW->WriteAnalogCh(IoHardware::AMP_SIGNAL, ampVCmd);
//HW->WriteAnalogCh(IoHardware::AMP_SIGNAL, 0.0);
}
else {
HW->WriteAnalogCh(IoHardware::AMP_SIGNAL, 0.0);
}
//*******************************************************
//**************************************************
HW->ProcessOutput(); // all digital & analog writing.
// keep the position angle within 2pi
//if ( pos > (mu_k+1)*2*3.141592 )
// mu_k++;
//pos = pos - mu_k*2*3.141592;
/*num[0] = sec; //aCmd; //correctedAcc; //eta1; //handVel; //pos; //handTheta; //pos; // angle in rad
num[1] = elapsedSec1; //test_v_prev; //feedforwardVal; //pCmd; //eta2; //obj.theta; //vnum[0]; //obj.theta; //vel; //obj.theta;
num[2] = elapsedSec2; //feedbackVal; //vCmd; //obj.angVel; //obj.x; //vnum[1]; //obj.x1; //ampVCmd; //obj.x1; // position in m, *10 for cm
num[3] = vCmd; //handTheta; //iCmd; //sec; //elapsedSec; //sec; //vnum[2]; //obj.y1;
*/
// set outputs
num[0] = vInp; //correctedAcc; //eta1; //handVel; //pos; //handTheta; //pos; // angle in rad
num[1] = eta1; //feedforwardVal; //pCmd; //eta2; //obj.theta; //vnum[0]; //obj.theta; //vel; //obj.theta;
num[2] = pos; //feedbackVal; //vCmd; //obj.angVel; //obj.x; //vnum[1]; //obj.x1; //ampVCmd; //obj.x1; // position in m, *10 for cm
num[3] = eta2; //handTheta; //iCmd; //sec; //elapsedSec; //sec; //vnum[2]; //obj.y1;
// acceleration innerloop
/*num[0] = aCmd; //aCmd;
num[1] = pCmd;
num[2] = pos;
//num[1] = feedforwardVal;
//num[2] = feedbackVal;
num[3] = vCmd;
*/
// motor modeling
/*num[0] = pos;
num[1] = iCmd;
num[2] = cnt;
num[3] = 0;
*/
MNET->Process();
} // while()
// for some reason, this does not work.
HW->SetAnalogOut(IoHardware::AMP_SIGNAL, 0.0);
HW->WriteDigitalBit(IoHardware::MOTOR_ENABLE, MOTOR_OFF);
HW->motorStatus = MOTOR_OFF;
delay(10);
FATAL_ERROR(err_msg);
}
void SampleLoopTask::Task() {
int cnt = 0;
int region;
int attempt;
static int dout = 1;
static int last_status = 0;
static double pos_prev = 0;
static double pCmd_prev = 0;
static double vCmd_prev = 0;
static double error = 0;
int temp = 0;
int mu_k = 0; // for test
int obj_ang_index = 0; // to keep continuous angle value when acrossing pi(or -pi).
//double obj_ang_offset = 0.0; // to subtract the object's angle at equilibrium position when inputting 'c' (camera)
double obj_x_offset = 0;
double obj_y_offset = 0;
uint64_t cycle, cycle_prev;
//test
double eta1_prev = 0;
double feedforwardVal = 0;
double feedbackVal = 0;
double test_v_prev = 0;
//double vCmd_prev = 0;
double elapsedSec = 0;
uint64_t cycle1, cycle2;
double elapsedSec1 = 0;
double elapsedSec2 = 0;
double aCmd_sin = 0;
double obj_vel_uf = 0;
double vInp_prev = 0;
double x_avg = 0; // for automatic x_offset update
double x_max = 0;
double x_min = 0;
//int update_count = 0; // for automatic x_offset update
//int update_trigger = 0; // for automatic x_offset update
int update_done = 0; // for automatic x_offset update
double x_offset_test = 0;
int switching_done = 0; // for controller swtiching
while(!lost) {
TimerWait();
ncycles = ClockCycles();
sec=(double)(ncycles - ncycles_prev)/cps;
ncycles_prev = ncycles;
TimingProcess();
// Read Inputs
HW->ProcessInput(); // all digital & analog, encoders reading.
// Get status of camera
last_status = HW->ReadDigitalBit(IoHardware::FRAME_STATUS);
// Send out pulse to trigger camera
HW->WriteDigitalBit(IoHardware::CAMERA_TRIGGER, 1);
HW->WriteDigitalBit(IoHardware::CAMERA_TRIGGER, 0);
// test
//cycle1 = ClockCycles();
if(camera) {// && !lost){
// Wait for camera to process data, with timeout counter
while(HW->ReadDigitalBit(IoHardware::FRAME_STATUS) == last_status) {
if(++attempt == (int)(1e8/SAMPLE_RATE)) {
HW->SetAnalogOut(IoHardware::AMP_SIGNAL, 0.0);
HW->WriteDigitalBit(IoHardware::MOTOR_ENABLE, MOTOR_OFF);
FATAL_ERROR("Frame not received -");
//strcpy(err_msg, "Frame not received -");
lost = 1;
}
}
attempt = 0;
// test
//cycle2 = ClockCycles();
//elapsedSec1 = (double)(cycle2 - cycle1)/cps;
vnum[0] = -99;
int n = VNET->Recv();
//test
//cycle1 = ClockCycles();
//elapsedSec2 = (double)(cycle1 - cycle2)/cps;
region = (int)vnum[0];
switch(region) {
case 0: //ROI_0:
obj.x0 = vnum[1]; //mm
obj.y0 = vnum[2]; //mm
break;
case 1: //ROI_1:
obj.x1 = vnum[1]; //mm
obj.y1 = vnum[2]; //mm
break;
default:
//HW->SetAnalogOut(IoHardware::AMP_SIGNAL, 0.0);
//HW->WriteDigitalBit(IoHardware::MOTOR_ENABLE, MOTOR_OFF);
printf("roi-vnum[0]:%d\n", region);
FATAL_ERROR("Object lost -");
//strcpy(err_msg, "Object lost -");
lost = 1;
}
obj.x = (obj.x0 + obj.x1)/2/1000; // convert to m
obj.y = (obj.y0 + obj.y1)/2/1000;
double obj_raw_angle = atan2(obj.y1-obj.y0, obj.x1-obj.x0); // within -pi to pi
// compensating angle offset
// set only once at the beginning
if (obj_x_offset == 0 && obj.x0 != 0 && obj.x1 != 0) {
obj_x_offset = obj.x;
obj_y_offset = obj.y;
x_offset_test = obj.x; // test
}
//uint64_t cycle1 = ClockCycles();
// to keep continuous angle value when acrossing pi(or -pi).
obj.theta = obj_raw_angle + obj_ang_index*2*3.141592; // converted angle
if ( fabs(obj.theta - obj.theta_prev) > 3.141592 ) {
if (obj.theta_prev > obj.theta) // pi to -pi region change
obj_ang_index++;
else
obj_ang_index--;
obj.theta = obj_raw_angle + obj_ang_index*2*3.141592; // newly converted angle
}
// calculation of the object angular velocity
//obj.angVel = (obj.theta - obj.theta_prev) * SAMPLE_RATE;
obj.angVel = (obj.theta - obj.theta_prev) / sec;
obj_vel_uf = obj.angVel;
// low pass filter
double alpha = 0.038; //0.06; //0.038; //0.02
obj.angVel = alpha*obj.angVel + (1-alpha)*obj.angVel_prev;
// calculation of the hand angular velocity
handTheta = -(HW->GetEncoderCount(IoHardware::ENC_0)) * ENC_RAD_PER_CNT / GR / 4; // just access the data previously read by GetEnc...()
handVel = (handTheta - handTheta_prev) / sec; //* SAMPLE_RATE;
//alpha = 0.1;
handVel = alpha*handVel + (1-alpha)*handVel_prev;
// calculation of sh and \dot sh (=shD)
sh = (obj.theta - handTheta) / K_R;
shD = (obj.angVel - handVel) / K_R;
// coordinate transformation
eta1 = M22*shD + M12*handVel;
//test filtering
//eta1 = alpha*eta1 + (1-alpha)*eta1_prev;
//eta1_prev = eta1;
//eta2 = handTheta + sh/RHO_H;
eta2 = asin(-(obj.x-obj_x_offset)/(RHO_H+RHO_O)); // alternative way to get eta2;
// calculation of the control input (acceleration command)
//xi := handVel;
//eta1D := COEFF_D * sin(eta2); // COEFF_D
//eta2D := COEFF_A * eta1 - xi/COEFF_B;
//double GAIN_C2 = GAIN_C2_FAR;
double GAIN_C2 = GAIN_C2_FAR;
/*if ( abs(eta2) < 0.03 && abs(handVel) < 0.2) { //0.015 ) { // eta2=0.015(0.86 deg) : 3.45 mm in x
//if ( abs(eta2) < 0.03 ) { //0.015 ) {
GAIN_C2 = GAIN_C2_NEAR;
}*/
double TANH = tanh(GAIN_C1*eta1 + GAIN_C2*eta2);
alpha_eta = COEFF_B * (COEFF_A*eta1 + GAIN_C0*TANH + GAIN_C3*eta2);
DalphaDeta1 = COEFF_B * ( COEFF_A + GAIN_C0*(1-TANH*TANH)*GAIN_C1 );
DalphaDeta2 = COEFF_B * ( GAIN_C0*(1-TANH*TANH)*GAIN_C2 + GAIN_C3 );
vInp = -GAIN_C * (handVel - alpha_eta) + DalphaDeta1*(COEFF_D * sin(eta2)) + DalphaDeta2*(COEFF_A * eta1 - handVel/COEFF_B);
//vInp = 0.05*vInp + (1-0.05)*vInp_prev;
aCmd = vInp; // calculated acceleration command
obj.theta_prev = obj.theta;
obj.angVel_prev = obj.angVel;
handTheta_prev = handTheta;
handVel_prev = handVel;
vInp_prev = vInp;
// automatic x offset update (old)
/*if (eta2*eta2 + handVel*handVel/100 < 0.00325) {
update_trigger = 1;
}
if (update_trigger == 1 && update_done != 1) {
update_count += 1;
x_avg += obj.x;
if (update_count > SAMPLE_RATE) { // averaging for 1 sec.
//obj_x_offset = x_avg / update_count;
x_offset_test = x_avg / update_count;
update_trigger = 0;
update_count = 0;
update_done = 1;
}
}*/
// automatic x offset update
/*if(HW->motorStatus == MOTOR_ON && update_done == 0) {
x_avg += obj.x;
if (obj.x > x_max) {
x_max = obj.x;
}
if (obj.x < x_min) {
x_min = obj.x;
}
if ( (cnt%int(SAMPLE_RATE)) == 1 ) {
x_avg = x_avg / SAMPLE_RATE;
if ( (x_max - x_avg) < 0.001 && (x_avg - x_min) < 0.001 ) {
//obj_x_offset = x_avg;
x_offset_test = x_avg;
update_done = 1;
}
else {
x_avg = 0;
x_max = obj.x;
x_min = obj.x;
}
}
}
*/
// Switching to a Linear Controller
/*if (HW->motorStatus == MOTOR_ON) {
if (switching_done == 1) {
aCmd = -(2565.2*eta1 + 292.0*eta2 + 16.1*handVel); // pole placement:sigma = 2.3, zeta = 0.85, third pole at 5sigma
}
else if ( eta2*eta2 + 0.01*handVel*handVel < 0.003125 ) {
switching_done = 1;
}
}*/
//uint64_t cycle2 = ClockCycles();
//elapsedSec=(double)(cycle2 - cycle1)/cps;
}
else { // Because the vision system keeps sending the data, QNX must read them, otherwise the vision system will get a send error.
VNET->Process();
//obj_ang_offset = 0.0; // reset the angle offset
//aCmd = 0;
//vCmd_prev = 0;
//pCmd_prev = 0;
}
// test
if (fabs(handVel) > 5.0 ) {//rad/sec
HW->SetAnalogOut(IoHardware::AMP_SIGNAL, 0.0);
HW->WriteDigitalBit(IoHardware::MOTOR_ENABLE, MOTOR_OFF);
HW->motorStatus = MOTOR_OFF;
FATAL_ERROR("MOTOR RUNS TOO FAST");
}
//**************************************************************
// reading & calculating output
//
// feedback
cycle = ClockCycles();
elapsedSec = (double)(cycle - cycle_prev)/cps;
cycle_prev = cycle;
//HW->ProcessInput(); // because of the encoder reading. Do not use this again. This takes a long time.
//cycle2 = ClockCycles();
enc = -(HW->ReadEncoder(IoHardware::ENC_0)); // direct read. Not working?
//cycle1 = ClockCycles();
//elapsedSec2 = (double)(cycle1 - cycle2)/cps;
//enc = -(HW->GetEncoderCount(IoHardware::ENC_0)); // sign change to agree with camera
pos = (enc * ENC_RAD_PER_CNT) / GR / 4; // convert to rad. GR:gear ratio (50). 4?
//vel = (pos - pos_prev) * SAMPLE_RATE;
vel = (pos - pos_prev) / elapsedSec; // to calculate more exact velocity.
// This is necessary for not having motor run unexpectedly when swtiching from motor off to motor on.
if(HW->motorStatus == MOTOR_ON) {
cnt++;
/*double T_period = 0.2; //2;
// sinusoidal wave
aCmd = -2*DEG2RAD*(2*3.141592/T_period)*(2*3.141592/T_period)*sin(2*3.141592*(cnt)/SAMPLE_RATE/T_period);
//aCmd_sin = aCmd;
if (cnt == 1) { // at the beginning
// because of the sinusoidal wave
vCmd_prev = 2*DEG2RAD*(2*3.141592/T_period);
}
*/
// acceleration inner loop
// Notice that using no filtering velocity
vCmd = vCmd_prev + aCmd / SAMPLE_RATE;
pCmd = pCmd_prev + vCmd_prev / SAMPLE_RATE + 0.5 * aCmd / (SAMPLE_RATE*SAMPLE_RATE);
//pCmd = pCmd_prev + vCmd / SAMPLE_RATE;
//vCmd = vCmd_prev + aCmd * elapsedSec;
//pCmd = pCmd_prev + vCmd * elapsedSec;
//test
test_v_prev = vCmd_prev;
iCmd = calcI(vCmd, aCmd); // feedforward control. Not being used currently.
feedforwardVal = iCmd;
//iCmd += (150* (pCmd - pos) + 0.6 * (vCmd - vel) + Ki * error); // Ki 0
//iCmd += (200* (pCmd - pos) + 0.6 * (vCmd - vel) + Ki * error); // Ki 0
//iCmd += (350* (pCmd - pos) + 1.0 * (vCmd - vel) + Ki * error); // Ki 0
iCmd += (150* (pCmd - pos) + 0.9 * (vCmd - vel) + Ki * error); // Ki 0
pos_prev = pos;
pCmd_prev = pCmd;
vCmd_prev = vCmd;
}
else {
iCmd = 0;
}
//**************************************************
//**************************************************
// control output
//
//limit current based on motor specs
if(iCmd > (MAX_CURRENT_MA) ) { // 1.6 mA
HW->SetAnalogOut(IoHardware::AMP_SIGNAL, 0.0);
HW->WriteDigitalBit(IoHardware::MOTOR_ENABLE, MOTOR_OFF);
HW->motorStatus = MOTOR_OFF;
printf("iCmd:%f, pCmd:%f, pos:%f, vCmd:%f, vel:%f, feedforwardVal:%f\n", iCmd, pCmd, pos, vCmd, vel, feedforwardVal);
FATAL_ERROR("MOTOR CURRENT TOO HIGH");
iCmd = MAX_CURRENT_MA;
}
else if(iCmd < (-MAX_CURRENT_MA) ) {
HW->SetAnalogOut(IoHardware::AMP_SIGNAL, 0.0);
HW->WriteDigitalBit(IoHardware::MOTOR_ENABLE, MOTOR_OFF);
HW->motorStatus = MOTOR_OFF;
printf("iCmd:%f, pCmd:%f, pos:%f, vCmd:%f, vel:%f, feedforwardVal:%f\n", iCmd, pCmd, pos, vCmd, vel, feedforwardVal);
FATAL_ERROR("MOTOR CURRENT TOO HIGH");
iCmd = -MAX_CURRENT_MA;
}
ampVCmd =-iCmd * AMP_GAIN; // convert to analog signal. +-10 V. -0.86 for 4.5 rad/s. for test use -0.8
// sign change to agree with camera
// output signal to amp
if(!done) {
HW->WriteAnalogCh(IoHardware::AMP_SIGNAL, ampVCmd);
//HW->WriteAnalogCh(IoHardware::AMP_SIGNAL, 0.0);
}
else {
HW->WriteAnalogCh(IoHardware::AMP_SIGNAL, 0.0);
}
//*******************************************************
//**************************************************
HW->ProcessOutput(); // all digital & analog writing.
// keep the position angle within 2pi
//if ( pos > (mu_k+1)*2*3.141592 )
// mu_k++;
//pos = pos - mu_k*2*3.141592;
/*num[0] = sec; //aCmd; //correctedAcc; //eta1; //handVel; //pos; //handTheta; //pos; // angle in rad
num[1] = elapsedSec1; //test_v_prev; //feedforwardVal; //pCmd; //eta2; //obj.theta; //vnum[0]; //obj.theta; //vel; //obj.theta;
num[2] = elapsedSec2; //feedbackVal; //vCmd; //obj.angVel; //obj.x; //vnum[1]; //obj.x1; //ampVCmd; //obj.x1; // position in m, *10 for cm
num[3] = vCmd; //handTheta; //iCmd; //sec; //elapsedSec; //sec; //vnum[2]; //obj.y1;
*/
// set outputs
/*num[0] = obj_vel_uf; //vInp; //correctedAcc; //eta1; //handVel; //pos; //handTheta; //pos; // angle in rad
num[1] = vel; //eta1; //feedforwardVal; //pCmd; //eta2; //obj.theta; //vnum[0]; //obj.theta; //vel; //obj.theta;
num[2] = obj.angVel; //feedbackVal; //vCmd; //obj.angVel; //obj.x; //vnum[1]; //obj.x1; //ampVCmd; //obj.x1; // position in m, *10 for cm
num[3] = aCmd; //handTheta; //iCmd; //sec; //elapsedSec; //sec; //vnum[2]; //obj.y1;
*/
num[0] = handVel;
num[1] = aCmd; //x_offset_test; // //obj_x_offset;
num[2] = switching_done; //obj.x;
num[3] = eta2;
/*num[0] = handVel;
num[1] = eta1;
num[2] = vel;
num[3] = eta2;
*/
/*num[0] = vInp;
num[1] = eta1;
num[2] = pos;
num[3] = eta2;
*/
// acceleration innerloop
/*num[0] = aCmd;
num[1] = pCmd;
num[2] = pos;
//num[1] = feedforwardVal;
//num[2] = feedbackVal;
num[3] = vCmd;
*/
// motor modeling
/*num[0] = pos;
num[1] = iCmd;
num[2] = cnt;
num[3] = 0;
*/
MNET->Process();
} // while()
// for some reason, this does not work.
HW->SetAnalogOut(IoHardware::AMP_SIGNAL, 0.0);
HW->WriteDigitalBit(IoHardware::MOTOR_ENABLE, MOTOR_OFF);
HW->motorStatus = MOTOR_OFF;
delay(10);
FATAL_ERROR(err_msg);
}
main(int argc,char *argv[])
{
CoolImage *image;
int x,y;
int i,j;
PtWidget_t *win;
PtArg_t args[3];
PhDim_t dim={m_W,m_H};
PhPoint_t pos={50,250};
int fd; //Bt878 driver file descriptor
int fd_temp;
int size_read;
struct timeval tv;
fd_set rfd;
int n;
int error;
int counter = 0;
int counter_mean = 0;
int file = 0;
//Timing calculation
uint64_t cps, cycle1, cycle2, ncycles;
float sec;
float msec;
// if a paramater was passed, grab it as the blit type
if (argc>1) blittype=atoi(argv[1]);
// initialize our connection to Photon, and create/realize a window
//PtInit("/net/irene2/dev/photon");
PtInit("/dev/photon");
PtSetArg(&args[0],Pt_ARG_POS,&pos,0);
PtSetArg(&args[1],Pt_ARG_DIM,&dim,0);
win=PtCreateWidget(PtWindow,Pt_NO_PARENT,2,args);
PtRealizeWidget(win);
// Allocate and fill a series of NUMIMAGES images with a little
// fading type animation. Put your own animation in here if you like.
/*
* Set a 5 second timeout.
*/
tv.tv_sec = 5;
tv.tv_usec = 0;
image = AllocBuffer(m_W,m_H,fd);
assert(image!=0);
if (file != 2)
{
init_bttvx(2,0, m_W,m_H,0,0);
open_bttvx();
BttvxSetImageBuffer(0, image->buffer);
}
fd_temp = fd;
FD_ZERO( &rfd );
FD_SET( fd, &rfd );
while(1)
{
//fd = open("/net/europa/dev/bttvx0",O_RDWR);
//if ( fd > 0 )
//{
///switch ( n = select( 1 + max( fd,0 ),
/// &rfd, 0, 0, &tv ) )
///{
/// case -1:
/// perror( "select" );
/// return EXIT_FAILURE;
/// case 0:
/// puts( "select timed out" );
/// break;
/// default:
//printf( "descriptor ready ...\n");
//if( FD_ISSET( console, &rfd ) )
// puts( " -- console descriptor has data pending" );
//if( FD_ISSET( serial, &rfd ) )
// puts( " -- serial descriptor has data pending" );
/* Read the text */
cycle1=ClockCycles( );
//lseek(fd,0L,SEEK_SET);
/// size_read = read( fd, image->buffer, W*H*deep );
if (file != 2)
{
BttvxWaitEvent();
BttvxAcquireBuffer(image->buffer);
}
switch(file)
{
case 0:
BlitBuffer(win,image);
break;
case 1:
SaveImage(counter,image);
break;
case 2:
LoadImage(counter,image);
BlitBuffer(win,image);
getchar();
break;
};
if (file!=2)
BttvxReleaseBuffer();
cycle2=ClockCycles( );
counter++;
counter_mean++;
ncycles=cycle2-cycle1;
//printf("%lld cycles elapsed \n", ncycles);
/* find out how many cycles per second */
cps = SYSPAGE_ENTRY(qtime)->cycles_per_sec;
//printf( "This system has %lld cycles/sec.\n",cps );
sec=(float)ncycles/cps;
msec +=sec;
if (counter_mean == 250 )
{
msec = msec/counter_mean;
printf("The cycles in seconds is %f \n",msec);
counter_mean = 0;
msec = 0;
}
//}else
//sleep(2);
}
//printf("Blitted %d frames using method %d\n",REPS*NUMIMAGES,blittype);
// now free the images
FreeBuffer(image);
close( fd );
/// hide the window and destroy it.
PtUnrealizeWidget(win);
PtDestroyWidget(win);
}
