// TODO There is a problem with this algorithm where it returns movement when the finger enters/leaves the sensor
void get_movement(const u8* p_curr, const u8* p_prev, int* p_dx, int* p_dy)
{
int x, y;
int comp_w = nav_setup.width - nav_setup.max_dx;
int comp_h = nav_setup.height - nav_setup.max_dy;
int min_diff = I32_MAX;
// Default vector
*p_dx = 0;
*p_dy = 0;
// Calculate translation vector
for (y = -nav_setup.max_dy; y <= nav_setup.max_dy; ++y)
{
for (x = -nav_setup.max_dx; x <= nav_setup.max_dx; ++x)
{
int diff;
diff = calculate_diff(
p_curr + ((x + nav_setup.max_dx) / 2) + ((y + nav_setup.max_dy) / 2) * nav_setup.width,
p_prev + ((nav_setup.max_dx - x) / 2) + ((nav_setup.max_dy - y) / 2) * nav_setup.width,
comp_w,
comp_h,
min_diff);
if (diff < min_diff)
{
min_diff = diff;
*p_dx = x;
*p_dy = y;
}
}
}
if ((min_diff / (comp_w * comp_h)) > NORM_MIN_DIFF_THRESHOLD)
{
*p_dx = 0;
*p_dy = 0;
}
// Account for masked columns and skipped rows
*p_dx *= nav_setup.col_mask;
*p_dy *= nav_setup.row_skip;
}
// TODO There is a problem with this algorithm where it returns movement when the finger enters/leaves the sensor
void get_movement(const u8* p_curr, const u8* p_prev, int* p_dx, int* p_dy)
{
int x, y;
int comp_w = NAV_IMG_W - MAXDX;
int comp_h = NAV_IMG_H - MAXDY;
int min_diff = I32_MAX;
// Default vector
*p_dx = 0;
*p_dy = 0;
// Calculate translation vector
for (y = -MAXDY; y <= MAXDY; ++y) {
for (x = -MAXDX; x <= MAXDX; ++x) {
int diff;
diff = calculate_diff(
p_curr + ((x + MAXDX) / 2) + ((y + MAXDY) / 2) * NAV_IMG_W,
p_prev + ((MAXDX - x) / 2) + ((MAXDY - y) / 2) * NAV_IMG_H,
comp_w,
comp_h,
min_diff);
if (diff < min_diff) {
min_diff = diff;
*p_dx = x;
*p_dy = y;
}
}
}
if ((min_diff / (comp_w * comp_h)) > NORM_MIN_DIFF_THRESHOLD) {
*p_dx = 0;
*p_dy = 0;
}
// Account for masked columns and skipped rows
*p_dx *= NAV_IMG_COL_MASK;
*p_dy *= NAV_IMG_ROW_SKIP;
}
/* Try to solve in solve_nr_remaining half-moves.
* @param si slice index
* @note assigns solve_result the length of solution found and written, i.e.:
* previous_move_is_illegal the move just played is illegal
* this_move_is_illegal the move being played is illegal
* immobility_on_next_move the moves just played led to an
* unintended immobility on the next move
* <=n+1 length of shortest solution found (n+1 only if in next
* branch)
* n+2 no solution found in this branch
* n+3 no solution found in next branch
* (with n denominating solve_nr_remaining)
*/
void bgl_adjuster_solve(slice_index si)
{
long int const diff = calculate_diff(CURRMOVE_OF_PLY(nbply));
TraceFunctionEntry(__func__);
TraceFunctionParam("%u",si);
TraceFunctionParamListEnd();
assert(BGL_values[trait[nbply]]>=diff); /* BGL enforcer takes care of that */
if (BGL_global)
{
do_bgl_adjustment(White,diff);
do_bgl_adjustment(Black,diff);
}
else
do_bgl_adjustment(trait[nbply],diff);
pipe_solve_delegate(si);
TraceFunctionExit(__func__);
TraceFunctionResultEnd();
}
void get_movement(const u8* p_curr, const u8* p_prev, int* p_dx, int* p_dy) {
int x, y;
int compare_width = NAV_IMG_W - C_W;
int compare_height = NAV_IMG_H - 2 * C_HP;
int min_diff = I32_MAX;
// Move pointers to after padding
p_curr += C_HP * NAV_IMG_W;
p_prev += C_HP * NAV_IMG_W;
// Default vector
*p_dx = 0;
*p_dy = 0;
//Calculate translation vector
for (y = 0; y <= C_H; ++y) {
for (x = 0; x <= C_W; ++x) {
int diff;
diff = calculate_diff(p_prev, p_curr + x + y*NAV_IMG_W, compare_width, compare_height);
if (diff < min_diff) {
min_diff = diff;
*p_dx = x;
*p_dy = y;
}
diff = calculate_diff(p_curr, p_prev + x + y*NAV_IMG_W, compare_width, compare_height);
if (diff < min_diff) {
min_diff = diff;
*p_dx = -x;
*p_dy = -y;
}
diff = calculate_diff(p_prev, p_curr + x - y*NAV_IMG_W, compare_width, compare_height);
if (diff < min_diff) {
min_diff = diff;
*p_dx = x;
*p_dy = -y;
}
diff = calculate_diff(p_curr, p_prev + x - y*NAV_IMG_W, compare_width, compare_height);
if (diff < min_diff) {
min_diff = diff;
*p_dx = -x;
*p_dy = y;
}
}
}
if (min_diff > MIN_DIFF_THRESHOLD) {
*p_dx = 0;
*p_dy = 0;
}
// Account for skipped rows
*p_dy *= NAV_IMG_ROW_SKIP;
*p_dy *= NAV_IMG_COL_MASK;
}
/*
* === FUNCTION =============================================================
* Name: start_next_ad
* Description: Styr muxar, ad och anropar omvandlingar samt anropar bussen
* för att skicka data. Det här kan ses som
* sensorenhetens huvudprogram då alla
* andra funktioner anropas härifrån.
* ============================================================================
*/
void start_next_ad()
{
unsigned char state=control_mux();
if (state==1){ //left_front klar
if(maze_mode==1 && auto_mode==1){
header = 0xC1; //Skicka till
// styr&pc med E-flagga
}
else if(maze_mode == 0 && auto_mode==1){
//linjeläge
header = 0x91; //Skicka till pc
}
else {
header = 0x80;
}
data=dist_left_front;
req_sending();
}
else if (state==2){ //left_back klar
if(maze_mode==1 && auto_mode==1){
header = 0xC5; //Skicka till styr&pc med E-flagga
}
else if(maze_mode == 0 && auto_mode==1){ //linjeläge
header = 0x95; //Skicka till pc
}
else {
header = 0x84;
}
data= dist_left_back;
req_sending();
}
else if (state==3){ //right_front klar
if(maze_mode==1 && auto_mode==1){
header = 0xC9; //Skicka till styr&pc med E-flagga
}
else if(maze_mode == 0 && auto_mode==1){ //linjeläge
header = 0x99; //Skicka till pc
}
else if(auto_mode == 0) {
header = 0x88;
}
data= dist_right_front;
req_sending();
}
else if (state==4){ //right_back klar
if(maze_mode==1 && auto_mode==1){
header = 0xCD; //Skicka till styr&pc med E-flagga
}
else if(maze_mode == 0 && auto_mode==1){ //linjeläge
header = 0x9D; //Skicka till pc
}
else {
header = 0x8C;
}
data= dist_right_back;
req_sending();
}
else if(state==5){ //front klar
if(display_ctr == 10){
data_to_display(dist_right_front,0x00);
data_to_display(dist_left_front,0x01);
data_to_display(dist_right_back,0x02);
data_to_display(dist_left_back,0x03);
data_to_display(dist_front,0x04);
if(maze_mode == 1){
data_to_display(//
(get_next_special_command()/16+100),//
0x05);
}
else{
data_to_display(//
get_next_special_command()/16,0x05);
}
display_ctr = 0;
}
display_ctr++;
}
else if(state==6){
header=0x4D;
data=dist_right_short | 0x80;
req_sending();
}
else if(state==7){
header=0x4D;
data=dist_left_short | 0xC0;
req_sending();
}
else if(state==8){ //linjesensor 0-7 pågår
create_line_array(truncate(ADCH), 1);
}
else if(state==9){ //linjesensor 8-10 pågår
create_line_array(truncate(ADCH), 2);
}
if (count==17){ //alla linjesensorer omvandlade
if (maze_mode==0 && auto_mode==1){
//kod som kollar om banan är s**t
data=calculate_diff(line_array_1, line_array_2);
if(data==0x60){ //bana s**t?
header=0xC3; //skicka till styr och dator
//med stopp-kod, D-flagga satt
}
else {
header=0x51; //Skicka till styr med A- och
//E-flagga
}
req_sending();
//inga linjer? byt till maze_mode=1 om väggar finns
if(line_array_1==0 && line_array_2==0) {
decide_maze_mode(1);
}
}
else if(maze_mode==1 && auto_mode==1){
int temp = markning(find_ones(line_array_1));
generate_special_command(temp);
int temp2 = search_for_crossroad();
if(temp2==1){
send_special_command(get_next_special_command()
| 0x01);
//Resetar next_special_command
generate_special_command(4);
}
else if(temp2==2){
send_special_command(get_next_special_command()
| 0x02);
//Resetar next_special_command
generate_special_command(4);
}
else if(temp2==3 && (get_next_special_command()==0x10))
{
generate_special_command(4);
}
else if(temp2==4){ //vanlig 90 högersväng
header=0xC3;
if(duplicate==1){
data=0x00;
duplicate=0;
}
else {
duplicate=1;
data=0x40;
}
req_sending();
}
else if(temp2==5){ //vanlig 90 vänstersväng
header=0xC3;
if(duplicate==1){
data=0x00;
duplicate=0;
}
else {
duplicate=1;
data=0x50;
}
req_sending();
}
//linjer? byt till maze_mode=0 om inga väggar finns
if(line_array_1!=0 || line_array_2!=0) {
decide_maze_mode(0);
}
}
create_line_array(0,0); //Nollställ
}
else if (count<17){
count++;
ADCSRA |= (1<<ADSC); //starta nästa omvandling
}
}
static boolean is_move_within_bounds(numecoup n)
{
return calculate_diff(n)<=BGL_values[trait[nbply]];
}
