Merge pull request #6 from team914/dev

👌 v0.9.3

Author: acetousk

autolib@0.9.3.zip

@@ -53,6 +53,20 @@ class PurePursuit {
     PurePursuitTriangle run( const okapi::OdomState &ipose, const std::string &iid );
     PurePursuitTriangle run( const Pose &ipose, const std::string &iid );
 
+    /**
+     * This should be used in a loop during your autonomous. It is static, but is meant for the PurePursuit class.
+     * It is recommended to use motion profiling for the first parameter. The second parameter should be from your
+     * run method from the PurePursuit class. The third method should be from you okapi's ChassisControllerBuilder.
+     * 
+     * Note: Using this method is optional. You can and probably should create your own controller to use the triangle
+     * motor output This is only meant for users of okapi >= v4.0.0.
+     * 
+     * @param  {double} reqVelocity                                : 
+     * @param  {PurePursuitTriangle} triangle                      : 
+     * @param  {std::shared_ptr<OdomChassisController>} controller : 
+     */
+    static void updateChassis( const double &reqVelocity, const PurePursuitTriangle &triangle, const std::shared_ptr<OdomChassisController> &controller );
+
     protected:
     const std::vector<IndexedDistancePosePath> paths;
     IndexedDistancePosePath path;

bin/_pros_ld_timestamp.o

@@ -39,6 +39,15 @@ PurePursuitTriangle PurePursuit::run( const Pose &ipose, const std::string &iid
     return findPurePursuitTriangle();
 }
 
+void PurePursuit::updateChassis( const double &reqVelocity, const PurePursuitTriangle &itriangle, const std::shared_ptr<OdomChassisController> &controller ){
+    PurePursuitTriangle triangle = itriangle;
+    double curvature_scales = triangle.localGoalPose.yaw * (controller->getChassisScales().wheelTrack.convert(meter))/2;
+    double left = reqVelocity * ( 2 + curvature_scales )/2;
+    double right = reqVelocity * ( 2 - curvature_scales )/2;
+    controller->getModel()->left( left );
+    controller->getModel()->right( right );
+}
+
 void PurePursuit::findPath( const std::string &iid ){
     for( const auto &ipath: paths ){
         if( ipath.id == iid ){

bin/monolith.bin

@@ -14,27 +14,25 @@ void opcontrol() {
     std::make_shared<Logger>(std::make_unique<Timer>(), "/ser/sout", Logger::LogLevel::debug));
 
   controller = ChassisControllerBuilder()
-  	.withMotors({1, -2}, {-6, 7})
-  	.withSensors( IntegratedEncoder{ 1 }, IntegratedEncoder{ 6, true } )
-	  .withOdometry( StateMode::FRAME_TRANSFORMATION )
+  	.withMotors({-1, 2}, {6, -7})
+  	.withSensors( IntegratedEncoder{ 1, true }, IntegratedEncoder{ 6 } )
+	  .withOdometry( StateMode::CARTESIAN )
   	.withGearset( AbstractMotor::GearsetRatioPair{ AbstractMotor::gearset::green, 5/3 } )
 	  .withDimensions( {{3.75_in, 15_in}, 900} )
 	  .buildOdometry();
-	
+
   model = controller->getModel();
 
   PathGenerator pathGenerator( {1.0, 2.0, 4.0} );
   pathGenerator.generatePath( 
     { 
 		Pose{ 1_ft, 1_ft, 270_deg }, 
 		Pose{ 1_ft, 0_ft, 90_deg } 
-	}, 
+	},
     std::string("test")
   );
 
   autolib::PurePursuit purePursuit( pathGenerator.getPaths(), 1_ft );
-  auto state = controller->getState();
-  PurePursuitTriangle triangle = purePursuit.run( state, std::string("test") );
 
   //  pros::Task printSensorValsTask(printSensorVals);
 
@@ -43,6 +41,9 @@ void opcontrol() {
   //  drive->moveDistance(6_in);
 
   while (true) {
+    auto state = controller->getState();
+    PurePursuit::updateChassis( 50, purePursuit.run( state, std::string("test") ), controller );
+
     pros::delay(50);
   }
 }