// This should be called after followSegment to drive to the // center of an intersection. It also uses the line sensors to // detect left, straight, and right exits. void driveToIntersectionCenter(bool * foundLeft, bool * foundStraight, bool * foundRight) { *foundLeft = 0; *foundStraight = 0; *foundRight = 0; // Drive stright forward to get to the center of the // intersection, while simultaneously checking for left and // right exits. // // readSensors() takes approximately 2 ms to run, so we use // it for our loop timing. A more robust approach would be // to use millis() for timing. motors.setSpeeds(straightSpeed, straightSpeed); for(uint16_t i = 0; i < intersectionDelay / 2; i++) { readSensors(); if(aboveLine(0)) { *foundLeft = 1; } if(aboveLine(4)) { *foundRight = 1; } } readSensors(); // Check for a straight exit. if(aboveLine(1) || aboveLine(2) || aboveLine(3)) { *foundStraight = 1; } }
int main(int argc, char *argv[]) { QApplication::setGraphicsSystem("raster"); QApplication app(argc, argv); QMainWindow mainWindow; mainWindow.resize(500, 300); mainWindow.setWindowTitle("PS Move API Labs - Sensor Filter"); QWidget centralWidget; QBoxLayout layout(QBoxLayout::LeftToRight); centralWidget.setLayout(&layout); QTimer timer; MoveGraph moveGraph; layout.addWidget(&moveGraph); mainWindow.setCentralWidget(¢ralWidget); QObject::connect(&timer, SIGNAL(timeout()), &moveGraph, SLOT(readSensors())); timer.start(1); mainWindow.show(); return app.exec(); }
void noreturn main(void) { init(); initSensors(); for(;;) { while(!loopActive) { ; // wait for next loop } readSensors(); attitudeCalculation(); control(); actuate(); triggerSonar(); input(); output(); leds(); ATOMIC_BLOCK(ATOMIC_FORCEON) { lastLoopTicks = ticksSinceLoopStart; loopActive = false; } } }
void Mirobot::collideHandler(){ readSensors(); if(_collideStatus == NORMAL){ if(leftCollide){ _collideStatus = LEFT_REVERSE; back(50); }else if(rightCollide){ _collideStatus = RIGHT_REVERSE; back(50); }else{ if(rightMotor.ready() && leftMotor.ready()){ forward(1000); } } }else if(rightMotor.ready() && leftMotor.ready()){ switch(_collideStatus){ case LEFT_REVERSE : _collideStatus = LEFT_TURN; right(90); break; case RIGHT_REVERSE : _collideStatus = RIGHT_TURN; left(90); break; case LEFT_TURN : case RIGHT_TURN : _collideStatus = NORMAL; case NORMAL : break; } } }
task main() { while(true) { readSensors(); if (touchL != 0 || touchR != 0) { avoidObstacle(); } else if (lightDetected && facingLight) { driveForward(); } else if (lightDetected && !facingLight) { turnTowardsLight(); } else { //Default case scanForLight(); } } }
void WUManager::init() { sendRequest(); QThread* workHorse = new QThread(this); QTimer* timer = new QTimer(0); QEventLoop* loop = new QEventLoop(); timer->setInterval(900000); // every hour read weather data timer->moveToThread(workHorse); connect(this, SIGNAL(replyProcessed()), loop, SLOT(quit()) ); connect(timer, SIGNAL(timeout()), this, SLOT(sendRequest())); connect(workHorse, SIGNAL(started()), timer, SLOT(start())); workHorse->start(); loop->exec(); QThread* workHorseSensors = new QThread(this); QTimer* timerSensors = new QTimer(0); timerSensors->setInterval(1000); timerSensors->moveToThread(workHorse); connect(timerSensors, SIGNAL(timeout()), this, SLOT(readSensors())); connect(workHorseSensors, SIGNAL(started()), timerSensors, SLOT(start())); workHorseSensors->start(); }
void Mirobot::sensorNotifier(){ StaticJsonBuffer<60> outBuffer; JsonObject& outMsg = outBuffer.createObject(); if(collideNotify){ collideState_t currentCollideState = collideState(); if(currentCollideState != lastCollideState){ lastCollideState = currentCollideState; switch(currentCollideState){ case BOTH: outMsg["msg"] = "both"; marcel.notify("collide", outMsg); break; case LEFT: outMsg["msg"] = "left"; marcel.notify("collide", outMsg); break; case RIGHT: outMsg["msg"] = "right"; marcel.notify("collide", outMsg); break; } } } if(followNotify){ readSensors(); int currentFollowState = leftLineSensor - rightLineSensor; if(currentFollowState != lastFollowState){ outMsg["msg"] = currentFollowState; marcel.notify("follow", outMsg); } lastFollowState = currentFollowState; } }
// Turns according to the parameter dir, which should be 'L' // (left), 'R' (right), 'S' (straight), or 'B' (back). We turn // most of the way using the gyro, and then use one of the line // sensors to finish the turn. We use the inner line sensor that // is closer to the target line in order to reduce overshoot. void turn(char dir) { if (dir == 'S') { // Don't do anything! return; } turnSensorReset(); uint8_t sensorIndex; switch(dir) { case 'B': // Turn left 125 degrees using the gyro. motors.setSpeeds(-turnSpeed, turnSpeed); while((int32_t)turnAngle < turnAngle45 * 3) { turnSensorUpdate(); } sensorIndex = 1; break; case 'L': // Turn left 45 degrees using the gyro. motors.setSpeeds(-turnSpeed, turnSpeed); while((int32_t)turnAngle < turnAngle45) { turnSensorUpdate(); } sensorIndex = 1; break; case 'R': // Turn right 45 degrees using the gyro. motors.setSpeeds(turnSpeed, -turnSpeed); while((int32_t)turnAngle > -turnAngle45) { turnSensorUpdate(); } sensorIndex = 3; break; default: // This should not happen. return; } // Turn the rest of the way using the line sensors. while(1) { readSensors(); if (aboveLine(sensorIndex)) { // We found the line again, so the turn is done. break; } } }
// Calibrates the line sensors by turning left and right, then // displays a bar graph of calibrated sensor readings on the LCD. // Returns after the user presses A. static void lineSensorSetup() { lcd.clear(); lcd.print(F("Line cal")); // Delay so the robot does not move while the user is still // touching the button. delay(1000); // We use the gyro to turn so that we don't turn more than // necessary, and so that if there are issues with the gyro // then you will know before actually starting the robot. turnSensorReset(); // Turn to the left 90 degrees. motors.setSpeeds(-calibrationSpeed, calibrationSpeed); while((int32_t)turnAngle < turnAngle45 * 2) { lineSensors.calibrate(); turnSensorUpdate(); } // Turn to the right 90 degrees. motors.setSpeeds(calibrationSpeed, -calibrationSpeed); while((int32_t)turnAngle > -turnAngle45 * 2) { lineSensors.calibrate(); turnSensorUpdate(); } // Turn back to center using the gyro. motors.setSpeeds(-calibrationSpeed, calibrationSpeed); while((int32_t)turnAngle < 0) { lineSensors.calibrate(); turnSensorUpdate(); } // Stop the motors. motors.setSpeeds(0, 0); // Show the line sensor readings on the LCD until button A is // pressed. lcd.clear(); while(!buttonA.getSingleDebouncedPress()) { readSensors(); lcd.gotoXY(0, 0); for (uint8_t i = 0; i < numSensors; i++) { uint8_t barHeight = map(lineSensorValues[i], 0, 1000, 0, 8); printBar(barHeight); } } lcd.clear(); }
void loop() { // read input: readSensors(); // draw the screen: refreshScreen(); }
void SensorModule::readWorker(){ while(shouldRead){ std::string data = readSensors(); Message* message = new Message(PHONE_HOME, data); broadcast(message); sleep(readInterval); //TODO find sleep solution } }
void appMain(void) { uint16_t i; // ------------------------- serial number DPRINTF("Mote %#04x starting...\n", localAddress); // ------------------------- external flash #if WRITE_TO_FLASH prepareExtFlash(); #endif // ------------------------- light sensors if (localAddress != 0x0796) { PRINT("init isl\n"); islInit(); islOn(); } else { PRINT("init ads\n"); adsInit(); adsSelectInput(0); } // ------------------------- main loop mdelay(3000); DPRINTF("starting main loop...\n"); for (i = 0; i < 6; ++i) { redLedToggle(); mdelay(100); } ledOff(); uint32_t nextDataReadTime = 0; uint32_t nextBlinkTime = 0; for (;;) { uint32_t now = getRealTime(); if (timeAfter32(now, nextDataReadTime)) { if (getJiffies() < 300 * 1000ul ) { nextDataReadTime = now + DATA_INTERVAL_SMALL; } else { nextDataReadTime = now + DATA_INTERVAL; } DataPacket_t packet; readSensors(&packet); #if PRINT_PACKET printPacket(&packet); #endif } if (timeAfter32(now, nextBlinkTime)) { nextBlinkTime = now + BLINK_INTERVAL; ledOn(); msleep(100); ledOff(); } msleep(1000); } }
inline void actionUntilColor(Colors c, void (*action)(void)) { Colors left, right; do { readSensors(left, right); action(); } while (left != c && right != c); stop(); }
uint8_t LineSensors::readSensor(uint8_t _sensor){ uint8_t _sensorsReading; if(_sensor > LINESENSORS_SENSOR6 ){ printf("Error reading the Line Sensor...\n"); return 0x00; } _sensorsReading = readSensors(); return (_sensorsReading >> _sensor) & 0x01; }
collideState_t Mirobot::collideState(){ readSensors(); if(leftCollide && rightCollide){ return BOTH; }else if(leftCollide){ return LEFT; }else if(rightCollide){ return RIGHT; }else{ return NONE; } }
inline bool followColorUntilColor(Colors c1, Colors c2) { Colors left, right; readSensors(left, right); if (left == c2 && right == c2) { stop(); delay(50); return true; } else if (left == c1 && right == c2) turnRight(); else if (left == c2 && right == c1) turnLeft(); else if (left == c2 && right == BLACK) forward(); else if (left == BLACK && right == c2) forward(); else if (left == c2 && right != c2) turnLeft(); else if (left != c2 && right == c2) turnRight(); else if (left == c1 && right == c1) forward(); else if (left == c1 && right != c1) turnLeft(); else if (left != c1 && right == c1) turnRight(); else if (left != c1 && right != c1) { Serial.println("Both off c1"); bool turning = false, turningLeft = !isBot1; int i = 0, prevMillis; do { if (!turning) { prevMillis = millis(); (turningLeft) ? turnLeft() : turnRight(); turning = true; turningLeft = !turningLeft; } /* turn other way after 100*i milliseconds */ if (millis() > (200 * i) + prevMillis) { turning = false; ++i; } readSensors(left, right); if (left == c2 || right == c2) return true; } while (left != c1 && right != c1); stop(); } delay(150); return false; }
// This function causes the robot to follow a line segment until // it detects an intersection, a dead end, or a dark spot. void followSegment() { while(1) { // Get the position of the line. uint16_t position = readSensors(); // Our "error" is how far we are away from the center of the // line, which corresponds to position 2000. int16_t error = (int16_t)position - 2000; // Compute the difference between the two motor power // settings, leftSpeed - rightSpeed. int16_t speedDifference = error / 4; // Get individual motor speeds. The sign of speedDifference // determines if the robot turns left or right. int16_t leftSpeed = (int16_t)straightSpeed + speedDifference; int16_t rightSpeed = (int16_t)straightSpeed - speedDifference; // Constrain our motor speeds to be between 0 and straightSpeed. leftSpeed = constrain(leftSpeed, 0, (int16_t)straightSpeed); rightSpeed = constrain(rightSpeed, 0, (int16_t)straightSpeed); motors.setSpeeds(leftSpeed, rightSpeed); // We use the inner four sensors (1, 2, 3, and 4) for // determining whether there is a line straight ahead, and the // sensors 0 and 5 for detecting lines going to the left and // right. // // This code could be improved by skipping the checks below // if less than 200 ms has passed since the beginning of this // function. Maze solvers sometimes end up in a bad position // after a turn, and if one of the far sensors is over the // line then it could cause a false intersection detection. if(!aboveLine(0) && !aboveLine(1) && !aboveLine(2) && !aboveLine(3) && !aboveLine(4)) { // There is no line visible ahead, and we didn't see any // intersection. Must be a dead end. break; } if(aboveLine(0) || aboveLine(4)) { // Found an intersection or a dark spot. break; } } }
void work() { try { working_.store(true); while(working_.load()) { readSensors(); std::this_thread::sleep_for(period_); } } catch (std::exception& e) { ERROR << "Poller::exception: " << e.what(); } catch (...) { ERROR << "Poller::unknown error"; } }
void appMain(void) { // ------------------------- serial number PRINTF("Mote %#04x starting...\n", localAddress); // ------------------------- light sensors islInit(); islOn(); for (;;) { DataPacket_t packet; readSensors(&packet); printPacket(&packet); mdelay(2000); } }
void MiniBeeV2::doLoopStep(void){ int bytestoread; // do something based on current status: switch( status ){ case SENSING: //send( N_INFO, "sensing" ); // read sensors: datacount = readSensors( datacount ); if ( curSample >= samplesPerMsg ){ sendData(); curSample = 0; datacount = 0; } delay( smpInterval ); break; case STARTING: // send( N_INFO, "starting", 8 ); delay( 100 ); break; case WAITFORCONFIG: // send( N_INFO, "waitforconfig" ); delay( 100 ); break; case WAITFORHOST: // send( N_INFO, "waitforhost" ); delay( 100 ); break; case ACTING: delay( smpInterval ); break; case PAUSING: delay( 500 ); break; } // read any new data from XBee: // do this at the end, so status change happens at the beginning of next loop bytestoread = Serial.available(); if ( bytestoread > 0 ){ // send( N_INFO, "reading" ); for ( i = 0; i < bytestoread; i++ ){ read(); } // } else { // send( N_INFO, "no data" ); } }
int main(int args, char *argv[], char *environ[]) { driver = (I2C_COGDRIVER*) malloc(sizeof(I2C_COGDRIVER)); i2cOpen(driver, SCL_PIN, SDA_PIN, 400000); comp = (float*) malloc(3 * sizeof(float)); acc = (float*) malloc(3 * sizeof(float)); gyro = (float*) malloc(3 * sizeof(float)); clock_t previous; clock_t current; clock_t cyclesElapsed; time_ms timeElapsed; setupSensors(); previous = clock(); while (1) { readSensors(); current = clock(); if (current > previous) { cyclesElapsed = current - previous; } else { cyclesElapsed = current + (UINT_MAX - previous); } timeElapsed = (time_ms) cyclesElapsed; timeElapsed /= 4000; //imu_update(timeElapsed, acc, comp, gyro); printVectorLine("compass", comp); printVectorLine("accelerometer", acc); printVectorLine("gyrometer", gyro); printMatrixLine("dcm", dcmEst); previous = current; } return 0; }
float LineSensors::readLine(){ uint8_t inputs = readSensors(); if(inputs == 0x00) return -1.0; int cumSum = 0; float avg = 0; for(uint8_t i = 0; i < 6; i++){ if(inputs & (1<<i)){ avg += 100 * (i*100); cumSum += 100; } } avg = avg/cumSum; // printf("%0.2f\n",avg); return avg; }
/** * \brief Top level run method. * * This is the top level run method. It triggers the sensor readings, * controller run methods and the WebServer processing. Needs to be called * periodically i.e. within the loop() function of the Arduino environment. */ void Aquaduino::run() { static int8_t curMin = minute(); #ifndef INTERRUPT_DRIVEN readSensors(); executeControllers(); #endif if (isXivelyEnabled() && minute() != curMin) { curMin = minute(); Serial.print(F("Sending data to Xively... ")); Serial.println(m_XivelyClient.put(*m_XivelyFeed, m_XivelyAPIKey)); } if (m_GUIServer != NULL) { m_GUIServer->run(); } }
void Sensors::determineMPUBias() //TODO: compass? { int counter = 0; int sampleCount = 500; double invSampleCount = 0.002; // 1/500 Vector3D::Vector invGravity; invGravity = vec3.double2vec(0, 0, 16384); // Gravity is -16384 but want to zero everything to find bias while (counter < sampleCount) { readSensors(); accel = vec3.add(accel, invGravity); accel = vec3.multiply(accel, invSampleCount); accelBias = vec3.add(accelBias, accel); gyro = vec3.multiply(gyro, invSampleCount); gyroBias = vec3.add(gyroBias, gyro); counter++; } // vec3.prettyPrint(accelBias, "ACCEL BIAS", "ticks"); // vec3.prettyPrint(gyroBias, "GYRO BIAS", "ticks"); // vec3.quickPrint(accelBias); // vec3.quickPrint(gyroBias); }
task main() { while(true) { readSensors(); if (lightDetected && facingLight) { motor[motorC] = 0; motor[motorB] = 0; } else if (lightDetected && !facingLight) { turnTowardsLight(); } else { //Default case scanForLight(); } } }
void loop() { tsCurr = getTimestamp(); if (diffTimestamps(tsCurr, tsLastSensorRead) >= SENSORS_READ_INTERVAL_SEC) { readSensors(); tsLastSensorRead = tsCurr; digitalWrite(HEARTBEAT_LED, digitalRead(HEARTBEAT_LED) ^ 1); } if (serial->available() > 0) { processSerialMsg(); } if (wifi->available() > 0) { processWifiMsg(); } if (diffTimestamps(tsCurr, tsLastStatusReport) >= STATUS_REPORTING_PERIOD_SEC) { reportStatus(); tsLastStatusReport = tsCurr; } tsPrev = tsCurr; }
void getOrientation() { readSensors(); float const PI_F = 3.14159265F; // i2cRoll: Rotation around the X-axis. -180 <= i2cRoll <= 180 // a positive i2cRoll angle is defined to be a clockwise rotation about the positive X-axis // y // i2cRoll = atan2(---) // z // where: y, z are returned value from accelerometer sensor i2cRoll = (float)atan2(ay, az); // i2cPitch: Rotation around the Y-axis. -180 <= i2cRoll <= 180 // a positive i2cPitch angle is defined to be a clockwise rotation about the positive Y-axis // -x // i2cPitch = atan(-------------------------------) // y * sin(i2cRoll) + z * cos(i2cRoll) // where: x, y, z are returned value from accelerometer sensor if (ay * sin(i2cRoll) + az * cos(i2cRoll) == 0) i2cPitch = ax > 0 ? (PI_F / 2) : (-PI_F / 2); else i2cPitch = (float)atan(-ax / (ay * sin(i2cRoll) + az * cos(i2cRoll))); // i2cHeading: Rotation around the Z-axis. -180 <= i2cRoll <= 180 // a positive i2cHeading angle is defined to be a clockwise rotation about the positive Z-axis // z * sin(i2cRoll) - y * cos(i2cRoll) // i2cHeading = atan2(--------------------------------------------------------------------------) // x * cos(i2cPitch) + y * sin(i2cPitch) * sin(i2cRoll) + z * sin(i2cPitch) * cos(i2cRoll)) // where: x, y, z are returned value from magnetometer sensor // i2cHeading = (float)atan2(mz * sin(i2cRoll) - my * cos(i2cRoll), mx * cos(i2cPitch) + my * sin(i2cPitch) * sin(i2cRoll) + mz * sin(i2cPitch) * cos(i2cRoll)); // Convert angular data to degree i2cRoll = - i2cRoll * 180.0 / PI_F; i2cPitch = i2cPitch * 180.0 / PI_F; i2cHeading = - i2cHeading * 180.0 / PI_F; }
// --------------------------------------- motors_thread_main -----------------------------------// int motors_thread_main(int argc, char *argv[]){ systeminitialization(); warnx("[motors test] starting\n"); sleep(2); thread_running = true; while (!thread_should_exit) { //warnx("Hello daemon!\n"); readSensors(); setESC(); } warnx("[motors test] exiting.\n"); thread_running = false; return 0; }
/*lint -save -e970 Disable MISRA rule (6.3) checking. */ int main(void) /*lint -restore Enable MISRA rule (6.3) checking. */ { /* Write your local variable definition here */ uint16_t sensorsVector[NUMBER_OF_SENSORS]; /* variables needed for deciding which direction it should turn more */ uint16_t robotTurnsRightMotor; uint16_t robotTurnsLeftMotor; /* in case of a white detection - it may be that simply the robot got completely * out of the track - but it should get back to the truck in case it was * just loosing a bit the track */ uint16_t flagStopWhite; /* correction algorith will relay on counting how bad * the curve (error), figuring out how many times the * sensor is activated */ uint16_t correctionS1; uint16_t correctionS2; uint16_t i; // for serial debug only flagStopWhite=0; /* initialize the correction counters * they follow how often in a loop cycle the error appears */ correctionS1 = 0; correctionS2 = 0; /*** Processor Expert internal initialization. DON'T REMOVE THIS CODE!!! ***/ PE_low_level_init(); /*** End of Processor Expert internal initialization. ***/ /* Write your code here */ initializePlatform(); /* Wait for a button press before doing anything*/ while(Button_GetVal()) {} while (1) { /* Read the input sensors */ readSensors(sensorsVector); robotTurnsLeftMotor = sensorsVector[0] + sensorsVector[1] + sensorsVector[2]; robotTurnsRightMotor = sensorsVector[3] + sensorsVector [4] + sensorsVector[5]; #if 1 for (i = 0; i < NUMBER_OF_SENSORS; i++) { Term2_SendNum(sensorsVector[i]); Term2_SendChar(' '); } Term2_SendChar('R'); Term2_SendChar('='); Term2_SendNum(robotTurnsRightMotor); Term2_SendChar(' '); Term2_SendChar('L'); Term2_SendChar('='); Term2_SendNum(robotTurnsLeftMotor); Term2_CRLF(); //setRightMotorSpeed(FULL_SPEED); //setLeftMotorSpeed(FULL_SPEED); /* motor right turns faster case robot turns left as issue is on the left side */ if (robotTurnsRightMotor > robotTurnsLeftMotor){ /* if last sensor is seeing the black line * is pretty bad so speed has to be pretty high * also if situation repeats correction must be more dramatic * */ if (sensorsVector[0]==0 && sensorsVector[1]!= 0){ if(correctionS1 < TOLERANCE_S1) correctionS1= correctionS1 + CORRECTION_STEP_S1; setRightMotorSpeed(FULL_SPEED); setRightMotorSpeed(SPEED_MOTOR_S1_HIGH_REFERENCE + correctionS1); setLeftMotorSpeed(SPEED_MOTOR_S1_LOW_REFERENCE - correctionS1); /* clean up correction flag for RS2*/ WAIT1_Waitus(WAIT_SENSORS_1); correctionS2 = 0; } /* the case in which second sensor sees black line * less critical but also with potential of becoming an * issue */ else if( sensorsVector[1]==0){ if(correctionS2 < TOLERANCE_S2) correctionS2 = correctionS2 + CORRECTION_STEP_S2; setRightMotorSpeed(SPEED_MOTOR_S2_HIGH_REFERENCE + correctionS2); setLeftMotorSpeed(SPEED_MOTOR_S2_LOW_REFERENCE - correctionS2); /* clean up the correction flag for RS1 */ correctionS1 = 0; WAIT1_Waitus(WAIT_SENSORS_2); } flagStopWhite=0; } /* motor left turns robot goes to right as issue is on the right side */ if ( robotTurnsLeftMotor > robotTurnsRightMotor){ /* if last sensor is seeing the black line * is pretty bad so speed has to be pretty high * also if situation repeats correction must be more dramatic */ if (sensorsVector[5]==0 && sensorsVector[4]!=0){ if(correctionS1 < TOLERANCE_S1) correctionS1= correctionS1 + CORRECTION_STEP_S1; setRightMotorSpeed(SPEED_MOTOR_S1_LOW_REFERENCE - correctionS1); setLeftMotorSpeed(SPEED_MOTOR_S1_HIGH_REFERENCE + correctionS1); /* clean up correction flag for S2*/ WAIT1_Waitus(WAIT_SENSORS_1); correctionS2 = 0; } /* the case in which second sensor sees black line * less critical but also with potential of becoming an * issue */ else if (sensorsVector[4]==0){ if(correctionS2 < TOLERANCE_S2) correctionS2 = correctionS2 + CORRECTION_STEP_S2; setRightMotorSpeed(SPEED_MOTOR_S2_LOW_REFERENCE - correctionS2); setLeftMotorSpeed(SPEED_MOTOR_S2_HIGH_REFERENCE + correctionS2); /* clean up the correction flag for RS1 */ correctionS1 = 0; WAIT1_Waitus(WAIT_SENSORS_2); } flagStopWhite=0; } //case line is correctly read and it should move full speed if(robotTurnsRightMotor!=0 && robotTurnsLeftMotor != 0 && sensorsVector[2]==0 && sensorsVector[3]==0){ if(robotTurnsRightMotor==robotTurnsLeftMotor && robotTurnsRightMotor== 2){ setRightMotorSpeed(FULL_SPEED); setLeftMotorSpeed (FULL_SPEED); WAIT1_Waitus(WAIT_NORMAL); correctionS1 = 0; correctionS2 = 0; flagStopWhite=0; } } //case when it is on the black it also stops if (robotTurnsRightMotor==0 && robotTurnsLeftMotor == 0 ){ setRightMotorSpeed(0); setLeftMotorSpeed (0); correctionS1 = 0; correctionS2 = 0; flagStopWhite=0; } // case when it is on white board not sensor read black it stops if (robotTurnsRightMotor==3 && robotTurnsLeftMotor == 3 ){ if(flagStopWhite==TIME_TO_STOP_ON_WHITE){ setRightMotorSpeed(0); setLeftMotorSpeed (0); } else{ flagStopWhite++; WAIT1_Waitus(WAIT_NORMAL); } } #endif } /*** Don't write any code pass this line, or it will be deleted during code generation. ***/ /*** RTOS startup code. Macro PEX_RTOS_START is defined by the RTOS component. DON'T MODIFY THIS CODE!!! ***/ #ifdef PEX_RTOS_START PEX_RTOS_START(); /* Startup of the selected RTOS. Macro is defined by the RTOS component. */ #endif /*** End of RTOS startup code. ***/ /*** Processor Expert end of main routine. DON'T MODIFY THIS CODE!!! ***/ for(;;){} /*** Processor Expert end of main routine. DON'T WRITE CODE BELOW!!! ***/ } /*** End of main routine. DO NOT MODIFY THIS TEXT!!! ***/
robot::robot(std::string filename,bool hideCollisionLinks,bool hideJoints,bool convexDecomposeNonConvexCollidables,bool createVisualIfNone,bool showConvexDecompositionDlg,bool centerAboveGround,bool makeModel,bool noSelfCollision,bool positionCtrl): filenameAndPath(filename) { printToConsole("URDF import operation started."); openFile(); readJoints(); readLinks(); readSensors(); createJoints(hideJoints,positionCtrl); createLinks(hideCollisionLinks,convexDecomposeNonConvexCollidables,createVisualIfNone,showConvexDecompositionDlg); createSensors(); std::vector<int> parentlessObjects; std::vector<int> allShapes; std::vector<int> allObjects; std::vector<int> allSensors; for (int i=0;i<int(vLinks.size());i++) { if (simGetObjectParent(vLinks[i]->nLinkVisual)==-1) parentlessObjects.push_back(vLinks[i]->nLinkVisual); allObjects.push_back(vLinks[i]->nLinkVisual); allShapes.push_back(vLinks[i]->nLinkVisual); if (vLinks[i]->nLinkCollision!=-1) { if (simGetObjectParent(vLinks[i]->nLinkCollision)==-1) parentlessObjects.push_back(vLinks[i]->nLinkCollision); allObjects.push_back(vLinks[i]->nLinkCollision); allShapes.push_back(vLinks[i]->nLinkCollision); } } for (int i=0;i<int(vJoints.size());i++) { if (vJoints[i]->nJoint!=-1) { if (simGetObjectParent(vJoints[i]->nJoint)==-1) parentlessObjects.push_back(vJoints[i]->nJoint); allObjects.push_back(vJoints[i]->nJoint); } } for (int i=0;i<int(vSensors.size());i++) { if (vSensors[i]->nSensor!=-1) { if (simGetObjectParent(vSensors[i]->nSensor)==-1) parentlessObjects.push_back(vSensors[i]->nSensor); allObjects.push_back(vSensors[i]->nSensor); allSensors.push_back(vSensors[i]->nSensor); } if (vSensors[i]->nSensorAux!=-1) { allObjects.push_back(vSensors[i]->nSensorAux); allSensors.push_back(vSensors[i]->nSensorAux); } } // If we want to alternate respondable mask: if (!noSelfCollision) { for (int i=0;i<int(parentlessObjects.size());i++) setLocalRespondableMaskCummulative_alternate(parentlessObjects[i],true); } // Now center the model: if (centerAboveGround) { bool firstValSet=false; C3Vector minV,maxV; for (int shNb=0;shNb<int(allShapes.size());shNb++) { float* vertices; int verticesSize; int* indices; int indicesSize; if (simGetShapeMesh(allShapes[shNb],&vertices,&verticesSize,&indices,&indicesSize,NULL)!=-1) { C7Vector tr; simGetObjectPosition(allShapes[shNb],-1,tr.X.data); C3Vector euler; simGetObjectOrientation(allShapes[shNb],-1,euler.data); tr.Q.setEulerAngles(euler); for (int i=0;i<verticesSize/3;i++) { C3Vector v(vertices+3*i); v*=tr; if (!firstValSet) { minV=v; maxV=v; firstValSet=true; } else { minV.keepMin(v); maxV.keepMax(v); } } simReleaseBuffer((char*)vertices); simReleaseBuffer((char*)indices); } } C3Vector shiftAmount((minV+maxV)*-0.5f); shiftAmount(2)+=(maxV(2)-minV(2))*0.5f; for (int i=0;i<int(parentlessObjects.size());i++) { C3Vector p; simGetObjectPosition(parentlessObjects[i],-1,p.data); p+=shiftAmount; simSetObjectPosition(parentlessObjects[i],-1,p.data); } } // Now create a model bounding box if that makes sense: if ((makeModel)&&(parentlessObjects.size()==1)) { int p=simGetModelProperty(parentlessObjects[0]); p|=sim_modelproperty_not_model; simSetModelProperty(parentlessObjects[0],p-sim_modelproperty_not_model); for (int i=0;i<int(allObjects.size());i++) { if (allObjects[i]!=parentlessObjects[0]) { int p=simGetObjectProperty(allObjects[i]); simSetObjectProperty(allObjects[i],p|sim_objectproperty_selectmodelbaseinstead); } } for (int i=0;i<int(allSensors.size());i++) { if (allSensors[i]!=parentlessObjects[0]) { int p=simGetObjectProperty(allSensors[i]); simSetObjectProperty(allSensors[i],p|sim_objectproperty_dontshowasinsidemodel); // sensors are usually large and it is ok if they do not appear as inside of the model bounding box! } } } // Now select all new objects: simRemoveObjectFromSelection(sim_handle_all,-1); for (int i=0;i<int(allObjects.size());i++) simAddObjectToSelection(sim_handle_single,allObjects[i]); printToConsole("URDF import operation finished.\n\n"); }