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)
229 lift_pid.set_target(setpoint);
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();