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