3#include "../core/include/utils/controls/pid.h"
20template <
typename T>
class Lift {
29 double up_speed, down_speed;
30 double softstop_up, softstop_down;
56 Lift(motor_group &lift_motors,
lift_cfg_t &lift_cfg, map<T, double> &setpoint_map, limit *homing_switch = NULL)
57 : lift_motors(lift_motors), cfg(lift_cfg), lift_pid(cfg.lift_pid_cfg), setpoint_map(setpoint_map),
58 homing_switch(homing_switch) {
95 if (get_sensor == NULL)
96 cur_pos = lift_motors.position(rev);
98 cur_pos = get_sensor();
100 if (up_ctrl && cur_pos < cfg.softstop_up) {
101 lift_motors.spin(directionType::fwd, cfg.up_speed, volt);
102 setpoint = cur_pos + .3;
108 }
else if (down_ctrl && cur_pos > cfg.softstop_down) {
110 if (setpoint > cfg.softstop_down)
111 setpoint = setpoint - (tmr.time(sec) * cfg.down_speed);
131 static bool down_hold =
false;
132 static bool init =
true;
135 if (init || up_btn || down_btn) {
140 double rev = lift_motors.position(rotationUnits::rev);
142 if (rev < cfg.softstop_down && down_btn)
147 if (up_btn && rev < cfg.softstop_up)
148 lift_motors.spin(directionType::fwd, volt_up, voltageUnits::volt);
149 else if (down_btn && rev > cfg.softstop_down && !down_hold)
150 lift_motors.spin(directionType::rev, volt_down, voltageUnits::volt);
152 lift_motors.spin(directionType::fwd, 0, voltageUnits::volt);
168 static bool up_last = up_step, down_last = down_step;
170 bool up_rising = up_step && !up_last;
171 bool down_rising = down_step && !down_last;
174 down_last = down_step;
176 static int cur_index = 0;
179 if (cur_index >= pos_list.size())
180 cur_index = pos_list.size() - 1;
183 if (up_rising && cur_index < (pos_list.size() - 1))
185 else if (down_rising && cur_index > 0)
202 this->setpoint = setpoint_map[pos];
215 this->setpoint = val;
232 if (get_sensor != NULL)
233 lift_pid.
update(get_sensor());
235 lift_pid.
update(lift_motors.position(rev));
239 lift_motors.spin(fwd, lift_pid.
get(), volt);
250 while (tmr.time(sec) < 3) {
251 lift_motors.spin(directionType::rev, 6, volt);
253 if (homing_switch == NULL && lift_motors.current(currentUnits::amp) > 1.5)
255 else if (homing_switch != NULL && homing_switch->pressing())
259 if (reset_sensor != NULL)
262 lift_motors.resetPosition();
298 motor_group &lift_motors;
301 map<T, double> &setpoint_map;
302 limit *homing_switch;
304 atomic<double> setpoint;
305 atomic<bool> is_async;
307 double (*get_sensor)(void) = NULL;
308 void (*reset_sensor)(void) = NULL;
void set_sensor_function(double(*fn_ptr)(void))
Definition lift.h:287
bool set_setpoint(double val)
Definition lift.h:214
bool get_async()
Definition lift.h:269
void set_sensor_reset(void(*fn_ptr)(void))
Definition lift.h:295
void control_setpoints(bool up_step, bool down_step, vector< T > pos_list)
Definition lift.h:166
void set_async(bool val)
Definition lift.h:276
void hold()
Definition lift.h:228
Lift(motor_group &lift_motors, lift_cfg_t &lift_cfg, map< T, double > &setpoint_map, limit *homing_switch=NULL)
Definition lift.h:56
bool set_position(T pos)
Definition lift.h:201
void home()
Definition lift.h:246
void control_manual(bool up_btn, bool down_btn, int volt_up, int volt_down)
Definition lift.h:130
double get_setpoint()
Definition lift.h:222
void control_continuous(bool up_ctrl, bool down_ctrl)
Definition lift.h:89
void set_target(double target)
Definition pid.cpp:109
double update(double sensor_val) override
Definition pid.cpp:23
double get() override
Definition pid.cpp:92
bool is_on_target() override
Definition pid.cpp:124
double get_target() const
Definition pid.cpp:104