45 lines
1.1 KiB
C
45 lines
1.1 KiB
C
#include "mode_position2.h"
|
|
|
|
#include "ctre.h"
|
|
#include "memecoder.h"
|
|
|
|
|
|
|
|
void mode_position2_init(app_state_t *app, mode_position2_state_t *state) {
|
|
state->time = 0;
|
|
state->zeroed = 0;
|
|
state->position = 0;
|
|
state->position2 = 0;
|
|
}
|
|
|
|
void mode_position2(app_state_t *app, mode_position2_state_t *state) {
|
|
ctre_feed_enable(app->switch_state > 0);
|
|
|
|
printf("Pos: %d, %d\r\n", state->position, state->position2);
|
|
if(app->switch_state > 0) {
|
|
if(!state->zeroed) {
|
|
if(app->switch_state == 1)
|
|
memecoder_set(state->position * 42 / 2048 / 5);
|
|
else
|
|
memecoder_set(state->position2 * 42 / 2048 / 5);
|
|
}
|
|
state->zeroed = 1;
|
|
} else {
|
|
state->zeroed = 0;
|
|
}
|
|
|
|
if(state->time % 20 == 0) {
|
|
int mc = memecoder_get();
|
|
int cmd_pos = mc * 5 * 2048 / 42;
|
|
if(app->switch_state == 1) {
|
|
state->position = (state->position * 9 + cmd_pos) / 10;
|
|
ctre_position_out(10, state->position);
|
|
}
|
|
if(app->switch_state == 2) {
|
|
state->position2 = (state->position2 * 9 + cmd_pos) / 10;
|
|
ctre_position_out(30, state->position2);
|
|
}
|
|
}
|
|
|
|
state->time++;
|
|
}
|