Box Placement

src/main/cpp/HAL/PlaceBox.cpp

@@ -0,0 +1,45 @@
+#include "HAL/PlaceBox.h"
+#include <iostream>
+
+
+PlaceBox::PlaceBox() {
+    double levelHeight=0;
+    int level=0;
+    double robotHeight=0;
+    double baseHeight=0;
+    double shelfPosition=0;
+    double boxSide=0;
+    double shelfWidth=0;
+    double position=0;
+    double boxPosition=0;
+    double distanceFromThePlatform=0;
+}
+
+double PlaceBox::distanceFromPlatform(double position, double shelfPosition)
+{
+    double distance = shelfPosition-position;
+    return distance;
+}
+
+int PlaceBox::getLevel(){
+    return int(robotHeight/levelHeight)-1;
+}
+void PlaceBox::liftBox(int level){
+    while(getLevel()<level){
+        //lift
+    }
+}
+void PlaceBox::moveToShelf(double shelfPosition, double position, double distanceFromPlatform, double boxSides){
+    while(shelfPosition-position > 0.5){
+    }
+}
+void PlaceBox::slideBoxIntoShelf(double shelfWidth, double boxPosition, double shelfPosition, double boxSides){
+    while(shelfPosition+shelfWidth > boxPosition+boxSides+1){
+    }
+}
+void PlaceBox::putBox(double distance, int goalLevel, double boxSides, double shelfPosition, double boxPosition){
+    liftBox(goalLevel);
+    moveToShelf(shelfPosition, position, distanceFromThePlatform, boxSides);
+    slideBoxIntoShelf(shelfWidth, boxPosition, shelfPosition, boxSides);
+
+}

src/main/include/HAL/PlaceBox.h

@@ -0,0 +1,28 @@
+#pragma once
+
+#include <iostream>
+
+class PlaceBox {
+    public:
+        PlaceBox();
+        ~PlaceBox() = default;
+        double position;
+        double boxPosition;
+        double distanceFromThePlatform;
+        double distanceFromPlatform(double position, double shelfPosition);
+        void putBox(double distance, int goallevel, double boxSides, double shelfPosition, double boxPosition);
+        int getLevel();
+        void liftBox(int level);
+        void moveToShelf(double shelfPosition, double position, double distanceFromPlatform, double boxSides);
+        void slideBoxIntoShelf(double shelfWidth, double boxPosition, double shelfPosition, double boxSides);
+
+    private:
+        double levelHeight;
+        double robotHeight;
+        double baseHeight;
+        double shelfPosition;
+        double boxSide;
+        double shelfWidth;
+        double level;
+};
+