void NavSystem::NavActivity::init(NavSystem* nav) {
this->nav = nav;
pid.init(0.02, 0.0, 0.07);
unsigned long now = runningTime();
lastStateTime = now;
lastLineTime = now;
}
void AMLoopAction3::internalOnCurrentActionProgressChanged(double numerator, double denominator)
{
if(internalAllActionsHaveExpectedDuration()) {
double totalNumerator = 0, totalDenominator = 0;
for(int i=0, cc=subActionCount(); i<cc; i++) {
AMAction3* action = subActionAt(i);
double expectedSecs = action->info()->expectedDuration();
totalDenominator += expectedSecs*loopCount();
int timesFinished = currentIteration_;
if(i < currentSubActionIndex_)
timesFinished++;
totalNumerator += expectedSecs*timesFinished;
if(i == currentSubActionIndex()) // if it's the current action, we have some partial progress.
totalNumerator += numerator/denominator * expectedSecs;
}
setProgress(totalNumerator, totalDenominator);
setExpectedDuration(totalDenominator);
}
// Otherwise, assume every subaction makes up an equal unit ('1') of the total amount of work. Our denominator will be the number of sub-actions * the number of loops, and our numerator will be the sub-actions we've completed (plus the current sub-action's fraction done).
else {
double totalDenominator = subActionCount()*loopCount();
double totalNumerator = subActionCount()*currentIteration_; // add the completed full loops
totalNumerator += currentSubActionIndex_; // add the actions done in this loop
totalNumerator += numerator/denominator; // add the fraction done for the current action.
setProgress(totalNumerator, totalDenominator);
setExpectedDuration(runningTime()*totalDenominator/totalNumerator);
}
}
int main() {
Matrix img;
clock_t tic, toc;
tic = clock();
imread( "pics/paint_86.bmp", &img );
//dump( &img, "img(color)", ALL, 0, img.size1-1, 0, img.size2-1, INT );
color2Gray( &img );
//dump( &img, "img(gray)", ALL, 0, img.size1-1, 0, img.size2-1, INT );
//gray2Color( &img );
//dump( &img, "img(gray)", ALL, 0, img.size1-1, 0, img.size2-1, INT );
imwrite( "pics/imwrite.bmp", &img, RED );
toc = clock();
runningTime( tic, toc );
return 0;
}
void AMControlMoveAction::onProgressTick()
{
double destination = control_->setpoint();
double fractionComplete = (control_->value() - startPosition_.value()) / (destination - startPosition_.value());
double runningSeconds = runningTime();
double expectedTotalSeconds = runningSeconds / fractionComplete;
if(fractionComplete != 0) {
// set expected duration based on time taken so far and distance travelled to destination.
setExpectedDuration(expectedTotalSeconds);
setProgress(runningSeconds, expectedTotalSeconds); // exactly equal to fractionComplete. We observe the convention that when possible, progress numerator and denominator should be in seconds.
}
else {
setProgress(0, 100);
// in this case, no idea on the expected duration (unless we knew the speed of the control)
}
}
MainWindow::MainWindow(QWidget *parent) :
QMainWindow(parent),
ui(new Ui::MainWindow)
{
ui->setupUi(this);
/////////////////////////////////////
///
///
///
h_WidgetChannel1 = AddNewChannelWidget( 0, 780, 91 );
h_WidgetChannel1->SetPosition( 780, 91 );
h_WidgetChannel2 = AddNewChannelWidget( 1, 780, 170 );
h_WidgetChannel3 = AddNewChannelWidget( 2, 780, 250 );
h_WidgetChannel4 = AddNewChannelWidget( 3, 780, 330 );
h_WidgetChannel5 = AddNewChannelWidget( 4, 780, 410 );
//////////////////////////////////////
///
///
///
dwYRange = 2.0;
h_Channel1 = new Channel( ui->customPlot );
h_Channel2 = new Channel( ui->customPlot );
h_Channel3 = new Channel( ui->customPlot );
h_Channel4 = new Channel( ui->customPlot );
h_Channel5 = new Channel( ui->customPlot );
h_Channel1->AddChannel( QPen(Qt::blue) ); h_WidgetChannel1->setLabelColor( Qt::blue );
h_Channel2->AddChannel( QPen(Qt::red ) ); h_WidgetChannel2->setLabelColor( Qt::red );
h_Channel3->AddChannel( QPen(Qt::yellow ) ); h_WidgetChannel3->setLabelColor( Qt::yellow );
h_Channel4->AddChannel( QPen(Qt::black ) ); h_WidgetChannel4->setLabelColor( Qt::black );
h_Channel5->AddChannel( QPen(Qt::green ) ); h_WidgetChannel5->setLabelColor( Qt::green );
dwGrid = 10.0;
ui->sliderGrid->setValue( 10 );
ui->customPlot->xAxis->setAutoTickStep( false );
ui->customPlot->yAxis->setAutoTickStep( false );
ui->customPlot->xAxis->setTickStep( dwGrid );
ui->customPlot->axisRect()->setupFullAxesBox( );
///
/// Status bar
///
dwPktCount = 0;
dwErrorCount = 0;
h_lblSerialPort = new QLabel( "Porta fechada" );
h_lblMsgCount = new QLabel( "0" );
h_lblErrors = new QLabel( "Erros: 0" );
h_lblActiveTime = new QLabel( "00:00:00" );
h_lblSerialPort->setFixedWidth( 200 );
h_lblMsgCount->setFixedWidth( 100 );
h_lblErrors->setFixedWidth( 100 );
h_lblActiveTime->setFixedWidth( 100 );
ui->statusBar->addWidget( h_lblSerialPort );
ui->statusBar->addWidget( h_lblMsgCount );
ui->statusBar->addWidget( h_lblErrors );
ui->statusBar->addWidget( h_lblActiveTime );
//////////////////////////////////////////////////////////////////////////////
///
///
ui->sliderAmplitude->setValue( 20 ); /// 2.0
///
/// make left and bottom axes transfer their ranges to right and top axes:
///
connect(ui->customPlot->xAxis, SIGNAL(rangeChanged(QCPRange)), ui->customPlot->xAxis2, SLOT(setRange(QCPRange)));
connect(ui->customPlot->yAxis, SIGNAL(rangeChanged(QCPRange)), ui->customPlot->yAxis2, SLOT(setRange(QCPRange)));
///
///
///
connect(&dataTimer, SIGNAL(timeout()), this, SLOT(realtimeDataSlot()));
dataTimer.start( 100 );
ui->sliderTimer->setValue( 100 );
///
///
///
connect(&tmrCaptureCountTime, SIGNAL(timeout()), this, SLOT(runningTime()));
////////////////////////////
///
///
h_RS = new SerialCommunication( this );
connect( h_RS, SIGNAL(DataArrive(QString)), this, SLOT(SerialDataArrive(QString)) );
h_RS->PopulateSerialList();
QStringList serialList = h_RS->getSerialList();
int i = 0;
ui->cmbSerialList->clear();
while ( i != serialList.count() )
{
ui->cmbSerialList->addItem( serialList.at( i ) );
i++;
}
h_lblSerialPort->setText( "Porta serial fechada" );
dwDivisor = 0;
ui->sliderDivisor->setValue( 0 );
}
int main(int argc, char *argv[]) {
DIR *dir;
ofstream fout;
char slash = Unix ? '/' : '\\'; // It is '/' or '\' depending on OS
char POS_PATH_BASE[MAX_PATH_LENGTH];
char NEG_PATH_BASE[MAX_PATH_LENGTH];
clock_t tic, toc;
if (argc != 4) {
error("Usage: train [POS_DIR] [NEG_DIR] [OUTPUT_FILE].");
}
if (!(dir = opendir(argv[1]))) {
error("train: [POS_DIR] open failed.");
}
/* Set POS_PATH_BASE to [POS_DIR] and append '/' or '\' */
strcpy(POS_PATH_BASE, argv[1]);
sprintf(POS_PATH_BASE, "%s%c", POS_PATH_BASE, slash);
closedir(dir);
if (!(dir = opendir(argv[2]))) {
error("train: [NEG_DIR] open failed.");
}
/* Set NEG_PATH_BASE to [NEG_DIR] and append '/' or '\' */
strcpy(NEG_PATH_BASE, argv[2]);
sprintf(NEG_PATH_BASE, "%s%c", NEG_PATH_BASE, slash);
closedir(dir);
fout.open(argv[3]);
if (!(fout)) {
error("train: [OUTPUT_FILE] open failed.");
}
/** Command is correct **/
echoOS();
cout << "This program provides cascaded-AdaBoost training.\n" <<
"Please make sure that [POS_DIR] and [NEG_DIR] contain only training image data,\n" <<
" whose size is " << WINDOW_WIDTH << " x " << WINDOW_HEIGHT << ".\n" <<
"Press [Enter] to continue, or ctrl+C/D/Z to exit ...";
getchar();
tic = clock();
/* Use only one large matrix for storing all POS / NEG feature data */
CvMat *POS = NULL, *NEG = NULL;
int N1, N2; /* number of positive / negative images */
int blockCount = 0; /* number of blocks in an image */
/* [1] Feature extraction */
/* First, check for validity of extraction parameters */
assert(!(360 % (BIN_NUM * 2))); // (360 / BIN_NUM / 2) should be an integer */
assert(360 / BIN_NUM == BIN_SIZE);
assert(BIN_SIZE >> 1 == HALF_BIN_SIZE);
cout << "\nStart of feature extraction ...\n";
/* Should pass (CvMat *&) to really create the matrices */
extractAll(POS_PATH_BASE, POS, N1, blockCount);
cout << "Extraction of POS data completed.\n";
#if CHECKNaN
cout << "Check POS matrix for NaN values:" << endl;
if (checkNaN(POS, "POS")) {
error("POS matrix contains NaN values!");
}
else {
cout << "OK!" << endl;
}
#endif
extractAll(NEG_PATH_BASE, NEG, N2, blockCount);
cout << "Extraction of NEG data completed.\n";
#if CHECKNaN
cout << "Check NEG matrix for NaN values:" << endl;
if (checkNaN(NEG, "NEG")) {
error("NEG matrix contains NaN values!");
}
else {
cout << "OK!" << endl;
}
#endif
assert(blockCount > 0);
cout << endl << "# of blocks per image: " << blockCount << ".\n";
#if GETCHAR
getchar();
#endif
/* [2] Cascaded-AdaBoost training */
cout << "Start of cascaded-AdaBoost training ...\n";
srand(time(NULL));
float F_current = 1.0; // current overall false positive rate
int i = 1; // AdaBoost stage counter
int rejectCount = 0; // # of negative images rejected so far
/* Allocate rejection table */
/** Note: All the xxxTable[]'s are of boolean flags **/
bool *rejectTable = new bool[ N2 ];
memset(rejectTable, 0, N2);
bool stop = false; // Stopping flag
vector<AdaStrong> H; // A cascade of AdaBoost strong classifiers
/*** The training algorithm ***/
while (F_current > F_target && !stop) {
/* upper bound for j */
int jEnd = (i == 1) ? (ni + 1) : ni;
for (int j = 1; j <= jEnd; j++) {
/* Learn an A[i,j] stage */
cout << "\nLearning stage A[" << i << "," << j << "]...\n";
if ((stop = learnA(N1, N2, blockCount, rejectCount, rejectTable, POS, NEG, H, F_current))) {
break;
}
cout << "Stage A[" << i << "," << j << "] completed.\n";
#if GETCHAR
getchar();
#endif
}
#if META
if (stop) break;
/* Learn a M[i] stage */
cout << "\nLearning stage M[" << i << "]...\n";
learnM();
cout << "Stage M[" << i << "," << j << "] completed.\n";
#if GETCHAR
getchar();
#endif
#endif
i++;
} // End of loop while (F_current > F_target && !stop)
cout << "The entire training process completed.\nWriting model parameters to " << argv[3] << " ... ";
/* Write model parameters to [OUTPUT_FILE]. See docs/train_format.txt for explanation. */
writeModel(H, fout);
cout << "done!\n";
/* Show running time */
toc = clock();
runningTime(tic, toc);
/* Don't forget to close the file stream */
fout.close();
return 0;
}
bool NavSystem::NavActivity::run() {
unsigned long now = runningTime();
unsigned long timeSinceLastState = now-lastStateTime;
unsigned long timeSinceLastLine = now-lastLineTime;
// Always read line sensor (so we know where the line was last seen
unsigned int sensorValues[NUM_SENSORS];
long position = nav->qtrrc8.readLine(sensorValues);
// Check if we've crossed an intersection
// This is checked by seening if the average of the sensor values is >900
if (timeSinceLastLine > kLineTimeout) {
long sum = 0;
for (int i = 0; i < NUM_SENSORS; ++i) {
sum += sensorValues[i];
}
if (sum > kIntersectionThresh*NUM_SENSORS) {
// If we've found and intersection, and are currently counting intersections,
// count it
if (nav->current_command.type == FOLLOW_COUNT) --nav->current_command.data;
// Reset time since we last saw a line.
lastLineTime = now;
}
}
// If the robot is not enabled, stop driving, and return
if(!nav->robot->status.enabled()) {
nav->stop();
return false;
}
switch (nav->current_command.type) {
case BACK_UP: {
int back_up_time = kNormalBackupTime;
if (nav->current_pos == REACTOR_B ||
nav->current_pos == REACTOR_A ) {
back_up_time = kReactorBackupTime;
}
if (timeSinceLastState > back_up_time) {
nav->stop();
nav->next();
lastStateTime = now;
} else {
nav->drive(50,-50);
}
break;
}
case FORWARD:
if (timeSinceLastState > 200) {
nav->stop();
nav->next();
lastStateTime = now;
} else {
nav->drive(-50,50);
}
break;
// Turning around follows the same process as turning left or right:
// turn until you don't see the line, then turn until the line is
// roughly centered.
case TURN_AROUND:
case TURN_LEFT: {
long sum = 0;
for (int i = 0; i < NavSystem::NUM_SENSORS; ++i) {
sum += sensorValues[i];
}
if (sum < 100*NUM_SENSORS) { nav->current_command.data = 1; }
if ((nav->current_command.data == 1) && ((position > 3000 && position < 4000))) {
nav->stop();
nav->next();
lastStateTime = now;
} else {
nav->drive(60,60);
}
break;
}
case TURN_RIGHT: {
long sum = 0;
for (int i = 0; i < NavSystem::NUM_SENSORS; ++i) {
sum += sensorValues[i];
}
if (sum < kNoLineThresh*NUM_SENSORS) { nav->current_command.data = 1; }
if ((nav->current_command.data == 1) && ((position > 3000 && position < 4000))) {
nav->stop();
nav->next();
lastStateTime = now;
} else {
nav->drive(-60,-60);
}
break;
}
case FOLLOW_LIMIT:
if (digitalRead(FRONT_BUMP_PORT)) {
followLine(position);
} else {
nav->current_pos = nav->desired_pos;
nav->current_dir = nav->desired_dir;
lastStateTime = now;
nav->next();
nav->stop();
}
break;
case FOLLOW_COUNT:
followLine(position);
if (nav->current_command.data <= 0) {
nav->stop();
nav->next();
lastStateTime = now;
}
break;
case DONE:
nav->stop();
break;
}
return false;
}
void NavSystem::NavActivity::resetStateTime() {
lastStateTime = runningTime();
}