talon/User/mode_position.c

58 lines
1.1 KiB
C
Raw Normal View History

2024-01-13 12:57:51 -05:00
#include "mode_position.h"
#include "ctre.h"
#include "memecoder.h"
int dicks;
int balls;
void mode_position_init(app_state_t *app, mode_position_state_t *state) {
state->time = 0;
state->zeroed = 0;
state->position = 0;
dicks = 1;
balls = 0;
}
void mode_position(app_state_t *app, mode_position_state_t *state) {
ctre_feed_enable(app->switch_state == 2);
printf("Pos: %d\r\n", state->position);
if(app->switch_state == 1) {
if(!state->zeroed) {
state->position = 0;
memecoder_zero();
// Set sensor position to 0
ctre_set_param(app->device, 331, 0, 0, 0);
}
state->zeroed = 1;
} else {
state->zeroed = 0;
}
if(state->time % 20 == 0) {
int mc = memecoder_get();
int cmd_pos = mc * 5 * 2048 / 42;
// state->position = (state->position * 9 + cmd_pos) / 10;
ctre_position_out(app->device, state->position);
}
if(dicks % 2000 == 0) {
balls = !balls;
dicks = 1;
if(balls) {
// state->position = -133120;
// state->position = 95320;
state->position = 350000;
} else {
state->position = 0;
}
}
dicks++;
state->time++;
}