/*! \brief Normalizes relative motion data to the given interval.
*/
point
normalize_relative
(const point input, const point lower, const point upper)
{
point out;
point slope;
slope = get_slope(lower, upper);
out.X = input.X * slope.X;
out.Y = input.Y * slope.Y;
return out;
}
/*! \brief Normalizes an absolute position to the range [-1, 1] in the
* given interval.
*/
point
normalize_absolute
(const point input, const point lower, const point upper)
{
point p1;
p1.X = 1.0;
p1.Y = 1.0;
point slope, intercept, out;
slope = get_slope(lower, upper);
intercept.X = p1.X - (slope.X * upper.X);
intercept.Y = p1.Y - (slope.Y * upper.Y);
out.X = (input.X * slope.X) + intercept.X;
out.Y = (input.Y * slope.Y) + intercept.Y;
return out;
}
line_t find_line_endpoints(zarray_t* loc_arr, loc_t* p1, loc_t* p2)
{
double largest_dist = INT_MIN;
loc_t largest_loc = {-1, -1};
double smallest_dist = INT_MAX;
loc_t smallest_loc = {-1, -1};
double slope = get_slope(*p1, *p2, 0, 0, NULL);
// find point on line to compare every node to, simple solution = set x = im->width/2
loc_t* Q = p1;
// get line unit vector, y=mx+b --> vector = (1,m)
double mag = sqrt(1 + slope*slope);
vec_t u = {1/mag, slope/mag};
// assert(sqrt((u.x*u.x) + (u.y*u.y)) == 1); // length of u should be 1
// printf("\nunit vec:(%lf(%lf), %lf) Point(%d, %d)\n", u.x, 1/mag, u.y, Q.x, Q.y);
for(int i = 0; i < zarray_size(loc_arr); i++)
{
loc_t* tmp;
zarray_get(loc_arr, i, &tmp);
// find the projection of point onto line
vec_t QP = {Q->x - tmp->x, Q->y - tmp->y};
double dist = u.x * QP.x + u.y * QP.y;
// printf("dist:%lf small:%lf larg:%lf QP:(%lf, %lf) loc:(%d, %d) \n",
// dist, smallest_dist, largest_dist, QP.x, QP.y, node->loc.x, node->loc.y);
if(dist < smallest_dist) {
smallest_dist = dist;
smallest_loc.x = tmp->x;
smallest_loc.y = tmp->y;
}
if(dist > largest_dist) {
largest_dist = dist;
largest_loc.x = tmp->x;
largest_loc.y = tmp->y;
}
}
line_t ret = { .end = largest_loc,
.start = smallest_loc,
.nodes = NULL };
return(ret);
}
void latex_endpoint(int x1, int y1, int x2, int y2, int *xout, int *yout, int arrow, int magnet)
{
int dx, dy, sx, sy, ds, dsx, dsy;
dx = x2 - x1;
dy = y2 - y1;
get_slope(dx, dy, &sx, &sy, arrow);
if (abs(sx) >= abs(sy)) {
ds = lcm(sx, magnet * gcd(sx, magnet));
dsx = (2 * abs(dx) / ds + 1) / 2;
dsx = (dx >= 0) ? dsx * ds : -dsx * ds;
*xout = x1 + dsx;
*yout = y1 + dsx * sy / sx;
} else {
ds = lcm(sy, magnet * gcd(sy, magnet));
dsy = (2 * abs(dy) / ds + 1) / 2;
dsy = (dy >= 0) ? dsy * ds : -dsy * ds;
*yout = y1 + dsy;
*xout = x1 + dsy * sx / sy;
}
}
// *****************LIC_6************************
void LIC_6()
{
CMV[6] = FALSE;
if(NUMPOINTS <3) //return if NUMPOINTS <3
return;
int i,j;
double m,c,d;
for(i=0;(i+PARAMETERS.N_PTS-1< NUMPOINTS);i++)
{
for(j=i+1;j<i+PARAMETERS.N_PTS-1;j++)
{
// if two points are coincident
if((DOUBLECOMPARE(X[i],X[i+PARAMETERS.N_PTS-1])==EQ) &&
(DOUBLECOMPARE(Y[i],Y[i+PARAMETERS.N_PTS-1])==EQ))
{
d = get_distance(X[i],X[j],Y[i],Y[j]);
if (DOUBLECOMPARE(d,PARAMETERS.DIST)==GT)
{
CMV[6] = TRUE;
return;
}
}//closes if statement which tests whether two points are coincident.
else
{
m = get_slope(X[i],X[i+PARAMETERS.N_PTS-1],Y[i],Y[i+PARAMETERS.N_PTS-1]);
c = Y[i]-m*X[i];
d = pt_line_distance(m,c,X[j],Y[j]);
if (DOUBLECOMPARE(d,PARAMETERS.DIST)==GT)
{
CMV[6] = TRUE;
return;
}
}//closes else
}//closes inner for loop j
}//closes outer for loop i
}//end of LIC_6()
int do_astar(void)
{
int r, c, r_nbr, c_nbr, ct_dir;
GW_LARGE_INT first_cum, count;
int nextdr[8] = { 1, -1, 0, 0, -1, 1, 1, -1 };
int nextdc[8] = { 0, 0, -1, 1, 1, -1, 1, -1 };
CELL ele_val, ele_up, ele_nbr[8];
WAT_ALT wa;
ASP_FLAG af;
char is_in_list, is_worked;
HEAP_PNT heap_p;
/* sides
* |7|1|4|
* |2| |3|
* |5|0|6|
*/
int nbr_ew[8] = { 0, 1, 2, 3, 1, 0, 0, 1 };
int nbr_ns[8] = { 0, 1, 2, 3, 3, 2, 3, 2 };
double dx, dy, dist_to_nbr[8], ew_res, ns_res;
double slope[8];
struct Cell_head window;
int skip_diag;
count = 0;
first_cum = n_points;
G_message(_("A* Search..."));
Rast_get_window(&window);
for (ct_dir = 0; ct_dir < sides; ct_dir++) {
/* get r, c (r_nbr, c_nbr) for neighbours */
r_nbr = nextdr[ct_dir];
c_nbr = nextdc[ct_dir];
/* account for rare cases when ns_res != ew_res */
dy = abs(r_nbr) * window.ns_res;
dx = abs(c_nbr) * window.ew_res;
if (ct_dir < 4)
dist_to_nbr[ct_dir] = dx + dy;
else
dist_to_nbr[ct_dir] = sqrt(dx * dx + dy * dy);
}
ew_res = window.ew_res;
ns_res = window.ns_res;
while (heap_size > 0) {
G_percent(count++, n_points, 1);
if (count > n_points)
G_fatal_error(_("%lld surplus points"),
heap_size);
if (heap_size > n_points)
G_fatal_error
(_("Too many points in heap %lld, should be %lld"),
heap_size, n_points);
heap_p = heap_drop();
r = heap_p.pnt.r;
c = heap_p.pnt.c;
ele_val = heap_p.ele;
for (ct_dir = 0; ct_dir < sides; ct_dir++) {
/* get r, c (r_nbr, c_nbr) for neighbours */
r_nbr = r + nextdr[ct_dir];
c_nbr = c + nextdc[ct_dir];
slope[ct_dir] = ele_nbr[ct_dir] = 0;
skip_diag = 0;
/* check that neighbour is within region */
if (r_nbr < 0 || r_nbr >= nrows || c_nbr < 0 || c_nbr >= ncols)
continue;
seg_get(&aspflag, (char *)&af, r_nbr, c_nbr);
is_in_list = FLAG_GET(af.flag, INLISTFLAG);
is_worked = FLAG_GET(af.flag, WORKEDFLAG);
if (!is_worked) {
seg_get(&watalt, (char *)&wa, r_nbr, c_nbr);
ele_nbr[ct_dir] = wa.ele;
slope[ct_dir] = get_slope(ele_val, ele_nbr[ct_dir],
dist_to_nbr[ct_dir]);
}
/* avoid diagonal flow direction bias */
if (!is_in_list) {
if (ct_dir > 3 && slope[ct_dir] > 0) {
if (slope[nbr_ew[ct_dir]] > 0) {
/* slope to ew nbr > slope to center */
if (slope[ct_dir] <
get_slope(ele_nbr[nbr_ew[ct_dir]],
ele_nbr[ct_dir], ew_res))
skip_diag = 1;
}
if (!skip_diag && slope[nbr_ns[ct_dir]] > 0) {
/* slope to ns nbr > slope to center */
if (slope[ct_dir] <
get_slope(ele_nbr[nbr_ns[ct_dir]],
ele_nbr[ct_dir], ns_res))
skip_diag = 1;
}
}
}
if (!skip_diag) {
if (is_in_list == 0) {
ele_up = ele_nbr[ct_dir];
af.asp = drain[r_nbr - r + 1][c_nbr - c + 1];
heap_add(r_nbr, c_nbr, ele_up);
FLAG_SET(af.flag, INLISTFLAG);
seg_put(&aspflag, (char *)&af, r_nbr, c_nbr);
}
else if (is_in_list && is_worked == 0) {
if (FLAG_GET(af.flag, EDGEFLAG)) {
/* neighbour is edge in list, not yet worked */
if (af.asp < 0) {
/* adjust flow direction for edge cell */
af.asp = drain[r_nbr - r + 1][c_nbr - c + 1];
seg_put(&aspflag, (char *)&af, r_nbr, c_nbr);
}
}
else if (FLAG_GET(af.flag, DEPRFLAG)) {
G_debug(3, "real depression");
/* neighbour is inside real depression, not yet worked */
if (af.asp == 0 && ele_val <= ele_nbr[ct_dir]) {
af.asp = drain[r_nbr - r + 1][c_nbr - c + 1];
FLAG_UNSET(af.flag, DEPRFLAG);
seg_put(&aspflag, (char *)&af, r_nbr, c_nbr);
}
}
}
}
} /* end neighbours */
/* add astar points to sorted list for flow accumulation and stream extraction */
first_cum--;
seg_put(&astar_pts, (char *)&heap_p.pnt, 0, first_cum);
seg_get(&aspflag, (char *)&af, r, c);
FLAG_SET(af.flag, WORKEDFLAG);
seg_put(&aspflag, (char *)&af, r, c);
} /* end A* search */
G_percent(n_points, n_points, 1); /* finish it */
return 1;
}
// Given three points, decides if they can be contained in a circle of radius.
// Returns true if they cannot be, false if they can.
boolean cannot_be_contained_in_circle(double x1, double y1, double x2,
double y2, double x3, double y3, double radius)
{
double l12 = get_distance(x1,x2,y1,y2);
double l13 = get_distance(x1,x3,y1,y3);
double l23 = get_distance(x2,x3,y2,y3);
//To include a case where all the three points are the same.
if((DOUBLECOMPARE(x1,x2)==EQ)&&(DOUBLECOMPARE(x2,x3)==EQ)&&
(DOUBLECOMPARE(y1,y2)==EQ)&&(DOUBLECOMPARE(y2,y3)==EQ))
{
return FALSE;
}
// Find the angle where these two lines intersect, if greater than 90
// then the line made by p1 and p3 is the diameter of the circle
double theta1 = get_angle(l12,l13,l23);
if (DOUBLECOMPARE(theta1,PI/2)==GT||DOUBLECOMPARE(theta1,PI/2)==EQ) {
// If this is greater than RADIUS1 then set the CMV and return
if (DOUBLECOMPARE(l23/2,radius)==GT) {
return TRUE;
}
else
return FALSE;
}
// If the angle is <= 90, then check one more angle
double theta2 = get_angle(l12,l23,l13);
// If theta1 and theta2 are zero, then we just have a line, not a triangle
if (DOUBLECOMPARE(theta1,0)==EQ&&DOUBLECOMPARE(theta2,0)==EQ) {
// Find a side that is non-zero
if (DOUBLECOMPARE(l12,0)!=EQ) {
if (DOUBLECOMPARE(l12/2,radius)==GT)
return TRUE;
else
return FALSE;
}
// We should only have to test one more side
else {
if (DOUBLECOMPARE(l13/2,radius)==GT)
return TRUE;
else
return FALSE;
}
}
if (DOUBLECOMPARE(theta2,PI/2)==GT||DOUBLECOMPARE(theta2,PI/2)==EQ) {
// If this is greater than RADIUS1 then set the CMV and return
if (DOUBLECOMPARE(l13/2,radius)==GT) {
return TRUE;
}
else
return FALSE;
}
// Check the last angle
if (DOUBLECOMPARE(theta1+theta2,PI/2)==LT||
DOUBLECOMPARE(theta1+theta2,PI/2)==EQ) {
if (DOUBLECOMPARE(l12/2,radius)==GT) {
return TRUE;
}
else
return FALSE;
}
// Now calculate the center of the circle
double m_a = get_slope(x1,x2,y1,y2);
double m_b = get_slope(x2,x3,y2,y3);
while (DOUBLECOMPARE(m_a,0)==EQ||isnan(m_a)||isnan(m_b)) {
double temp_x = x1;
double temp_y = y1;
x1 = x2;
y1 = y2;
x2 = x3;
y2 = y3;
x3 = temp_x;
y3 = temp_y;
m_a = get_slope(x1,x2,y1,y2);
m_b = get_slope(x2,x3,y2,y3);
}
double center_x = (m_a*m_b*(y1-y3)+m_b*(x1+x2)-m_a*(x2+x3))
/(2*(m_b-m_a));
double center_y = -1/m_a*(center_x-(x1+x2)/2)+(y1+y2)/2;
// All three points lie on the circle, so calculate the distance from
// here out
double radius1 = get_distance(center_x,x1,center_y,y1);
if (DOUBLECOMPARE(radius1,radius)==GT) {
return TRUE;
}
else
return FALSE;
}
/* Benchmark and return linear regression slope in nanoseconds per byte. */
double
do_slope_benchmark (struct bench_obj *obj)
{
unsigned int num_measurements;
double *measurements = NULL;
double *measurement_raw = NULL;
double slope, overhead;
unsigned int loop_iterations, midx, i;
unsigned char *real_buffer = NULL;
unsigned char *buffer;
size_t cur_bufsize;
int err;
err = obj->ops->initialize (obj);
if (err < 0)
return -1;
num_measurements = get_num_measurements (obj);
measurements = calloc (num_measurements, sizeof (*measurements));
if (!measurements)
goto err_free;
measurement_raw =
calloc (obj->num_measure_repetitions, sizeof (*measurement_raw));
if (!measurement_raw)
goto err_free;
if (num_measurements < 1 || obj->num_measure_repetitions < 1 ||
obj->max_bufsize < 1 || obj->min_bufsize > obj->max_bufsize)
goto err_free;
real_buffer = malloc (obj->max_bufsize + 128);
if (!real_buffer)
goto err_free;
/* Get aligned buffer */
buffer = real_buffer;
buffer += 128 - ((real_buffer - (unsigned char *) 0) & (128 - 1));
for (i = 0; i < obj->max_bufsize; i++)
buffer[i] = 0x55 ^ (-i);
/* Adjust number of loop iterations up to timer accuracy. */
loop_iterations = adjust_loop_iterations_to_timer_accuracy (obj, buffer,
measurement_raw);
/* Perform measurements */
for (midx = 0, cur_bufsize = obj->min_bufsize;
cur_bufsize <= obj->max_bufsize; cur_bufsize += obj->step_size, midx++)
{
measurements[midx] =
do_bench_obj_measurement (obj, buffer, cur_bufsize, measurement_raw,
loop_iterations);
measurements[midx] /= loop_iterations;
}
assert (midx == num_measurements);
slope =
get_slope (&get_bench_obj_point_x, obj, measurements, num_measurements,
&overhead);
free (measurement_raw);
free (measurements);
free (real_buffer);
obj->ops->finalize (obj);
return slope;
err_free:
if (measurement_raw)
free (measurement_raw);
if (measurements)
free (measurements);
if (real_buffer)
free (real_buffer);
obj->ops->finalize (obj);
return -1;
}
/***********************************主函数**************************************/
void main(void)
{
for(i = 0; i < 110; i++) //装载摄像头边线初值
{
slope.left_initial_value[i] = left_initial[i];
slope.right_initial_value[i] = right_initial[i];
}
camera_init(imgbuff); //初始化摄像头
//配置中断服务函数
set_vector_handler(PORTA_VECTORn , PORTA_IRQHandler); //设置LPTMR的中断服务函数为 PORTA_IRQHandler
set_vector_handler(DMA0_VECTORn , DMA0_IRQHandler); //设置LPTMR的中断服务函数为 PORTA_IRQHandler
set_vector_handler(PORTE_VECTORn ,PORTE_IRQHandler); //设置PORTE的中断服务函数为 PORTE_IRQHandler
enable_irq (PORTE_IRQn); //使能PORTE中断
port_init(PTE2, ALT1 | PULLUP ); //5 蓝牙同时发车
if(!gpio_get (PTE2)) //蓝牙通讯同时发车
{
key_init(KEY_A);
OLED_Init();
while(1)
{
#if ( CAR_NUMBER == 2 )
OLED_P6x8Str(0, 6, "Press K6 to start!");
if(key_check(KEY_A) == KEY_DOWN) //按K3发车
{
uart_putchar(VCAN_PORT, ch);
OLED_P6x8Str(0, 6, " ");
bluetooth = 1;
break;
}
#endif
#if ( CAR_NUMBER == 1 )
OLED_P6x8Str(0, 6, "Start 2B to start!");
while(uart_query (VCAN_PORT) == 0);
OLED_P6x8Str(0, 6, " ");
bluetooth = 1;
break;
#endif
}
}
else
{
bluetooth = 2;
}
mk60int();
init_PID();
while(1)
{
oled_display();
camera_get_img(); //摄像头获取图像
img_extract(img, imgbuff, CAMERA_SIZE); //解压图像
get_slope(img, &slope); //获取斜率
boundary_detection();//扫描图像获取边界
picture_analysis();//获取中线
//SetMotorVoltage(0.2,0.2);
if(!gpio_get (PTE1))
{
vcan_sendimg(imgbuff,CAMERA_SIZE);//摄像头看图像
}
if(!gpio_get (PTE2)) //蓝牙同时发车
{
#if ( CAR_NUMBER == 1 )
if(uart_querychar (VCAN_PORT, &ch) != 0)
{
if(ch == 's')
{
stop_done = 1;
}
}
#endif
#if ( CAR_NUMBER == 2 )
if(stop_done)
{
lptmr_delay_ms(500); //延时确保后车过线
while(1)
{
uart_putchar(VCAN_PORT, ch_stop);
}
}
#endif
}
if(!gpio_get (PTE3)) //速度40
{
vPID.Setpoint = 40;
}
if(!gpio_get (PTE4)) //速度45
{
vPID.Setpoint = 45;
}
if(!gpio_get (PTE5)) //速度50
{
vPID.Setpoint = 50;
}
if(!gpio_get (PTE6)) //速度47
{
vPID.Setpoint = 46;
}
if(!gpio_get (PTE7)) //速度48
{
vPID.Setpoint = 47;
}
if(!gpio_get (PTE8)) //速度49
{
vPID.Setpoint = 48;
}
//ware1[0]=xielv;
//vcan_sendware(ware1, sizeof(xielv));
/*
Display_number(36, 6, ttt);
Display_number(0, 6, sss);
printf("%d,", left_diuxian1);
printf("%d\n",tt-t);
*/
//printf("%d\n",speed1);
//printf("%d,",left_diuxian1);
//printf("%d\n",slope3);
//printf("%d,",left_chazi);
//printf("%d\n",right_chazi);
//printf("%d,",m);
//printf("%d\n",a);
// if(!gpio_get (PTE8))
// {
// for(l=0;l<120;l++)
// {
// printf("%d,",left_bianjie[l]);
// }
// printf("\n");
// }
}
}
