PROS Example Code for Logging Kinematics from 2 Encoders and Position from Odom

KinematicsLogging.cpp

pros::task_t graphing;

void loop(void* params){

	auto lposFilter   = std::make_shared<AverageFilter<10>>();
    auto lspeedFilter = std::make_shared<AverageFilter<10>>();
    auto laccelFilter = std::make_shared<AverageFilter<10>>();
    auto ljerkFilter  = std::make_shared<AverageFilter<10>>();
    auto lsnapFilter  = std::make_shared<AverageFilter<10>>();

	 auto rposFilter   = std::make_shared<AverageFilter<10>>();
    auto rspeedFilter = std::make_shared<AverageFilter<10>>();
    auto raccelFilter = std::make_shared<AverageFilter<10>>();
    auto rjerkFilter  = std::make_shared<AverageFilter<10>>();
    auto rsnapFilter  = std::make_shared<AverageFilter<10>>();

    int cnt = 1;
 
    bool disabled{false};
    double dt{0};
    double lpos{0},   llastPos{0};
    double lspeed{0}, llastSpeed{0};
    double laccel{0}, llastAccel{0};
    double ljerk{0},  llastJerk{0};
    double lsnap{0},  llastSnap{0};

    double rpos{0},   rlastPos{0};
    double rspeed{0}, rlastSpeed{0};
    double raccel{0}, rlastAccel{0};
    double rjerk{0},  rlastJerk{0};
    double rsnap{0},  rlastSnap{0};

    double tpr{imev5GreenTPR};
    double ratio{1};
    QLength wheelDiameter{4.036_in};
    std::unique_ptr<AbstractTimer> loopDtTimer = std::make_unique<Timer>();

	while(true){
        //get delta time
        dt = loopDtTimer->getDtFromMark().convert(second);

        //get position from encoder feed back and convert to meters
        lpos = lposFilter->filter(chassis->skidSteerModel->getSensorVals()[0]) / (tpr * ratio) * wheelDiameter.convert(meter) * PI;
        rpos = rposFilter->filter(chassis->skidSteerModel->getSensorVals()[1]) / (tpr * ratio) * wheelDiameter.convert(meter) * PI;

        //this will make it so that the sensors don't send crazy feedback at program beginning
        if(std::abs(lpos) >= 1000){lpos = 0;}
        if(std::abs(rpos) >= 1000){rpos = 0;}

        //calculate derivatives of speed to 4th degree
        if(llastPos != 0){  lspeed = lspeedFilter->filter((lpos-  llastPos)/ dt);}
        if(llastSpeed != 0){laccel = laccelFilter->filter((lspeed-llastSpeed)/dt);}
        if(llastAccel != 0){ljerk =  ljerkFilter->filter( (laccel-llastAccel)/dt);}
        if(llastJerk != 0){ lsnap =  lsnapFilter->filter( (ljerk- llastJerk)/dt);}

        if(rlastPos != 0){  rspeed = rspeedFilter->filter((rpos-  rlastPos)/ dt);}
        if(rlastSpeed != 0){raccel = raccelFilter->filter((rspeed-rlastSpeed)/dt);}
        if(rlastAccel != 0){rjerk =  rjerkFilter->filter( (raccel-rlastAccel)/dt);}
        if(rlastJerk != 0){ rsnap =  rsnapFilter->filter( (rjerk- rlastJerk)/dt);}

			std::string msg = "knmcs\t," + 
			std::to_string( (double)pros::millis() / 1000) + 
			"\t," +
			std::to_string( lpos ) + 
			"\t," + 
			std::to_string(lspeed) + 
			"\t,"+ 
			std::to_string(laccel) + 
			"\t," + 
			std::to_string(ljerk) + 
			"\t," + 
			std::to_string(lsnap) + 
			"\n";
			printf(msg.c_str());

		//update last derivative each loop
		llastPos   = lpos;
		llastSpeed = lspeed;
		llastAccel = laccel;
		llastJerk  = ljerk;
		llastSnap  = lsnap;

		rlastPos   = rpos;
		rlastSpeed = rspeed;
		rlastAccel = raccel;
		rlastJerk  = rjerk;
		rlastSnap  = rsnap;

    //place mark for timer
		loopDtTimer->placeMark();

		if(ROS){
			//spin
			nh->spinOnce();
		}


		pros::delay(50);
	}
}

void initalize(){
	graphing = task_create(loop, NULL, TASK_PRIORITY_DEFAULT,
		TASK_STACK_DEPTH_DEFAULT, "graphing");
}

PositionLogging.cpp

pros::task_t odomTask;

void odomFnc(void* params){
	while(true){
		chassis->odom->step();

		double x = chassis->odom->getState().x.convert(meter);
		double y = chassis->odom->getState().y.convert(meter);
		double theta = chassis->odom->getState().theta.convert(radian);

			std::string msg = 
			"odom\t," + 
			std::to_string( (double)pros::millis() / 1000) + 
			"\t," + 
			std::to_string(x)     				 + 
			"\t," + 
			std::to_string(y)     				 + 
			"\t," + 
			std::to_string(theta) 				 + 
			"\n";
			printf(msg.c_str());

		pros::delay(50);
	}
}

void initalize(){
	odomTask = task_create(odomFnc, NULL, TASK_PRIORITY_DEFAULT,
		TASK_STACK_DEPTH_DEFAULT, "odom task");
}