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); }