int avoid_booster()
{
register unsigned int current_ET; // declare ET value
while (1)
{
current_ET = get_ET();
mav(lego.left.port , 100);
mav(lego.right.port , 300);
msleep(10); // arc until you are at the closest point to the wall (should be parallel)
if (current_ET > get_ET())
ao();
mav(lego.left.port , -100);
mav(lego.right.port , -300);
msleep(10);
break;
}
while (1)
{
mav(lego.left.port , 300);
mav(lego.right.port , 300);
msleep(10); // drive to the line
if (get_middle() < THRESH)
break;
}
}
int t_line_follow()
{
printf("%d , %d , %d\n" , get_left() , get_middle() , get_right());
if (get_left() < THRESH && get_middle() < THRESH && get_right() < THRESH) // 0 , 0 , 0 // spin in place
{
mav(lego.left.port , LOW);
mav(lego.right.port , -LOW);
msleep(10);
return 0;
}
if (get_left() > THRESH && get_middle() < THRESH && get_right() < THRESH) // 1 , 0 , 0 //
{
mav(lego.left.port , LOW);
mav(lego.right.port , HIGH);
msleep(10);
return 0;
}
if (get_left() < THRESH && get_middle() > THRESH && get_right() < THRESH) // 0 , 1 , 0
{
mav(lego.left.port , HIGH);
mav(lego.right.port , HIGH);
msleep(10);
return 0;
}
if (get_left() < THRESH && get_middle() < THRESH && get_right() > THRESH) // 0 , 0 , 1
{
mav(lego.left.port , HIGH);
mav(lego.right.port , LOW);
msleep(10);
return 0;
}
if (get_left() > THRESH && get_middle() > THRESH && get_right() < THRESH) // 1 , 1 , 0
{
mav(lego.left.port , HIGH);
mav(lego.right.port , LOW);
msleep(10);
return 0;
}
if (get_left() < THRESH && get_middle() > THRESH && get_right() > THRESH) // 0 , 1 , 1
{
mav(lego.left.port , HIGH);
mav(lego.right.port , LOW);
msleep(10);
return 0;
}
if (get_left() > THRESH && get_middle() > THRESH && get_right() > THRESH) // 1 , 1 , 1
{
mav(lego.left.port , LOW);
mav(lego.right.port , -LOW);
msleep(10);
return 0;
}
return 0;
}
int main()
{
while (b_button_clicked() == 0);
while (a_button() == 0)
{
t_line_follow();
printf("%d , %d , %d\n" , get_left() , get_middle() , get_right());
}
return 0;
}
size_t Merge_and_CountSplitInv(int *arr, size_t len)
{
size_t invs;
size_t i, j, k;
int *B, *C, *D;
D = calloc(len, sizeof(int));
if (D == NULL)
fprintf(stderr, "Calloc failed\n");
B = arr;
C = &arr[get_middle(len)];
invs = 0;
for (i = j = k = 0; k < len; k++) {
if (i == max_B(len)) {
D[k] = C[j];
j++;
continue;
}
if (j == max_C(len)) {
D[k] = B[i];
i++;
continue;
}
if ((B[i] < C[j])) {
D[k] = B[i];
__inc(&i, max_B(len));
} else if (B[i] > C[j]) {
D[k] = C[j];
__inc(&j, max_C(len));
invs += get_middle(len) - i;
}
}
memcpy(arr, D, len * sizeof(int));
free(D);
return invs;
}
void _quick_sort(int array[], int start, int end)
{
int middle;
if(start >= end)
return;
middle = get_middle(array, start, end);
_quick_sort(array, start, middle -1);
_quick_sort(array, middle + 1, end);
}
Pos Area::adjacent_rect(const Size &otherSize,Dir dir) const {
assert (shape == AR_RECT);
Pos middle = get_middle();
Pos ret;
switch (dir) {
case CO_UP_R_R:
case CO_R:
case CO_DN_R_R:
ret.x = pos.x + size.width;
ret.y = middle.y - (int)(0.5 * otherSize.height);
break;
case CO_DN_R:
ret = pos + size;
break;
case CO_DN_DN_R:
case CO_DN:
case CO_DN_DN_L:
ret.x = middle.x - (int)(0.5 * otherSize.width);
ret.y = pos.y + size.height;
break;
case CO_DN_L:
ret.x = pos.x - otherSize.width;
ret.y = pos.y + size.height;
break;
case CO_DN_L_L:
case CO_L:
case CO_UP_L_L:
ret.x = pos.x - otherSize.width;
ret.y = middle.y - (int)(0.5 * otherSize.height);
break;
case CO_UP_L:
ret = pos - otherSize;
break;
case CO_UP_UP_L:
case CO_UP:
case CO_UP_UP_R:
ret.x = middle.x - (int)(0.5 * otherSize.width);
ret.y = pos.y - otherSize.height;
break;
case CO_UP_R:
ret.x = pos.x + size.width;
ret.y = pos.y - otherSize.height;
break;
default:
assert(0);
break;
}
return ret;
}
int t_line_follow()
{
if (get_left() < THRESH && get_middle() < THRESH && get_right() < THRESH) // 0 , 0 , 0 // spin in place
{
mav(0 , LOW);
mav(2 , -LOW);
msleep(10);
return 0;
}
if (get_left() > THRESH && get_middle() < THRESH && get_right() < THRESH) // 1 , 0 , 0 //
{
mav(0 , LOW);
mav(2 , HIGH);
msleep(10);
return 0;
}
if (get_left() < THRESH && get_middle() > THRESH && get_right() < THRESH) // 0 , 1 , 0
{
mav(0 , HIGH);
mav(2 , HIGH);
msleep(10);
return 0;
}
if (get_left() < THRESH && get_middle() < THRESH && get_right() > THRESH) // 0 , 0 , 1
{
mav(0 , HIGH);
mav(2 , LOW);
msleep(10);
return 0;
}
if (get_left() > THRESH && get_middle() > THRESH && get_right() < THRESH) // 1 , 1 , 0
{
mav(0 , HIGH);
mav(2 , LOW);
msleep(10);
return 0;
}
if (get_left() < THRESH && get_middle() > THRESH && get_right() > THRESH) // 0 , 1 , 1
{
mav(0 , HIGH);
mav(2 , LOW);
msleep(10);
return 0;
}
if (get_left() > THRESH && get_middle() > THRESH && get_right() > THRESH) // 1 , 1 , 1
{
mav(0 , -LOW);
mav(2 , LOW);
msleep(10);
return 0;
}
return 0;
}
int avoid_cubeguy()
{
while (1)
{
mav(lego.left.port , -300);
mav(lego.right.port , -300);
msleep(10);
if (get_left() < THRESH)
break;
}
while (1)
{
t_line_follow();
if (digital(15) == 1)
break;
}
mav(lego.left.port , -300);
mav(lego.right.port , -300);
msleep(2500);
while (1)
{
mav(lego.left.port , 300);
mav(lego.right.port , -300);
if (get_middle() > THRESH && get_left() > THRESH && get_right() > THRESH)
break;
}
set_servo_position(arm_servo , ARM_OUT);
msleep(500);
while (1)
{
mav(lego.left.port , -300);
mav(lego.right.port , 300);
if (get_middle() < THRESH)
break;
}
return 0;
}
size_t Sort_and_Count(int *arr, size_t len)
{
size_t x, y, z;
if (len == 1)
return 0;
/* TODO: Wont work for odd number of elements */
x = Sort_and_Count(arr, max_B(len));
y = Sort_and_Count(&arr[get_middle(len)], max_C(len));
z = Merge_and_CountSplitInv(arr, len);
return x + y + z;
}
void Simulator::mouse_move_event(Mouse_Event const& event)
{
m_camera_controller.mouse_move_event(event);
if (event.buttons.test(Mouse_Event::Button::LEFT) &&
(m_action == Action::MOVE || m_action == Action::ROTATE))
{
if (!m_gizmos.started)
{
m_gizmos.move.set_override_axis_filter(m_gizmos.move.get_axis_filter());
m_gizmos.rotate.set_override_axis_filter(m_gizmos.rotate.get_axis_filter());
m_gizmos.point = m_world->get_uav().get_position();
m_gizmos.local_base = m_world->get_uav().get_rotation();
m_gizmos.local_base.get_as_euler_xyz(m_gizmos.rotate_start.x, m_gizmos.rotate_start.y, m_gizmos.rotate_start.z);
m_world->get_uav().get_body().setAngularVelocity(btVector3(0, 0, 0));
m_world->get_uav().get_body().clearForces();
if (m_action == Action::MOVE)
{
m_world->get_uav().get_body().setAngularFactor(btVector3(0, 0, 0));
}
else if (m_action == Action::ROTATE)
{
m_world->get_uav().get_body().setLinearFactor(btVector3(0, 0, 0));
}
}
if (m_action == Action::MOVE)
{
math::vec3f filter = m_gizmos.move.get_axis_filter();
math::line3f ray = m_context.camera.get_ray_from_screen_coordinates(m_camera_controller.get_pointer_2d());
math::vec3f dir = ray.get_vector_normalized();
math::vec3f delta;
{
math::vec3f normals[2];
if (filter.x > 0.5f)
{
normals[0] = math::rotate(m_gizmos.local_base, math::vec3f(0, 1, 0));
normals[1] = math::rotate(m_gizmos.local_base, math::vec3f(0, 0, 1));
}
else if (filter.y > 0.5f)
{
normals[0] = math::rotate(m_gizmos.local_base, math::vec3f(1, 0, 0));
normals[1] = math::rotate(m_gizmos.local_base, math::vec3f(0, 0, 1));
}
else if (filter.z > 0.5f)
{
normals[0] = math::rotate(m_gizmos.local_base, math::vec3f(1, 0, 0));
normals[1] = math::rotate(m_gizmos.local_base, math::vec3f(0, 1, 0));
}
math::vec3f line_axis = math::rotate(m_gizmos.local_base, filter) * 1000.0f;
math::line3f line(m_gizmos.point - line_axis, m_gizmos.point + line_axis);
math::vec3f point = m_gizmos.point;
float min_len_sq = FLT_MAX;
for (size_t i = 0; i < 2; i++)
{
math::planef plane(m_gizmos.point, normals[i]);
math::vec3f tmp;
if (plane.get_intersection_with_line(ray.start, dir, tmp))
{
math::vec3f p = line.get_closest_point(tmp);
float len_sq = math::distance_sq(tmp,p);
if (len_sq < min_len_sq)
{
min_len_sq = len_sq;
point = p;
}
}
}
delta = point - m_gizmos.point;
m_gizmos.point = point;
}
if (m_gizmos.started)
{
m_gizmos.delta = delta;
}
m_gizmos.started = true;
}
else if (m_action == Action::ROTATE)
{
auto tangent = m_gizmos.rotate.get_tangent();
if (!math::equals(tangent.start, tangent.end))
{
math::vec2f p = tangent.get_closest_point(m_camera_controller.get_pointer_2d());
float sign = math::sgn(math::dot(tangent.get_vector_normalized(), p - tangent.get_middle()));
float dist = 0.005f * sign * math::distance(p, tangent.get_middle());
auto euler = math::vec3f(dist, dist, dist) * m_gizmos.rotate.get_axis_filter();
if (euler != m_gizmos.rotate_start)
{
auto delta = euler - m_gizmos.rotate_start;
if (m_gizmos.started)
{
m_gizmos.delta.set(delta.x, delta.y, delta.z);
}
m_gizmos.started = true;
m_gizmos.rotate_start = euler;
}
}
}
}
}
size_t max_C(const size_t len)
{
return len - get_middle(len);
}
size_t max_B(const size_t len)
{
return get_middle(len);
}
int main()
{
msleep(2500);
set_analog_pullup(ET_s , 0);
extra_buttons_show(1); // show three extra buttons
set_a_button_text("COORDS"); // set the text of various buttons
set_b_button_text("POM SIZE");
set_c_button_text("BOTGUY SIZE");
set_x_button_text("CUBE SIZE");
lego.left.port = 0; // set motor ports
lego.right.port = 2;
camera_open(LOW_RES);
camera_update();
while (a_button() == 0) // press the a button to set the coordinates
{
camera_update();
target.green.x = get_object_center(0 , 0).x; // sets target coordinates (x)
target.green.y = get_object_center(0 , 0).y; // sets target coordinates (y)
printf("(%d , %d)\n" , target.green.x , target.green.y);
msleep(10);
}
while (b_button() == 0)
{
camera_update();
target.green.size = get_object_area(0 , 0);
target.orange.size = get_object_area(1 , 0);
printf("Orange Size = %d" , target.orange.size);
printf(" Green Size = %d\n" , target.green.size);
}
enable_servo(arm_servo);
enable_servo(push_servo);
enable_servo(basket_servo);
set_servo_position(arm_servo , ARM_UP);
set_servo_position(push_servo , P_DOWN);
set_servo_position(basket_servo , B_UP);
printf("(%d , %d)\n" , target.green.x , target.green.y);
while(a_button() == 0)
{
printf("%d , %d , %d\n" , get_left() , get_middle() , get_right());
}
while (1) // line follow until poms are seen
{
blob_update();
t_line_follow();
if (current.orange.size > target.orange.size && current.green.size > target.green.size)
break;
}
blob_update();
get_pom(); // pick up a pom
pom_push(); // push it into the basket
while (1) // turn to next pom
{
blob_update();
mav(lego.left.port , 300);
mav(lego.right.port , -300);
msleep(10);
if (current.green.size > target.green.size)
break;
}
blob_update();
get_pom(); // pick up pom
pom_push(); // push it into the basket
avoid_cubeguy(); // avoid the cube or botguy
avoid_booster();
while (1)
{
blob_update();
t_line_follow();
if (current.orange.size > target.orange.size && current.green.size > target.green.size)
break;
}
blob_update();
pom_push();
while (1)
{
blob_update();
mav(lego.left.port , 300);
mav(lego.right.port , -300);
msleep(10);
if (current.green.size > target.green.size)
break;
}
blob_update();
get_pom();
pom_push();
avoid_cubeguy();
int start_time = seconds();
int t;
while (1)
{
t_line_follow();
if (seconds() < start_time + t)
break;
}
while (1)
{
}
}
