extras.hpp

Go to the documentation of this file.

#pragma once
 
#include "LightLib/lib.hpp"
#include "LightLib/drive/odometry.hpp"
#include "pros/motor_group.hpp"
 
void screen_print_tracker(light::tracking_wheel* tracker, std::string name, int line);
 
void screen_task();
 
void lib_extras();
 
void checkMotorTemp(pros::Controller& controller, pros::Motor& Top, pros::Motor& Bottom);
 
void default_positions();
 
void track_basket();
 
namespace light {
void extras_init(light::Drive* chassis,
                 pros::MotorGroup* leftMotors,
                 pros::MotorGroup* rightMotors);
 
void getDriveMotorGroups(pros::MotorGroup** leftOut,
                         pros::MotorGroup** rightOut);
 
light::Drive* getChassis();
}  // namespace light
 
struct LightPID {
  float kP, kI, kD;             
  float prevError = 0;          
  float integral = 0;           
  float integralCap = 1000.0f;  
 
  LightPID(float kP, float kI, float kD) : kP(kP), kI(kI), kD(kD) {}
 
  float update(float error) {
    integral += error;
    if (integralCap > 0.0f) {
      if (integral > integralCap) integral = integralCap;
      if (integral < -integralCap) integral = -integralCap;
    }
    float derivative = error - prevError;
    prevError = error;
    return kP * error + kI * integral + kD * derivative;
  }
 
  void reset() {
    prevError = 0;
    integral = 0;
  }
};
 
void moveToPoint(float x, float y, int timeout,
                 float maxSpeed = 127.0f, bool reversed = false);