AMiRo-OS

#include "global.hpp"

#include "linefollow.hpp"

      chprintf((BaseSequentialStream *)&global.sercanmux1, "Current Point: %d\n", global.odometry.getPosition().x);

      chprintf((BaseSequentialStream *)&global.sercanmux1,

               "Current Point: %d\n", global.odometry.getPosition().x);

      global.odometry.setPosition(1.2, 0.0, 0.0);

      global.tcase++; // 8

      // Fixpoint on left side, no update should appear because fixpoint already

      // marked

      ret = map.update(20000, 20000, LineFollowStrategy::EDGE_RIGHT);

      chprintf((BaseSequentialStream *)&global.sercanmux1,

               "Update test %d: Ret %d, cur %d, nex %d, dist %d, len %d\n", global.tcase, ret,

               map.get_state()->current, map.get_state()->next, map.get_state()->dist, map.get_state()->eLength);

      // No case should be true because neither was a node visited nor

      // was a fixpoint detected.

      global.testres[global.tcase] =

          (ret == 0x04) && (map.get_state()->strategy == 0x01)

        && (map.get_state()->dist == 0);

      global.odometry.setPosition(.5, 0.0, 0.0);

      chprintf((BaseSequentialStream *)&global.sercanmux1,

               "Current Point: %d\n", global.odometry.getPosition().x);

      global.tcase++; // 9

      // Fixpoint on left side, no update should appear because fixpoint already

      // marked

      ret = map.update(0, 20000, LineFollowStrategy::EDGE_RIGHT);

      chprintf((BaseSequentialStream *)&global.sercanmux1,

               "Update test %d: Ret %d, cur %d, nex %d, dist %d, len %d\n",

               global.tcase, ret, map.get_state()->current,

               map.get_state()->next, map.get_state()->dist,

               map.get_state()->eLength);

      // No case should be true because neither was a node visited nor

      // was a fixpoint detected.

      global.testres[global.tcase] =

        (ret == 9) &&

        (map.get_state()->strategy == 1) &&

        (map.get_state()->dist == 0) &&

        (map.get_state()->eLength == 50);

      global.odometry.setPosition(.75, 0.0, 0.0);

      chprintf((BaseSequentialStream *)&global.sercanmux1,

               "Current Point: %d\n", global.odometry.getPosition().x);

      global.tcase++; // 10

      // Fixpoint on left side, no update should appear because fixpoint already

      // marked

      ret = map.update(20000, 20000, LineFollowStrategy::EDGE_RIGHT);

      chprintf((BaseSequentialStream *)&global.sercanmux1,

               "Update test %d: Ret %d, cur %d, nex %d, dist %d, len %d\n",

               global.tcase, ret, map.get_state()->current,

               map.get_state()->next, map.get_state()->dist,

               map.get_state()->eLength);

      // No case should be true because neither was a node visited nor

      // was a fixpoint detected.

      global.testres[global.tcase] =

          (ret == 12) && (map.get_state()->strategy == 1) &&

          (map.get_state()->dist == 50) && (map.get_state()->eLength == 50);