Added tunable velocity & acceleration

Chassim.vcxproj

@@ -29,6 +29,7 @@
     <ClCompile Include="src\pointsList.cpp" />
     <ClCompile Include="src\robot.cpp" />
     <ClCompile Include="src\simController.cpp" />
+    <ClCompile Include="src\utils.cpp" />
   </ItemGroup>
   <ItemGroup>
     <ClInclude Include="include\constants.hpp" />
@@ -54,6 +55,7 @@
     <ClInclude Include="include\sfLine.hpp" />
     <ClInclude Include="include\structs.hpp" />
     <ClInclude Include="include\simController.hpp" />
+    <ClInclude Include="include\utils.hpp" />
   </ItemGroup>
   <PropertyGroup Label="Globals">
     <VCProjectVersion>16.0</VCProjectVersion>

Chassim.vcxproj.filters

@@ -92,6 +92,9 @@
     <ClCompile Include="src\field.cpp">
       <Filter>src\Source Files</Filter>
     </ClCompile>
+    <ClCompile Include="src\utils.cpp">
+      <Filter>src\Source Files</Filter>
+    </ClCompile>
   </ItemGroup>
   <ItemGroup>
     <ClInclude Include="include\constants.hpp">
@@ -163,5 +166,8 @@
     <ClInclude Include="include\sfLine.hpp">
       <Filter>include\Header Files</Filter>
     </ClInclude>
+    <ClInclude Include="include\utils.hpp">
+      <Filter>include\Header Files</Filter>
+    </ClInclude>
   </ItemGroup>
 </Project>

asset/field-2020.json

@@ -0,0 +1,9 @@
+{
+  "fieldPixelWidth": 1286,
+  "fieldMeterWidth": 15.9832,
+  "fieldPixelHeight": 663,
+  "fieldMeterHeight": 8.2121,
+  "spawnPoint": [264, 369],
+  "walls": [],
+  "polygons": []
+}

asset/field.json

@@ -1,9 +1,9 @@
 {
-  "fieldPixelWidth": 1286,
-  "fieldMeterWidth": 15.9832,
-  "fieldPixelHeight": 663,
-  "fieldMeterHeight": 8.2121,
-  "spawnPoint": [264, 369],
+  "fieldPixelWidth": 730,
+  "fieldMeterWidth": 3.5687,
+  "fieldPixelHeight": 730,
+  "fieldMeterHeight": 3.5687,
+  "spawnPoint": [64, 596],
   "walls": [],
   "polygons": []
 }

asset/robot.json

@@ -1,3 +1,3 @@
 {
-  "trackWidthMeter": 0.7
+  "trackWidthMeter": 0.4
 }

include/field.hpp

@@ -16,6 +16,7 @@ class Field {
     const sf::Vector2i& getSpawnPoint() const;
     const std::vector<std::vector<b2Vec2>>& getWalls() const;
     const std::vector<std::vector<b2Vec2>>& getPolygons() const;
+    bool isInField(int x, int y);
 
     const inline float p2mX(float coord) const {
         return coord / fieldXPixelPerMeter;

include/pathgen.hpp

@@ -10,16 +10,16 @@ struct TrajectoryPair {
     Segment* right;
     Segment* original;
     int length;
+    double totalTime;
+    double pathLength;
 };
 
 class PathGenerator {
 public:
-    PathGenerator(float trackwidth, double maxVel, double maxAccel, double maxJerk);
+    PathGenerator(float trackwidth);
 
-    TrajectoryPair* generatePath(std::vector<Point> waypoints);
+    TrajectoryPair* generatePath(std::vector<Point> waypoints, 
+        double maxVel, double maxAccel, double maxJerk);
 private:
     float trackwidth;
-    double maxVel;
-    double maxAccel;
-    double maxJerk;
 };

include/simController.hpp

@@ -51,6 +51,9 @@ class SimController {
         return newAngle;
     }
 private:
+    void setMaxVelocity(float velocity);
+    void setMaxAcceleration(float acceleration);
+
     void createComponents();
 
     sf::RenderWindow& window;
@@ -73,4 +76,7 @@ class SimController {
 
     int pointDraggingIndex = -1;
     bool isDraggingPoint = false;
+
+    float maxVelocity = 0.5;
+    float maxAcceleration = 0.5;
 };

include/utils.hpp

@@ -0,0 +1,9 @@
+#pragma once
+
+#include <string>
+
+double ROUND1(double x);
+double ROUND2(double x);
+
+std::string ROUND1STR(double x);
+std::string ROUND2STR(double x);

src/field.cpp

@@ -30,6 +30,11 @@ const std::vector<std::vector<b2Vec2>>& Field::getPolygons() const {
     return polygons;
 }
 
+bool Field::isInField(int x, int y) {
+    const auto& size = texture.getSize();
+    return x > 0 && x < size.x && y > 0 && y < size.y;
+}
+
 void Field::loadImage() {
     texture.loadFromFile(FIELD_IMAGE_NAME);
 }

src/pathgen.cpp

@@ -8,19 +8,18 @@
 #include "structs.hpp"
 #include "pathfinder/pathfinder.h"
 
-PathGenerator::PathGenerator(float trackwidth, double maxVel, double maxAccel, double maxJerk) :
-    trackwidth{trackwidth},
-    maxVel{maxVel},
-    maxAccel{maxAccel},
-    maxJerk{maxJerk} {
+PathGenerator::PathGenerator(float trackwidth) :
+    trackwidth{trackwidth} {
 
 }
 
 /**
  * Original author: Ryan Benasutti, WPI
  * Modified from OkapiLib
  */
-TrajectoryPair* PathGenerator::generatePath(std::vector<Point> waypoints) {
+TrajectoryPair* PathGenerator::generatePath(std::vector<Point> waypoints,
+    double maxVel, double maxAccel, double maxJerk 
+) {
     if (waypoints.size() == 0) {
         // No point in generating a path
         std::cout <<
@@ -46,8 +45,9 @@ TrajectoryPair* PathGenerator::generatePath(std::vector<Point> waypoints) {
         maxAccel,
         maxJerk,
         &candidate);
-
     const int length = candidate.length;
+    const double totalTime = candidate.length * candidate.info.dt;
+    const double pathLength = candidate.totalLength;
 
     if (length < 0) {
         auto pointToString = [](Waypoint point) {
@@ -126,5 +126,6 @@ TrajectoryPair* PathGenerator::generatePath(std::vector<Point> waypoints) {
     std::cout << "PathGenerator: Completely done generating path" << std::endl;
     std::cout << "PathGenerator: " + std::to_string(length) << std::endl;
 
-    return new TrajectoryPair{leftTrajectory, rightTrajectory, trajectory, length};
+    return new TrajectoryPair{leftTrajectory, rightTrajectory, trajectory, length,
+        totalTime, pathLength};
 }

src/simController.cpp

@@ -3,13 +3,15 @@
 #include <TGUI/TGUI.hpp>
 #include <vector>
 #include <iostream>
+#include <iomanip>
 
 #include "constants.hpp"
 #include "field.hpp"
 #include "structs.hpp"
 #include "pointsList.hpp"
 #include "pathgen.hpp"
 #include "sfLine.hpp"
+#include "utils.hpp"
 
 sf::Cursor defaultCursor;
 sf::Cursor grabCursor;
@@ -20,7 +22,7 @@ SimController::SimController(sf::RenderWindow& window, Field& field) :
     field{field},
     env{field},
     robot{env, field.getSpawnPoint().x, field.getSpawnPoint().y},
-    pathGen{robot.getTrackWidth(), 3.0, 4.0, 20.0},
+    pathGen{robot.getTrackWidth()},
     splineLines{} {
     defaultCursor.loadFromSystem(sf::Cursor::Type::Arrow);
     grabCursor.loadFromSystem(sf::Cursor::Type::Hand);
@@ -61,6 +63,11 @@ void SimController::handleEvent(sf::Event event) {
                 generateProfile();
             }
             break;
+        case sf::Keyboard::G:
+            if (event.key.control) {
+                generateProfile();
+            }
+            break;
         }
         break;
     }
@@ -88,9 +95,13 @@ void SimController::handleEvent(sf::Event event) {
                 window.setMouseCursor(defaultCursor);
 
                 generateProfile();
-            } else {
+            } else if (gui.get<tgui::EditBox>("rowEditBox") == nullptr) {
                 int pixelX = event.mouseButton.x;
                 int pixelY = event.mouseButton.y;
+
+                if (!field.isInField(pixelX, pixelY - MENU_BAR_HEIGHT)) {
+                    return;
+                }
 
                 if (window.getViewport(window.getDefaultView()).contains(pixelX, pixelY)) {
                     auto meters = metersRelativeToOrigin(pixelX, pixelY);
@@ -268,7 +279,7 @@ void SimController::generateProfile() {
     }
 
     // TODO: Generate the modified tank trajectory only when executing the profile
-    traj = pathGen.generatePath(waypoints);
+    traj = pathGen.generatePath(waypoints, maxVelocity, maxAcceleration, 20.0);
 
     if (splineLines.size() < traj->length) {
         splineLines.reserve(traj->length);
@@ -288,6 +299,18 @@ void SimController::generateProfile() {
             }
         );
     }
+
+    // Update the info box
+    tgui::Label::Ptr infoBox = gui.get<tgui::Label>("infoBox");
+
+    std::ostringstream ss;
+    ss << "Path Length: ";
+    ss << std::fixed << std::setprecision(2) << ROUND2(traj->pathLength);
+    ss << " m" << std::endl;
+    ss << "Path Time: ";
+    ss << std::fixed << std::setprecision(2) << ROUND2(traj->totalTime);
+    ss << " s" << std::endl;
+    infoBox->setText(ss.str());
 }
 
 void SimController::executeProfile() {
@@ -394,6 +417,26 @@ std::vector<Point> SimController::getPoints() {
     return points.getPoints();
 }
 
+void SimController::setMaxVelocity(float velocity) {
+    maxVelocity = velocity;
+    tgui::Label::Ptr velocitySliderText = gui.get<tgui::Label>("velocitySliderText");
+    std::ostringstream ss;
+    ss << "Max Velocity: ";
+    ss << ROUND1(maxVelocity);
+    ss << " m/s";
+    velocitySliderText->setText(ss.str());
+}
+
+void SimController::setMaxAcceleration(float acceleration) {
+    maxAcceleration = acceleration;
+    tgui::Label::Ptr accelSliderText = gui.get<tgui::Label>("accelSliderText");
+    std::ostringstream ss;
+    ss << "Max Acceleration: ";
+    ss << ROUND1(maxAcceleration);
+    ss << " m/s^2";
+    accelSliderText->setText(ss.str());
+}
+
 void SimController::createComponents() {
     // Make the top menu bar
     tgui::MenuBar::Ptr menuBar = tgui::MenuBar::create();
@@ -405,16 +448,16 @@ void SimController::createComponents() {
     menuBar->addMenuItem("Reset Robot");
     menuBar->connectMenuItem("Edit", "Reset Robot", &SimController::resetRobot, this);
     menuBar->addMenu("Pathing");
-    menuBar->addMenuItem("Generate Profile");
-    menuBar->connectMenuItem("Pathing", "Generate Profile", &SimController::generateProfile, this);
+    menuBar->addMenuItem("Generate Profile (Ctrl-G)");
+    menuBar->connectMenuItem("Pathing", "Generate Profile (Ctrl-G)", &SimController::generateProfile, this);
     menuBar->addMenuItem("Execute Profile");
     menuBar->connectMenuItem("Pathing", "Execute Profile", &SimController::executeProfile, this);
     gui.add(menuBar, "menuBar");
 
     // Make the table of points
     tgui::ListView::Ptr pointsList = tgui::ListView::create();
     pointsList->setTextSize(24);
-    pointsList->setSize({POINTS_LIST_WIDTH, "100%"});
+    pointsList->setSize({POINTS_LIST_WIDTH, "60%"});
     pointsList->setPosition({"parent.width - width", MENU_BAR_HEIGHT});
     pointsList->setHeaderHeight(35);
     pointsList->setHeaderSeparatorHeight(1);
@@ -429,4 +472,67 @@ void SimController::createComponents() {
     pointsList->addColumn(L"\u03B8 (\u00B0)", POINTS_LIST_COLUMN_WIDTH);
 
     gui.add(pointsList, "pointsList");
+
+    tgui::Label::Ptr infoBox = tgui::Label::create();
+    infoBox->setTextSize(20);
+    infoBox->setSize({"pointsList.width", "15%"});
+    infoBox->setPosition({"pointsList.left", "pointsList.bottom"});
+    infoBox->setText("Path Length: 0 in\nPath Time: 0 s");
+    gui.add(infoBox, "infoBox");
+
+    tgui::Label::Ptr velocitySliderText = tgui::Label::create();
+    velocitySliderText->setTextSize(18);
+    velocitySliderText->setText("Max Velocity: " + ROUND1STR(maxVelocity) + " m/s");
+    velocitySliderText->setPosition({"infoBox.left + 15", "infoBox.bottom"});
+    gui.add(velocitySliderText, "velocitySliderText");
+
+    tgui::Slider::Ptr velocitySlider = tgui::Slider::create();
+    velocitySlider->setSize({"pointsList.width - 30", "2%"});
+    velocitySlider->setPosition({"velocitySliderText.left", "velocitySliderText.bottom + 3"});
+    velocitySlider->setMinimum(0.1);
+    velocitySlider->setMaximum(1.0);
+    velocitySlider->setStep(0.1);
+    velocitySlider->setValue(maxVelocity);
+    velocitySlider->connect("ValueChanged", &SimController::setMaxVelocity, this);
+    gui.add(velocitySlider, "velocitySlider");
+
+    for (int i = 1; i <= 10; i += 1) {
+        tgui::Label::Ptr velocitySliderTick = tgui::Label::create();
+        velocitySliderTick->setTextSize(13);
+        velocitySliderTick->setText(std::to_string(i));
+        velocitySliderTick->setHorizontalAlignment(tgui::Label::HorizontalAlignment::Center);
+        velocitySliderTick->setPosition({
+            "velocitySliderText.left + velocitySlider.width * " + std::to_string((i - 1) / 9.0) + " - 8",
+            "velocitySlider.bottom + 2"
+        });
+        gui.add(velocitySliderTick);
+    }
+
+    tgui::Label::Ptr accelSliderText = tgui::Label::create();
+    accelSliderText->setTextSize(18);
+    accelSliderText->setText("Max Accel: " + ROUND1STR(maxAcceleration) + " m/s^2");
+    accelSliderText->setPosition({"velocitySliderText.left", "velocitySlider.bottom + 15"});
+    gui.add(accelSliderText, "accelSliderText");
+
+    tgui::Slider::Ptr accelSlider = tgui::Slider::create();
+    accelSlider->setSize({"pointsList.width - 30", "2%"});
+    accelSlider->setPosition({"accelSliderText.left", "accelSliderText.bottom + 3"});
+    accelSlider->setMinimum(0.1);
+    accelSlider->setMaximum(1.0);
+    accelSlider->setStep(0.1);
+    accelSlider->setValue(maxAcceleration);
+    accelSlider->connect("ValueChanged", &SimController::setMaxAcceleration, this);
+    gui.add(accelSlider, "accelSlider");
+
+    for (int i = 1; i <= 10; i += 1) {
+        tgui::Label::Ptr accelSliderTick = tgui::Label::create();
+        accelSliderTick->setTextSize(13);
+        accelSliderTick->setText(std::to_string(i));
+        accelSliderTick->setHorizontalAlignment(tgui::Label::HorizontalAlignment::Center);
+        accelSliderTick->setPosition({
+            "accelSliderText.left + accelSlider.width * " + std::to_string((i - 1) / 9.0) + " - 8",
+            "accelSlider.bottom + 2"
+        });
+        gui.add(accelSliderTick);
+    }
 }