void out_gyro()
{
short roop_frag=0;
short deg=0;
short Gy=0;
short gyr=0;
short ave_gyr=0;
short i=0;
reset();
while(roop_frag==0){
if(PORTE.PORT.BIT.B0 == 0){
TIMER_WAIT(2000);
for(i=0; i<100; i++)
zero_gyro=get_gyro();
start0=1;
roop_frag=1;
}
}
while(1){
Gy=(short)GYRO_ERR;
gyr=(short)gyro2;
ave_gyr=(short)ave_gyro;
dec_out(Gy, 5); outs(" ");
dec_out(deg, 5); outs(" ");
dec_out(gyr, 5); outs(" ");
dec_out(ave_gyr, 5); outs(" ");
outs("\n");
}
}
void maxmin_out()
{
short i=0;
for(i=1; i<9; i++){
dec_out(SEN_max[i],5); outs(" ");
dec_out(SEN_min[i],5); outs(" ");
dec_out(SEN_minmax[i],5); outs("\n"); outs("\n");
}
}
void out_ENC()
{
TIMER_WAIT(1000);
ENC_R=0; //max:65535
ENC_L=0;
start0=1;
while(1){
dec_out(now_ENC_R, 8); outs(" ");
dec_out(now_ENC_L, 8); outs(" ");
dec_out((now_ENC_R-now_ENC_L)/6400, 8); outs(" ");
outs("\n");
}
}
void out_curve() //ログチェック
{
short i=0;
TIMER_WAIT(2000);
dec_out(ST_count, 8); outs("\n");
for(i=0; i<201; i++){
// dec_out(ST_check[i], 8); outs(" ");
// dec_out((short)sub_SEN_check[i], 8); outs(" ");
// dec_out(vel_check[i], 8); outs(" ");
dec_out(ENC_check[i], 8); outs(" ");
dec_out((short)ENC_dis[i], 8); outs("\n");
}
}
void sens_out()
{
short i=0;
get_sens_frag=1;
while(1){
for(i=1; i<9; i++)
dec_out(ave_SEN[i],4); outs(" ");
outs("\n");
}
}
void line_check()
{
short i;
double max=0.0;
TIMER_WAIT(2000);
while(1){
get_senc();
dec_out((short)SEN[goal], 4); outs("\n");
}
}
void out_senc_drive_mot()
{
short i;
TIMER_WAIT(2000);
search_max();
start0=1;
mot_STB(START_A);
mot_frag=1;
TIMER_WAIT(2000);
while(1){
MTU0.TGRA=100;
MTU0.TGRC=100;
for(i=1; i<11; i++){
dec_out((short)ave_SEN[i], 4); outs(" ");
}
outs("\n");
}
}
static int _vprintf_f(putchar_f f, void *ctx, const char *fmt, va_list va)
{
char c;
int r = 0;
int escaped = 0;
unsigned int longs = 0;
uint64_t v;
while ((c = (*fmt++))) {
if (escaped) {
switch(c) {
case '%':
f(ctx, c);
r++;
break;
case 'p':
r += hex_out(f, ctx, (uintptr_t) va_arg(va, void *));
break;
case 'u':
case 'd':
case 'x':
switch (longs) {
default:
v = va_arg(va, unsigned int);
break;
case 1:
v = va_arg(va, long unsigned int);
break;
case 2:
v = va_arg(va, long long unsigned int);
break;
}
if (c == 'x') {
r += hex_out(f, ctx, v);
} else {
r += dec_out(f, ctx, v);
}
break;
case 'c':
f(ctx, va_arg(va, int));
r += 1;
break;
case 's':
r += str_out(f, ctx, va_arg(va, char *));
break;
case 'z':
continue;
case 'l':
longs++;
continue;
default:
break;
}
escaped = 0;
longs = 0;
} else {
if (c == '%') {
escaped = 1;
}
else
f(ctx, c);
}
}
