44 lines
1.1 KiB
C
44 lines
1.1 KiB
C
|
#include "mode_openloop.h"
|
||
|
|
|
||
|
|
#include "ctre.h"
|
||
|
|
#include "memecoder.h"
|
||
|
|
|
||
|
|
|
||
|
|
void mode_openloop_init(app_state_t *app, mode_openloop_state_t *state) {
|
||
|
|
state->time = 0;
|
||
|
|
state->is_continuous = 0;
|
||
|
|
state->velocity = 0.0;
|
||
|
|
state->prev_mc = 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
void mode_openloop(app_state_t *app, mode_openloop_state_t *state) {
|
||
|
|
ctre_feed_enable(app->switch_state > 0);
|
||
|
|
|
||
|
|
if(app->switch_state == 1) {
|
||
|
|
if(!state->is_continuous) memecoder_zero();
|
||
|
|
state->is_continuous = 1;
|
||
|
|
} else {
|
||
|
|
state->is_continuous = 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
if(state->time % 20 == 0) {
|
||
|
|
int mc = memecoder_get();
|
||
|
|
// printf("mc: %d, is_continuous: %d\r\n", mc, state->is_continuous);
|
||
|
|
if(state->is_continuous) {
|
||
|
|
ctre_percent_out(app->device, (double)mc / 128.0);
|
||
|
|
// ctre_position_out(app->device, mc * 1024 / 42);
|
||
|
|
} else {
|
||
|
|
int delta = mc - state->prev_mc;
|
||
|
|
int delta_exp = delta * delta;
|
||
|
|
if(delta < 0) delta_exp *= -1;
|
||
|
|
state->velocity += (double)delta_exp * .01;
|
||
|
|
ctre_percent_out(app->device, state->velocity);
|
||
|
|
// printf("velocity: %lf\r\n", state->velocity);
|
||
|
|
}
|
||
|
|
state->prev_mc = mc;
|
||
|
|
state->velocity *= 0.90;
|
||
|
|
}
|
||
|
|
|
||
|
|
state->time++;
|
||
|
|
}
|