virtual void addCallback(GlobalData& globalData, bool draw, bool pause, bool control) {
if(!pause){
if(friction>0){
OdeRobot* robot = globalData.agents[0]->getRobot();
Pos vel = robot->getMainPrimitive()->getAngularVel();
robot->getMainPrimitive()->applyTorque(-vel*friction);
}
}
}
/** is called if a key was pressed.
For keycodes see: osgGA::GUIEventAdapter
@return true if the key was handled
*/
virtual bool command(const OdeHandle&, const OsgHandle&, GlobalData& globalData,
int key, bool down) {
if (down) { // only when key is pressed, not when released
switch ( (char) key ) {
case 'T' : robot->getMainPrimitive()->applyTorque(0 , 0 , 3 ); break;
case 't' : robot->getMainPrimitive()->applyTorque(0 , 0 , -3 ); break;
default:
return false;
}
return true;
} else return false;
}
// add own key handling stuff here, just insert some case values
virtual bool command(const OdeHandle&, const OsgHandle&, GlobalData& globalData, int key, bool down)
{
if (down) { // only when key is pressed, not when released
switch ( (char) key )
{
case 'y' : dBodyAddForce ( sphere1->getMainPrimitive()->getBody() , 30 ,0 , 0 ); break;
case 'Y' : dBodyAddForce ( sphere1->getMainPrimitive()->getBody() , -30 , 0 , 0 ); break;
case 'x' : dBodyAddTorque ( sphere1->getMainPrimitive()->getBody() , 0 , 10 , 0 ); break;
case 'X' : dBodyAddTorque ( sphere1->getMainPrimitive()->getBody() , 0 , -10 , 0 ); break;
case 'S' : controller->setParam("sinerate", controller->getParam("sinerate")*1.2);
printf("sinerate : %g\n", controller->getParam("sinerate"));
break;
case 's' : controller->setParam("sinerate", controller->getParam("sinerate")/1.2);
printf("sinerate : %g\n", controller->getParam("sinerate"));
break;
default:
return false;
break;
}
}
return false;
}
virtual void addCallback(GlobalData& global, bool draw, bool pause, bool control) {
if(!control || pause) return;
if(counter == maxCounter){
counter = 0;
if(sphere) {
// measure final speed
double dat[3];
speedsensor->sense(global);
speedsensor->get(dat,3);
Matrix s(1,3,dat);
logfile << s << endl;
cout << "End: " << s << endl;
if(runcounter>=startAngles.size()){
this->pause=true;
simulation_time_reached=true;
return;
}
// destroy sphere
global.agents.pop_back();
delete sphere;
delete controller;
delete wiring;
delete agent;
}
// create new sphere
sphere = new Sphererobot3Masses ( odeHandle, osgHandle.changeColor(Color(0,0.0,2.0)),
conf, "Sphere_test_sat", 0.3);
// we will accelerate the sphere in positive x direction
// we place it the way that all (straight) rolling modes are possible
double theta = startAngles[runcounter].val(0,0);
double omega = startAngles[runcounter].val(1,0);
sphere->place ( osg::Matrix::rotate(omega, 0,0,1)*osg::Matrix::rotate(theta, 1,0,0));
// sphere->place ( osg::Matrix::rotate(M_PI/2, 0,1,0));
// sphere->place ( osg::Matrix::rotate(M_PI/2, 0,0,1));
speedsensor->init(sphere->getMainPrimitive());
// controller = new SatNetControl(networkfilename);
controller = new SatNetControl_NoY(networkfilename);
wiring = new One2OneWiring ( new ColorUniformNoise(0.20) );
agent = new OdeAgent ( plotoptions );
agent->init ( controller , sphere , wiring );
// agent->setTrackOptions(TrackRobot(true, true, false, true, "ZSens", 50));
global.agents.push_back ( agent );
runcounter++;
} else if(counter > 1 && counter < accelerationTime){
// speed up sphere
double power = startAngles[runcounter-1].val(2,0);
dBodyAddTorque ( sphere->getMainPrimitive()->getBody() , 0, power, 0 );
}else if(counter == accelerationTime){ // measure start speed
double dat[3];
speedsensor->sense(global);
speedsensor->get(dat,3);
Matrix s(1,3,dat);
logfile << s;
cout << "Run: " << runcounter << "/" << startAngles.size() << endl;
cout << "Start: " << s << endl;
}
counter++;
}
