58 lines
1.1 KiB
C
58 lines
1.1 KiB
C
|
#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++;
|
||
|
|
}
|