void setup()
{
motors.init();
gyroscope.init();
keyboard.init();
/*
PID coefficients may vary depending on mechanical properties of the robot/motors/battery
Here are some instructions how to tune the PID
http://robotics.stackexchange.com/questions/167/what-are-good-strategies-for-tuning-pid-loops
*/
// Angle regulator. This PID adjusts the desired angle (normally 0) depending
// on the mean speed of the robot. The goal is to keep the mean speed near 0.
// (robot is staying on one place and does not drive away)
angleRegulator.init(-0.02, 0, 0); // P is negative, because to compensate positive speed we mast tilt the robot little bit back
// Speed regulator. This PID gets the desired angle from the angle regulator and
// adjusts the acceleration (power applied to motors) to keep the angle near the desired value.
speedRegulator.init(35, 0.2, 0);
wprintf(L"Press Esc to exit\n");
}
int main(){
Brain* brain = new Brain();
Moteur* moteur = new Moteur();
Sonar* sonarAvant = new Sonar();
Sonar* sonarArriere = new Sonar();
Gyroscope* gyroscope = new Gyroscope();
FeedbackCurrent* feedbackCurrentReader = new FeedbackCurrent();
Stage1* stage1 = new Stage1(brain);
/* Initialisations */
brain->init();
moteur->init();
feedbackCurrentReader->init();
sonarAvant->init(AVANT);
sonarArriere->init(ARRIERE);
gyroscope->init();
//bras->int();
/* Bindings */
brain->bindService(moteur);
brain->bindService(feedbackCurrentReader);
brain->bindService(sonarAvant);
brain->bindService(sonarArriere);
brain->bindService(gyroscope);
brain->bindService(stage1);
/* Lancement du brain */
stage1->init();
brain->start();
/* Rajouter delete pour les composants ??? */
return EXIT_SUCCESS;
}
