lift.h

#pragma once
 
#include "core/utils/controls/pid.h"
#include "vex.h"
#include <atomic>
#include <iostream>
#include <map>
#include <vector>
 
using namespace std;

template <typename T> class Lift {
public:
  struct lift_cfg_t {
    double up_speed, down_speed;
    double softstop_up, softstop_down;
 
    PID::pid_config_t lift_pid_cfg;
  };

  Lift(vex::motor_group &lift_motors, lift_cfg_t &lift_cfg, map<T, double> &setpoint_map, vex::limit *homing_switch = NULL)
      : lift_motors(lift_motors), cfg(lift_cfg), lift_pid(cfg.lift_pid_cfg), setpoint_map(setpoint_map),
        homing_switch(homing_switch) {
 
    is_async = true;
    setpoint = 0;
 
    // Create a background task that is constantly updating the lift PID, if requested.
    // Set once, and forget.
    vex::task t(
        [](void *ptr) {
          Lift &lift = *((Lift *)ptr);
 
          while (true) {
            if (lift.get_async())
              lift.hold();
 
            vexDelay(50);
          }
 
          return 0;
        },
        this);
  }

  void control_continuous(bool up_ctrl, bool down_ctrl) {
    static vex::timer tmr;
 
    double cur_pos = 0;
 
    // Check if there's a hook for a custom sensor. If not, use the motors.
    if (get_sensor == NULL)
      cur_pos = lift_motors.position(vex::rev);
    else
      cur_pos = get_sensor();
 
    if (up_ctrl && cur_pos < cfg.softstop_up) {
      lift_motors.spin(vex::directionType::fwd, cfg.up_speed, vex::volt);
      setpoint = cur_pos + .3;
 
      // std::cout << "DEBUG OUT: UP " << setpoint << ", " << tmr.time(sec) << ", " << cfg.down_speed << "\n";
 
      // Disable the PID while going UP.
      is_async = false;
    } else if (down_ctrl && cur_pos > cfg.softstop_down) {
      // Lower the lift slowly, at a rate defined by down_speed
      if (setpoint > cfg.softstop_down)
        setpoint = setpoint - (tmr.time(vex::sec) * cfg.down_speed);
      // std::cout << "DEBUG OUT: DOWN " << setpoint << ", " << tmr.time(sec) << ", " << cfg.down_speed << "\n";
      is_async = true;
    } else {
      // Hold the lift at the last setpoint
      is_async = true;
    }
 
    tmr.reset();
  }

  void control_manual(bool up_btn, bool down_btn, int volt_up, int volt_down) {
    static bool down_hold = false;
    static bool init = true;
 
    // Allow for setting position while still calling this function
    if (init || up_btn || down_btn) {
      init = false;
      is_async = false;
    }
 
    double rev = lift_motors.position(vex::rotationUnits::rev);
 
    if (rev < cfg.softstop_down && down_btn)
      down_hold = true;
    else if (!down_btn)
      down_hold = false;
 
    if (up_btn && rev < cfg.softstop_up)
      lift_motors.spin(vex::directionType::fwd, volt_up, vex::voltageUnits::volt);
    else if (down_btn && rev > cfg.softstop_down && !down_hold)
      lift_motors.spin(vex::directionType::rev, volt_down, vex::voltageUnits::volt);
    else
      lift_motors.spin(vex::directionType::fwd, 0, vex::voltageUnits::volt);
  }

  void control_setpoints(bool up_step, bool down_step, vector<T> pos_list) {
    // Make sure inputs are only processed on the rising edge of the button
    static bool up_last = up_step, down_last = down_step;
 
    bool up_rising = up_step && !up_last;
    bool down_rising = down_step && !down_last;
 
    up_last = up_step;
    down_last = down_step;
 
    static int cur_index = 0;
 
    // Avoid an index overflow. Shouldn't happen unless the user changes pos_list between calls.
    if (cur_index >= pos_list.size())
      cur_index = pos_list.size() - 1;
 
    // Increment or decrement the index of the list, bringing it up or down.
    if (up_rising && cur_index < (pos_list.size() - 1))
      cur_index++;
    else if (down_rising && cur_index > 0)
      cur_index--;
 
    // Set the lift to hold the position in the background with the PID loop
    set_position(pos_list[cur_index]);
    is_async = true;
  }

  bool set_position(T pos) {
    this->setpoint = setpoint_map[pos];
    is_async = true;
 
    return (lift_pid.get_target() == this->setpoint) && lift_pid.is_on_target();
  }

  bool set_setpoint(double val) {
    this->setpoint = val;
    return (lift_pid.get_target() == this->setpoint) && lift_pid.is_on_target();
  }

  double get_setpoint() { return this->setpoint; }

  void hold() {
    lift_pid.set_target(setpoint);
    // std::cout << "DEBUG OUT: SETPOINT " << setpoint << "\n";
 
    if (get_sensor != NULL)
      lift_pid.update(get_sensor());
    else
      lift_pid.update(lift_motors.position(vex::rev));
 
    // std::cout << "DEBUG OUT: ROTATION " << lift_motors.rotation(rev) << "\n\n";
 
    lift_motors.spin(vex::fwd, lift_pid.get(), vex::volt);
  }

  void home() {
    static vex::timer tmr;
    tmr.reset();
 
    while (tmr.time(vex::sec) < 3) {
      lift_motors.spin(vex::directionType::rev, 6, vex::volt);
 
      if (homing_switch == NULL && lift_motors.current(vex::currentUnits::amp) > 1.5)
        break;
      else if (homing_switch != NULL && homing_switch->pressing())
        break;
    }
 
    if (reset_sensor != NULL)
      reset_sensor();
 
    lift_motors.resetPosition();
    lift_motors.stop();
  }

  bool get_async() { return is_async; }

  void set_async(bool val) { this->is_async = val; }

  void set_sensor_function(double (*fn_ptr)(void)) { this->get_sensor = fn_ptr; }

  void set_sensor_reset(void (*fn_ptr)(void)) { this->reset_sensor = fn_ptr; }
 
private:
  vex::motor_group &lift_motors;
  lift_cfg_t &cfg;
  PID lift_pid;
  map<T, double> &setpoint_map;
  vex::limit *homing_switch;
 
  atomic<double> setpoint;
  atomic<bool> is_async;
 
  double (*get_sensor)(void) = NULL;
  void (*reset_sensor)(void) = NULL;
};