359 lines
6.8 KiB
C
359 lines
6.8 KiB
C
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#include <string.h>
|
|
#include <math.h>
|
|
#include <time.h>
|
|
#include <sys/times.h>
|
|
#include <sys/time.h>
|
|
|
|
#define CPU_FREQ_HZ 100000000
|
|
#include "libsys.h"
|
|
#include "irq.h"
|
|
|
|
uint32_t buffer[16384];
|
|
|
|
#define T_SAMPLE 5000
|
|
#define TIMEOUT 1000
|
|
|
|
typedef struct _bar_t
|
|
{
|
|
uint32_t T;
|
|
uint32_t black;
|
|
} bar_t;
|
|
|
|
enum
|
|
{
|
|
state_idle = 0,
|
|
state_sample,
|
|
state_finish
|
|
};
|
|
|
|
typedef struct _sbar_cand_t
|
|
{
|
|
uint32_t T;
|
|
uint32_t is_primitive;
|
|
} bar_cand_t;
|
|
|
|
bar_t bars[1024];
|
|
|
|
void bsort(float *pData, uint32_t len)
|
|
{
|
|
float t;
|
|
int i, j;
|
|
|
|
for(i=len-1; i>=0; i--)
|
|
{
|
|
for(j=1; j<len; j++)
|
|
{
|
|
if (pData[j-1] > pData[j])
|
|
{
|
|
t = pData[j-1];
|
|
pData[j-1] = pData[j];
|
|
pData[j] = t;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
typedef struct _smedian_t
|
|
{
|
|
uint32_t N;
|
|
uint32_t is_dirty;
|
|
float *pBuf, *pSorted;
|
|
uint32_t w;
|
|
float median;
|
|
} median_t;
|
|
|
|
void Median_init(median_t *pObj, uint32_t N, float init_val)
|
|
{
|
|
int i;
|
|
pObj->pBuf = (float*)malloc(N*sizeof(float));
|
|
pObj->pSorted = (float*)malloc(N*sizeof(float));
|
|
for (i=0; i < N; i++)
|
|
pObj->pBuf[0] = init_val;
|
|
pObj->median = init_val;
|
|
pObj->is_dirty = 0;
|
|
pObj->w = 0;
|
|
pObj->N = N;
|
|
}
|
|
|
|
void Median_free(median_t *pObj)
|
|
{
|
|
if (pObj->pBuf)
|
|
free(pObj->pBuf);
|
|
|
|
if (pObj->pSorted)
|
|
free(pObj->pSorted);
|
|
|
|
pObj->pBuf = NULL;
|
|
pObj->pSorted = NULL;
|
|
|
|
}
|
|
|
|
void Median_value_add(median_t *pObj, float value)
|
|
{
|
|
pObj->is_dirty = 1;
|
|
pObj->pBuf[pObj->w++] = value;
|
|
|
|
if (pObj->w == pObj->N)
|
|
pObj->w = 0;
|
|
|
|
}
|
|
|
|
float Median_get(median_t *pObj)
|
|
{
|
|
if (!pObj->is_dirty)
|
|
return pObj->median;
|
|
|
|
memcpy(pObj->pSorted, pObj->pBuf, pObj->N*sizeof(float));
|
|
|
|
bsort(pObj->pSorted, pObj->N);
|
|
|
|
if (pObj->N&1)
|
|
{
|
|
pObj->median = pObj->pSorted[(pObj->N-1)/2];
|
|
}
|
|
else
|
|
{
|
|
pObj->median = 0.5f*(pObj->pSorted[pObj->N/2] + pObj->pSorted[pObj->N/2+1]);
|
|
}
|
|
|
|
pObj->is_dirty = 0;
|
|
|
|
return pObj->median;
|
|
}
|
|
|
|
// -------------------------------------------------------------
|
|
uint32_t detect_module(uint32_t T, uint32_t T0)
|
|
{
|
|
uint32_t m, best_M;
|
|
float err, kc, best_err;
|
|
|
|
best_err = 4.f;
|
|
best_M = 0;
|
|
|
|
for (m=1; m < 5; m++)
|
|
{
|
|
kc = (float)T/(m*T0);
|
|
err = fabs(kc - 1);
|
|
|
|
if (err < best_err)
|
|
{
|
|
best_M = m;
|
|
best_err = err;
|
|
}
|
|
}
|
|
|
|
return best_M;
|
|
}
|
|
|
|
void handler7(void)
|
|
{
|
|
uint32_t volatile *pTim_ctrl = (uint32_t*)SYS_ITIM_CTRL;
|
|
uint32_t volatile *pTim_stat = (uint32_t*)SYS_ITIM_STAT;
|
|
uint32_t volatile *pPS20_stat = (uint32_t*)SYS_PS2_0_STAT;
|
|
uint32_t volatile *pPS21_stat = (uint32_t*)SYS_PS2_1_STAT;
|
|
static uint32_t state, bar_count;
|
|
static uint32_t timeout_count, timeout_reload;
|
|
static uint32_t white, pin_last, skip, start;
|
|
static uint32_t module_cnt_w, module_cnt_b, Tmod0_w, Tmod0_b, M, T0;
|
|
float ratio, error, kc;
|
|
uint32_t i;
|
|
static median_t med_b, med_w;
|
|
static bar_cand_t bc_b, bc_w;
|
|
static median_t speed_b, speed_w;
|
|
|
|
if (*pTim_stat & 1)
|
|
{
|
|
*pTim_stat = 1;
|
|
white = ((*pPS20_stat & SYS_PS2_BIT_PIN_DATA) == 0);
|
|
|
|
switch (state)
|
|
{
|
|
case state_idle:
|
|
timeout_reload = TIMEOUT;
|
|
if (white != pin_last)
|
|
{
|
|
if (white)
|
|
break;
|
|
|
|
state = state_sample;
|
|
module_cnt_b = 0;
|
|
module_cnt_w = 0;
|
|
Tmod0_w = 0xFFFFFFFF;
|
|
Tmod0_b = 0xFFFFFFFF;
|
|
bar_count = 0;
|
|
Median_init(&med_b, 3, 0);
|
|
Median_init(&med_w, 3, 0);
|
|
Median_init(&speed_b, 3, 1);
|
|
Median_init(&speed_w, 3, 1);
|
|
}
|
|
break;
|
|
|
|
case state_sample:
|
|
if (!timeout_count)
|
|
{
|
|
state = state_finish;
|
|
break;
|
|
}
|
|
|
|
if (white)
|
|
{
|
|
module_cnt_w++;
|
|
}
|
|
else
|
|
{
|
|
module_cnt_b++;
|
|
}
|
|
if (white != pin_last)
|
|
{
|
|
if (pin_last)
|
|
{
|
|
if (bar_count < 10)
|
|
{
|
|
if (1.5*module_cnt_w < Tmod0_w)
|
|
{
|
|
Tmod0_w = module_cnt_w;
|
|
}
|
|
}
|
|
Median_value_add(&med_w, Tmod0_w);
|
|
|
|
timeout_reload = 10*Tmod0_w;
|
|
bars[bar_count].T = module_cnt_w;
|
|
bars[bar_count].black = 0;
|
|
module_cnt_w = 0;
|
|
|
|
}
|
|
else
|
|
{
|
|
if (bar_count < 10)
|
|
{
|
|
if (1.5*module_cnt_b < Tmod0_b)
|
|
{
|
|
Tmod0_b = module_cnt_b;
|
|
}
|
|
}
|
|
Median_value_add(&med_b, Tmod0_b);
|
|
|
|
timeout_reload = 10*Tmod0_b;
|
|
bars[bar_count].T = module_cnt_b;
|
|
bars[bar_count].black = 1;
|
|
module_cnt_b = 0;
|
|
}
|
|
bar_count++;
|
|
|
|
}
|
|
break;
|
|
|
|
case state_finish:
|
|
printf("------------------------------------------------\n");
|
|
printf(" Black White \n");
|
|
skip = 0;
|
|
start = 1;
|
|
Tmod0_w = Median_get(&med_w);
|
|
Tmod0_b = Median_get(&med_b);
|
|
for (i=0; i < bar_count; i++)
|
|
{
|
|
if (bars[i].black)
|
|
{
|
|
T0 = Median_get(&med_b);
|
|
ratio = (float)bars[i].T/T0;
|
|
if (start)
|
|
{
|
|
if (ratio > 4)
|
|
{
|
|
skip = 1;
|
|
continue;
|
|
}
|
|
}
|
|
start = 0;
|
|
skip = 0;
|
|
M = detect_module(bars[i].T, T0);
|
|
kc = (float)bars[i].T/(M*T0);
|
|
error = kc - 1;
|
|
|
|
// printf("M=%d, R=%2.2f, E=%+2.2f [%04d]", M, ratio, error, T0);
|
|
// if (M == 1)
|
|
Median_value_add(&med_b, (uint32_t)(T0*kc));
|
|
Median_value_add(&speed_b, (float)bars[i].T/(M*Tmod0_b));
|
|
|
|
printf("%06d %2.6f ", bars[i].T, Median_get(&speed_b));
|
|
|
|
}
|
|
else
|
|
{
|
|
if (skip)
|
|
continue;
|
|
|
|
T0 = Median_get(&med_w);
|
|
ratio = (float)bars[i].T/T0;
|
|
if (ratio > 9)
|
|
{
|
|
start = 1;
|
|
// printf("\n");
|
|
// printf("------------------------------------------------\n");
|
|
// printf(" Black White \n");
|
|
continue;
|
|
}
|
|
|
|
|
|
M = detect_module(bars[i].T, T0);
|
|
kc = (float)bars[i].T/(M*T0);
|
|
error = kc - 1;
|
|
|
|
// printf(" M=%d, R=%2.2f, E=%+2.2f [%04d]", M, ratio, error, T0);
|
|
// if (M == 1)
|
|
Median_value_add(&med_w, (uint32_t)(T0*kc));
|
|
Median_value_add(&speed_w, (float)bars[i].T/(M*Tmod0_w));
|
|
|
|
printf("%06d %2.6f ", bars[i].T, Median_get(&speed_w));
|
|
printf("\n");
|
|
}
|
|
}
|
|
Median_free(&med_b);
|
|
Median_free(&med_w);
|
|
Median_free(&speed_b);
|
|
Median_free(&speed_w);
|
|
if (bar_count)
|
|
printf("\n");
|
|
|
|
state = state_idle;
|
|
break;
|
|
|
|
}
|
|
if (white != pin_last)
|
|
timeout_count = timeout_reload;
|
|
|
|
pin_last = white;
|
|
if (timeout_count)
|
|
timeout_count--;
|
|
|
|
}
|
|
}
|
|
|
|
int main(void)
|
|
{
|
|
|
|
int i;
|
|
uint32_t volatile *pTim_ctrl = (uint32_t*)SYS_ITIM_CTRL;
|
|
uint32_t volatile *pTim_stat = (uint32_t*)SYS_ITIM_STAT;
|
|
uint32_t volatile *pTim0_cnt = (uint32_t*)SYS_ITIM0_CNT;
|
|
uint32_t volatile *pTim0_cmp = (uint32_t*)SYS_ITIM0_CMP;
|
|
|
|
*pTim0_cnt = 0;
|
|
*pTim0_cmp = (uint32_t)T_SAMPLE;
|
|
*pTim_stat = 1;
|
|
*pTim_ctrl = 3;
|
|
|
|
interrupt_register(7, (void*)handler7);
|
|
|
|
printf("Jenners BarCoder\n");
|
|
|
|
interrupt_enable(7);
|
|
|
|
while(1)
|
|
{
|
|
}
|
|
return 0;
|
|
}
|