void drive() { float targetValues[6] = { 0.591398, 0.522972, 0.532747, 0.796676, 0.821114, 0.904203}; // this is where we will store the current position we want float caryPose[6] = { 0.591398, 0.522972, 0.532747, 0.796676, 0.821114, 0.904203}; // this is the pose to cary an object. Use your poser program from earlier to create your own values float pressures[8] = { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f }; float values[6] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; float pickupPose[6]={0.777126,0.469208,0.454545,0.938416,0.782014,0.826002}; // this is the pose to pick up an object. Use your poser program from earlier to create your own values float errors[6] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; // we want to know how far away we are from the desired values we will store that error here float tolerance=.02; // this is how far we are willing to be off from our desired values float vel[3] = {0.0, 0.0, 0.0}; // the velocity see the rectangle tutorial for more info unsigned int * seq; double x; // x position double y; // y positon double phi; // rotation bool inTolerance=false; // are we in acceptable tolerance for arm position bool closed=false; // is the claw closed float stepSize=.5; float lastTime=com.msecsElapsed(); float lowerAvg; float upperAvg; for( unsigned int i = 0; i < 6; ++i ){ targetValues[i]=pickupPose[i]; } while( com.isConnected() && false == bumper.value() && _run ) { // update the string pot values cbha.stringPots( values ); // this is the same as the poser program pressures[0]=pressures[0]+(-values[0]+targetValues[0])*stepSize+(values[1]-targetValues[1])*stepSize+(values[2]-targetValues[2])*stepSize; pressures[1]=pressures[1]+(values[0]-targetValues[0])*stepSize+(-values[1]+targetValues[1])*stepSize+(values[2]-targetValues[2])*stepSize; pressures[2]=pressures[2]+(values[0]-targetValues[0])*stepSize+(values[1]-targetValues[1])*stepSize+(-values[2]+targetValues[2])*stepSize; lowerAvg=(pressures[0]+pressures[1]+pressures[2])/3; pressures[0]-=lowerAvg; pressures[1]-=lowerAvg; pressures[2]-=lowerAvg; pressures[3]=pressures[3]+(-values[3]+targetValues[3])*stepSize+(values[4]-targetValues[4])*stepSize+(values[5]-targetValues[5])*stepSize; pressures[4]=pressures[4]+(values[3]-targetValues[3])*stepSize+(-values[4]+targetValues[4])*stepSize+(values[5]-targetValues[5])*stepSize; pressures[5]=pressures[5]+(values[3]-targetValues[3])*stepSize+(values[4]-targetValues[4])*stepSize+(-values[5]+targetValues[5])*stepSize; upperAvg=(pressures[3]+pressures[4]+pressures[5])/3; pressures[3]-=upperAvg; pressures[4]-=upperAvg; pressures[5]-=upperAvg; pressures[7]=10; cbha.setPressures( pressures ); // determine if we are in tolerance for( unsigned int i = 0; i < 6; ++i ){ errors[i]=targetValues[i]-values[i]; std::cout<< errors[i] << std::endl; if(tolerance > std::abs(errors[i]) ){ std::cout << "in tolerance: " << i << std::endl; } else{ break; } if(i==5){ inTolerance=true; } else inTolerance=false; } // movement odometry.readings( &x, &y, &phi ); std::cout << "Pos:" << x << "," << y << ","<< phi << std::endl; omniDrive.setVelocity( vel[0], vel[1], vel[2] ); // update our velocity vector using the robotino api omni drive // algorithm // this is the first step. If we are in tolerance and at the start location start moving. if (inTolerance==true && x<=0 && !closed){ vel[0]=.2; } // second step. stop when we get .5 meters away from start then close the gripper if (inTolerance==true && x<=0.51 && x>=.49 && !closed){ vel[0]=0; rec::robotino::api2::msleep( 2000 ); // wait 2 seconds cbha.setGripperValve1( true ); // close gripper valves cbha.setGripperValve2( true ); rec::robotino::api2::msleep( 1000 ); // wait 1 second closed=true; for( unsigned int i = 0; i < 6; ++i ){ // switch our pose to the cary pose targetValues[i]=caryPose[i]; } } // now that the gripper is closed we want to start rotating. if (inTolerance==true && x<=0.51 && x>=.49 && closed){ vel[2]=.5; } // once you have rotated 2.25 radians stop and wait 5 seconds. if (phi>=2.2&&phi<=2.3) { vel[2]=0; vel[0]=.2; rec::robotino::api2::msleep( 5000 ); break; } // list values std::cout << std::endl; if (inTolerance){ std::cout << " all in tolerance: "<< std::endl; } std::cout << std::endl; for( unsigned int i = 0; i < 8; ++i ) { std::cout << "B" << i << ": " << pressures[i] << " bar" << std::endl; } std::cout << std::endl; for( unsigned int i = 0; i < 6; ++i ) { std::cout << "String pot " << i << ": " << values[i] << std::endl; } // syncronize and delay lastTime=com.msecsElapsed(); com.processEvents(); rec::robotino::api2::msleep( 100 ); } }