climber_setpoints.h

Go to the documentation of this file.

 
 
#pragma once
 
#include <units/length.h>
#include <units/velocity.h>
 
#include <vector>
 
#include "constants/measure_up.h"
#include "utils/sensor_conversions.h"
 
struct ClimberPoint {
  units::inch_t hookExtension;
  units::inch_t armExtension;
  units::velocity::inches_per_second_t hookSpeed;
  units::velocity::inches_per_second_t armSpeed;
  char pidSlot;
};
 
namespace ClimberSetpoints {
  constexpr ClimberPoint setup = {measure_up::climber_hook::maxExtension - 1_in, 37_in, 25_ips, 25_ips, 0};
  constexpr ClimberPoint readyClimb = {measure_up::climber_hook::maxExtension - 1_in, 31.5_in, 25_ips, 25_ips, 0};
  constexpr ClimberPoint storage = {measure_up::climber_hook::maxExtension - 6_in, 21.5_in, 25_ips, 25_ips, 0};
  constexpr ClimberPoint latchL2 = {30.75_in, 37_in, 17_ips, 17_ips, 0};
  constexpr ClimberPoint prepL2 = {28.7_in, 35.8_in, 10_ips, 10_ips, 0};
  constexpr ClimberPoint secureL2 = {1_in, 37_in, 18_ips, 18_ips, 1};
  constexpr ClimberPoint passL3 = {34.4_in, 31.1_in, 20_ips, 20_ips, 0};
  constexpr ClimberPoint latchL3 = {31.5_in, 37.1_in, 10_ips, 10_ips, 0};
  constexpr ClimberPoint prepTransferL3 = {31.5_in, 29_in, 10_ips, 10_ips, 0};
  constexpr ClimberPoint transferL3 = {26_in, 29_in, 10_ips, 10_ips, 0};
  constexpr ClimberPoint holdL3 = {26_in, 31.1_in, 17_ips, 25_ips, 1};
 
  namespace ClimbBeta {
    constexpr ClimberPoint secureL2 = {3.48_in, 37_in, 20_ips, 18_ips, 1};
    constexpr ClimberPoint prepReleaseL2 = {5.48_in, 31.1_in, 20_ips, 20_ips, 1};
    constexpr ClimberPoint releaseL2 = {18_in, 31.1_in, 18_ips, 18_ips, 1};
  }  // namespace ClimbBeta
 
  // all the setpoints for ready here
  namespace PreClimb {
    const std::vector<ClimberPoint> preClimbSequence = {storage, readyClimb, setup, latchL2};
  }  // namespace PreClimb
 
}  // namespace ClimberSetpoints
 
// the sequence to get fed into climb command
namespace ClimberSequence {
  const std::vector<ClimberPoint> sequence = {ClimberSetpoints::ClimbBeta::secureL2,
                                              ClimberSetpoints::ClimbBeta::prepReleaseL2,
                                              ClimberSetpoints::ClimbBeta::releaseL2};
}  // namespace ClimberSequence