void DBG_testUltraDistance(S_robot*r, uint32_t reps)
{
uint32_t period = 100;
uint32_t prStart = _tic();
S_sensor_ultra* u = 0;
u = r->ults.u[0];
while(reps>1)
{
CLOCK_digiPrint(r);
if( _tocFrom(prStart) > period )
{
gpio_toggle(PLED,LEDBLUE3);
LCD_gotoxy(r->lcd,0,0);
fprintf(r->flcd, "=%0.5f|s=%u", u->dist, u->state );
LCD_gotoxy(r->lcd,0,1);
fprintf(r->flcd, "=%6lu |p=%u ", u->nTicks, u->nOwerflow);
//fprintf(r->flcd, "[s=%u][%4u]", u->state, cnt);
//cnt = timer_get_counter(u->TIMX);
prStart = _tic();
}
reps--;
}
int main_debug(S_robot* r)
{
_tic();
ROBOT_initLifeDebug(r);
fprintf(r->flcd, "Init=");
_tocPrint(r->flcd);
mswait(400);
LCD_clear(r->lcd);
ROBOT_START(r);
uint32_t period = 10;
uint32_t prStart = _tic();
uint8_t q=0;
//uint32_t ocval = 0;
S_robot_dcmotors* ds = 0;
ds = &(r->dcs);
S_actuator_dcmotor* m = 0;
m = ds->m[q];
float dcadd = 0.00001;
mswait(1000);
while(1)
{
CLOCK_digiPrint(r);
if(m->dutyCycle >= 1.0) m->dutyCycle = 0.0;
DCMOT_SET_dutyCycle(m, m->dutyCycle+dcadd);
}
while (1)
{
/**
DO not debug inside this while!
Make a separate function like DBG_tryADC(void),
and call it from here :)
*/
// DBG_testButtonState(r,100,10);
// DBG_testUltraDistanceOld(r,100);
//TRY_buzzer();
//LCD_displayWriteCheck(r->lcd);
//dev_LCD_checkSeek(flcd);
//DBG_tryADC(r);
//DBG_tryCNY70(r
//DBG_testUltraDistance(r,0xFFFF);
//DBG_testAllUltraDistance(r,0xFFFF);
DBG_testActuators(r,0xFFFFFF);
//INIT_tim(r);
}
return 0;
}
void DBG_testActuators_wInit(S_robot*r, uint32_t reps)
{
uint32_t period = 100;
uint32_t prStart = _tic();
uint8_t q=0;
//uint32_t ocval = 0;
//init
rcc_clock_setup_in_hse_8mhz_out_72mhz();
/* enable GPIOC clock */
rcc_periph_clock_enable(RCC_GPIOC);
/*
* set GPIO12 at PORTC (led) to 'output alternate function push-pull'.
*/
// gpio_set_mode(GPIOC, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_PUSHPULL, GPIO12);
gpio_mode_setup(GPIOA,GPIO_MODE_AF,GPIO_PUPD_NONE,GPIO1);
gpio_set_af(GPIOA,GPIO_AF1,GPIO1);
uint32_t *p = TIM2_CCMR1;
*p = 0x6800;
p = TIM2_CR1 ;
*p |= 1<<7; // ARPE
p = TIM2_EGR ;
*p |= 1<<0;
p = TIM2_CCER ;
*p |= 1<<4; //enable output
while(reps>1)
{
CLOCK_digiPrint(r);
if( _tocFrom(prStart) > period )
{
//TIM_CCR1(m->TIMX) = m->OCval;
//ocval += 10;
//model_timer_SET_outputCompare(TIM2,TIM_OC2,ocval);
// printit
LCD_gotoxy(r->lcd,0,0);
LCD_gotoxy(r->lcd,0,1);
//fprintf(r->flcd, "");
//for(q=0;q<ROB_MOTOR_COUNT;q++)
//fprintf(r->flcd, "%.0f%%=%lu/%lu", ds->m[q]->dutyCycle*100, ds->m[q]->OCval, ds->m[q]->tim_s->period );
//fprintf(r->flcd, "oc=%lu=%lu", ocval, m->OCval );
prStart = _tic();
}
reps--;
}
void DBG_testActuators(S_robot*r, uint32_t reps)
{
uint32_t period = 10;
uint32_t prStart = _tic();
uint8_t q=0;
//uint32_t ocval = 0;
S_robot_dcmotors* ds = 0;
ds = &(r->dcs);
S_actuator_dcmotor* m = 0;
m = ds->m[q];
float dcadd = 0.00001;
mswait(1000);
while(1)
{
CLOCK_digiPrint(r);
if(m->dutyCycle >= 1.0) m->dutyCycle = 0.0;
DCMOT_SET_dutyCycle(m, m->dutyCycle+dcadd);
}
while(reps>1)
{
CLOCK_digiPrint(r);
if( _tocFrom(prStart) > period )
{
// cycle dutyCycle
if(m->dutyCycle >= 1.0) m->dutyCycle = 0.0;
DCMOT_SET_dutyCycle(m, m->dutyCycle+0.02);
TIM_CCR1(m->TIMX) = m->OCval;
//ocval += 10;
//model_timer_SET_outputCompare(TIM2,TIM_OC2,ocval);
// printit
LCD_gotoxy(r->lcd,0,0);
fprintf(r->flcd, "%.0f%%", ds->m[q]->dutyCycle*100);
LCD_gotoxy(r->lcd,0,1);
//fprintf(r->flcd, "");
//for(q=0;q<ROB_MOTOR_COUNT;q++)
fprintf(r->flcd, "%lu/%lu|", ds->m[q]->OCval, ds->m[q]->tim_s->period );
//fprintf(r->flcd, "oc=%lu=%lu", ocval, m->OCval );
prStart = _tic();
}
reps--;
}
gpio_toggle(PLED,LEDORANGE1);
static void dbg_memspy_init() { // initial state of the analyzed memory
bmp_printf(FONT_MED, 10,10, "memspy init @ f:%x ... (l:+%x) ... t:%x", mem_spy_start, mem_spy_len, mem_spy_start + mem_spy_len * 4);
SleepTask(1000);
//debug_log("memspy init @ from:%x ... (len:%x) ... to:%x", mem_spy_start, mem_spy_len, mem_spy_start + mem_spy_len * 4);
//mem_spy_len is number of int32's
if (!dbg_memmirror) dbg_memmirror = AllocateMemory(mem_spy_len*4 + 100); // local copy of mem area analyzed
if (!dbg_memmirror) return;
if (!dbg_memchanges) dbg_memchanges = AllocateMemory(mem_spy_len*4 + 100); // local copy of mem area analyzed
if (!dbg_memchanges) return;
//if (!dbg_memlogged) dbg_memlogged = AllocateMemory(mem_spy_len*4 + 100); // is this change logged
//if (!dbg_memlogged) return;
int i;
int crc = 0;
for (i = 0; i < mem_spy_len; i++) {
unsigned int addr = dbg_memspy_get_addr(i);
dbg_memmirror[i] = MEMX(addr);
dbg_memchanges[i] = 0;
//dbg_memlogged[i] = 0;
crc += dbg_memmirror[i];
}
bmp_printf(FONT_MED, 10,10, "memspy OK: %x", crc);
//debug_log("memspy OK: %x", crc);
_tic();
SleepTask(1000);
}
void DBG_testAllUltraDistance(S_robot*r, uint32_t reps)
{
uint32_t period = 100;
uint32_t prStart = _tic();
uint8_t q=0;
S_robot_ultras* us = 0;
us = &(r->ults);
while(reps>1)
{
CLOCK_digiPrint(r);
if( _tocFrom(prStart) > period )
{
gpio_toggle(PLED,LEDBLUE3);
LCD_gotoxy(r->lcd,0,0);
fprintf(r->flcd, "s");
for(q=0;q<ROB_ULTRA_COUNT;q++)
fprintf(r->flcd, "%u", us->u[q]->state );
fprintf(r->flcd, "p");
for(q=0;q<ROB_ULTRA_COUNT;q++)
fprintf(r->flcd, "%u", us->u[q]->nOwerflow);
fprintf(r->flcd, " ");
//fprintf(r->flcd, "p=");
//u->nOwerflow
LCD_gotoxy(r->lcd,0,1);
//fprintf(r->flcd, "a");
for(q=0;q<ROB_ULTRA_COUNT;q++)
{
fprintf(r->flcd, "%3.0f|", us->u[q]->dist );
//fprintf(r->flcd, "%3.0f", us->u[q]->dist );
//if(q<3) fprintf(r->flcd, "|");
}
//fprintf(r->flcd, "[s=%u][%4u]", u->state, cnt);
//cnt = timer_get_counter(u->TIMX);
prStart = _tic();
}
reps--;
}
void DBG_ticTocSumoWait(S_robot* r)
{
while(1)
{
_tic();
uint8_t a=0;
for(a=0; a<5; a++) SUMO_wait5secAndSample(r);
_tocPrint(r->flcd);
mswait(300);
}
void CLOCK_digiPrint(S_robot* r)
{
static uint32_t world_time = 0;
static uint32_t last_wtime = 0;
world_time = _tic()/1000;
if(world_time != last_wtime)
{
char str_hhmmss[] = "%2lu:%02lu:%02lu";
char str_mss[] = "%2lu:%02lu";
char str_s[] = "%2lu";
uint32_t hh = (world_time/3600);
uint32_t mm = (world_time/60)%60;
uint32_t ss = world_time%60;
if(mm > 0)
{
if(hh>0)
{
LCD_gotoxy(r->lcd,8,0);
fprintf(r->flcd, str_hhmmss, hh, mm, ss);
}
else
{
LCD_gotoxy(r->lcd,11,0);
fprintf(r->flcd, str_mss, mm, ss);
}
}
else
{
LCD_gotoxy(r->lcd,14,0);
fprintf(r->flcd, str_s, ss);
}
last_wtime = world_time;
}
