RocketSystem::RocketSystem(
Accelerometer& accel,
optional<Accelerometer *> accelH,
optional<Barometer *> bar,
optional<Geiger *> ggr,
optional<GPS *> gps,
Gyroscope& gyr,
optional<Magnetometer *> mag,
WorldEstimator& estimator, InputSource& inputSource,
MotorMapper& motorMapper, Communicator& communicator, Logger& logger,
Platform& platform)
: VehicleSystem(communicator), MessageListener(communicator),
params(params),
accel(accel), accelH(accelH), bar(bar), ggr(ggr), gps(gps), gyr(gyr), mag(mag),
estimator(estimator), inputSource(inputSource),
motorMapper(motorMapper), platform(platform),
systemStream(communicator, 10),
logger(logger),
state(RocketState::DISARMED) {
// Disarm by default. A set_arm_state_message_t message is required to enable
// the control pipeline.
setArmed(false);
gyr.setAxisConfig(unit_config::GYR_AXES);
accel.setAxisConfig(unit_config::ACC_AXES);
(*accelH)->setAxisConfig(unit_config::ACCH_AXES);
gyr.setOffsets(unit_config::GYR_OFFSETS);
accel.setOffsets(unit_config::ACC_OFFSETS);
(*accelH)->setOffsets(unit_config::ACCH_OFFSETS);
}
void Config::Apply()
{
std::list< std::string > accelerometers;
std::list< std::string > gyroscopes;
std::list< std::string > magnetometers;
lua_getglobal( L, "accelerometers" );
lua_pushnil( L );
while ( lua_next( L, -2 ) ) {
accelerometers.emplace_back( lua_tolstring( L, -2, nullptr ) );
lua_pop(L, 1);
}
lua_pop(L, 1);
lua_getglobal( L, "gyroscopes" );
lua_pushnil( L );
while ( lua_next( L, -2 ) ) {
gyroscopes.emplace_back( lua_tolstring( L, -2, nullptr ) );
lua_pop(L, 1);
}
lua_pop(L, 1);
lua_getglobal( L, "magnetometers" );
lua_pushnil( L );
while ( lua_next( L, -2 ) ) {
magnetometers.emplace_back( lua_tolstring( L, -2, nullptr ) );
lua_pop(L, 1);
}
lua_pop(L, 1);
for ( auto it : gyroscopes ) {
Gyroscope* gyro = Sensor::gyroscope( it );
if ( gyro ) {
int swap[4] = { 0, 0, 0, 0 };
swap[0] = integer( "gyroscopes." + it + ".axis_swap.x" );
swap[1] = integer( "gyroscopes." + it + ".axis_swap.y" );
swap[2] = integer( "gyroscopes." + it + ".axis_swap.z" );
gyro->setAxisSwap( swap );
}
}
for ( auto it : accelerometers ) {
Accelerometer* accel = Sensor::accelerometer( it );
if ( accel ) {
int swap[4] = { 0, 0, 0, 0 };
swap[0] = integer( "accelerometers." + it + ".axis_swap.x" );
swap[1] = integer( "accelerometers." + it + ".axis_swap.y" );
swap[2] = integer( "accelerometers." + it + ".axis_swap.z" );
accel->setAxisSwap( swap );
}
}
for ( auto it : magnetometers ) {
Magnetometer* magn = Sensor::magnetometer( it );
if ( magn ) {
int swap[4] = { 0, 0, 0, 0 };
swap[0] = integer( "magnetometers." + it + ".axis_swap.x" );
swap[1] = integer( "magnetometers." + it + ".axis_swap.y" );
swap[2] = integer( "magnetometers." + it + ".axis_swap.z" );
magn->setAxisSwap( swap );
}
}
}
void loop() {
Accelerometer *accelerometer = Sensors::getAccelerometer();
if(accelerometer) {
Vector3 a = accelerometer->getAcceleration();
Serial.print("Acceleration (m/s^2) ");
Serial.print(a.x);
Serial.print(", ");
Serial.print(a.y);
Serial.print(", ");
Serial.println(a.z);
}
Barometer *barometer = Sensors::getBarometer();
if(barometer) {
float p = barometer->getPressure();
Serial.print("Pressure (hPa) ");
Serial.println(p);
float a = barometer->getAltitude();
Serial.print("Altitude (m) ");
Serial.println(a);
}
Gyroscope *gyroscope = Sensors::getGyroscope();
if(gyroscope) {
Vector3 g = gyroscope->getRotation();
Serial.print("Rotation (rad/s) ");
Serial.print(g.x);
Serial.print(", ");
Serial.print(g.y);
Serial.print(", ");
Serial.println(g.z);
}
Magnetometer *magnetometer = Sensors::getMagnetometer();
if(magnetometer) {
Vector3 m = magnetometer->getMagneticField();
Serial.print("Magnetic Field (uT) ");
Serial.print(m.x);
Serial.print(", ");
Serial.print(m.y);
Serial.print(", ");
Serial.println(m.z);
float azimuth = magnetometer->getAzimuth();
Serial.print("Azimuth (deg) ");
Serial.println(azimuth);
}
Thermometer *thermometer = Sensors::getThermometer();
if(thermometer) {
float t = thermometer->getTemperature();
Serial.print("Temperature (C) ");
Serial.println(t);
}
delay(50);
}
float gyro(int samples) {
float value = 0;
for (int i = 0; i < samples; i++) {
value += GYRO.getAngularDisplacement();
GYRO.update();
delayMicroseconds(10);
}
return value / samples;
}
// the loop routine runs over and over again forever:
void loop()
{
wprintf(L"Calibration in 3 seconds\n");
delay(3000);
wprintf(L"Calibrating...");
gyroscope.calibrate();
wprintf(L"Cycle tyme in microseconds: %ld \n", TICK_LENGTH_MICROSECONDS);
// main cycle
delay(100);
unsigned long lastIteration = micros();
unsigned long nextIteration = lastIteration + TICK_LENGTH_MICROSECONDS;
int n = 0;
do {
float actualAngle, deltaAngle;
while (micros() < nextIteration)
{
// do nothing
}
unsigned long now = micros();
gyroscope.getAngleFiltered(actualAngle, deltaAngle, now);
targetAngle = angleRegulator.getResult(speedIntegrator.getSum(), 0, now - lastIteration);
float motorSpeed = 0;
motorSpeed = speedRegulator.getResult(actualAngle - targetAngle, deltaAngle, now - lastIteration);
n++;
if (n % 20 == 0) // Print on every 20th iteration
{
wprintf(L"%d\t%f\t%f\t%f\t%f\n", n, targetAngle, actualAngle, deltaAngle, motorSpeed);
}
motors.setSpeedBoth(motorSpeed);
speedIntegrator.pushValue(motorSpeed);
lastIteration = now;
nextIteration = now + TICK_LENGTH_MICROSECONDS;
keyboard.processEvents();
if (keyboard.shouldExit())
{
break;
}
} while (true);
motors.stopAll();
wprintf(L"Exiting...\n");
exit(0);
}
void setup() {
Serial.begin(BAUDRATE);
ENC_LEFT.attach(ENC_LEFT_PIN);
ENC_RIGHT.attach(ENC_RIGHT_PIN);
ENC_LEFT.begin();
ENC_RIGHT.begin();
PROX.attach(PROX_TRIGGER_PIN, PROX_ECHO_PIN);
MOTORS.init();
GYRO.attach();
GYRO.begin();
}
SimObject* Gyroscope::clone() const
{
Gyroscope* newGyroscope = new Gyroscope();
newGyroscope->copyStandardMembers(this);
newGyroscope->sensorReading = sensorReading;
newGyroscope->sensorReading.data.doubleArray = newGyroscope->valueAngleVel.v;
std::list<SimObject*>::const_iterator pos;
for(pos = childNodes.begin(); pos != childNodes.end(); ++pos)
{
SimObject* childNode = (*pos)->clone();
newGyroscope->addChildNode(childNode, false);
}
SimObject* newObject = newGyroscope;
return newObject;
}
void turn(int target, int speed) {
GYRO.calibrate(10);
float zero = gyro(4);
float value;
MOTORS.setMotorSpeed(speed, -speed);
do {
value = abs(zero - gyro(4));
} while (value < target);
MOTORS.setMotorSpeed(0, 0);
}
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;
}
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");
}
void loop() {
GYRO.update();
if (Serial.available()) {
char in = Serial.read();
switch (in) {
case 'l':
turn(10, SPEED);
break;
case 'r':
turn(10, -SPEED);
break;
case 'f':
MOTORS.setMotorSpeed(SPEED, SPEED);
break;
case 'b':
MOTORS.setMotorSpeed(-SPEED, -SPEED);
break;
case 's':
MOTORS.setMotorSpeed(0, 0);
break;
}
}
}
