diff --git a/app/OrbitalCam.py b/app/OrbitalCam.py index a0355b4..0f2cc32 100644 --- a/app/OrbitalCam.py +++ b/app/OrbitalCam.py @@ -37,7 +37,6 @@ def OrbitalController(keyboard, mouse, cam_pos, cam_rot, cam_tgt, dt, width, hei smoothed_dx += (delta_x - smoothed_dx) * 0.1 smoothed_dy += (delta_y - smoothed_dy) * 0.1 - state_modified = False speed = dt_sec * cam_rot_speed cam_rot.x += smoothed_dy * speed cam_rot.y += smoothed_dx * speed diff --git a/app/poppy_api_rest.py b/app/poppy_api_rest.py index 9f11ad7..7a5b889 100644 --- a/app/poppy_api_rest.py +++ b/app/poppy_api_rest.py @@ -2,6 +2,7 @@ import requests import socket from math import pi +from statistics import median import math from OrbitalCam import OrbitalController @@ -39,6 +40,12 @@ def lerp(k, a, b): res_x, res_y = 1920, 1080 +#initialize lists and variables for toggle button (swipe style) +mousexlist = [] +mouseylist = [] +has_switched = False +compliance_mode = False + win = hg.NewWindow("Harfang - Poppy", res_x, res_y, 32)#, hg.WV_Fullscreen) hg.RenderInit(win) hg.RenderReset(res_x, res_y, hg.RF_MSAA8X | hg.RF_FlipAfterRender | hg.RF_FlushAfterRender | hg.RF_MaxAnisotropy) @@ -71,6 +78,12 @@ def lerp(k, a, b): target_tex = hg.LoadTextureFromAssets("Asset_2.png", 0)[0] texture_asset2 = hg.MakeUniformSetTexture("s_texTexture", target_tex, 0) +target_tex = hg.LoadTextureFromAssets("switch-on.png", 0)[0] +texture_on = hg.MakeUniformSetTexture("s_texTexture", target_tex, 0) + +target_tex = hg.LoadTextureFromAssets("switch-off.png", 0)[0] +texture_off = hg.MakeUniformSetTexture("s_texTexture", target_tex, 0) + # load shaders render_state_quad = hg.ComputeRenderState(hg.BM_Alpha, hg.DT_Less, hg.FC_Disabled) @@ -143,8 +156,68 @@ def lerp(k, a, b): hg.ImGuiInit(10, imgui_prg, imgui_img_prg) app_clock = 0 +app_status = "dancing" + +def toggle_button(label, value, x, y): + global has_switched + mat = hg.TransformationMat4(hg.Vec3(x, y, 1), hg.Vec3(0, 0, 0), hg.Vec3(1, 1, 1)) + pos = hg.GetT(mat) + axis_x = hg.GetX(mat) * 56 + axis_y = hg.GetY(mat) * 24 + + toggle_vtx = hg.Vertices(vtx_layout, 4) + toggle_vtx.Begin(0).SetPos(pos - axis_x - axis_y).SetTexCoord0(hg.Vec2(0, 1)).End() + toggle_vtx.Begin(1).SetPos(pos - axis_x + axis_y).SetTexCoord0(hg.Vec2(0, 0)).End() + toggle_vtx.Begin(2).SetPos(pos + axis_x + axis_y).SetTexCoord0(hg.Vec2(1, 0)).End() + toggle_vtx.Begin(3).SetPos(pos + axis_x - axis_y).SetTexCoord0(hg.Vec2(1, 1)).End() + toggle_idx = [0, 3, 2, 0, 2, 1] + hg.DrawTriangles(view_id, toggle_idx, toggle_vtx, shader_for_plane, [], [texture_on if value else texture_off], render_state_quad) + + if mouse.Down(hg.MB_0): + mousexlist.append(mouse.X()) + mouseylist.append(mouse.Y()) + else: + mousexlist.clear() + mouseylist.clear() + has_switched = False + + if len(mousexlist) > 20: + mousexlist.pop(0) + if len(mouseylist) > 20: + mouseylist.pop(0) + + if len(mouseylist) > 0: + mouse_x = median(mousexlist) + mouse_y = median(mouseylist) + if mouse_x > pos.x - axis_x.x and mouse_x < pos.x + axis_x.x and mouse_y > pos.y - axis_y.y and mouse_y < pos.y + axis_y.y and not has_switched: + value = True if not value else False + has_switched = True + mousexlist.clear() + mouseylist.clear() + + mat = hg.TranslationMat4(hg.Vec3(pos.x + axis_x.x + 10, y - 10, 1)) + hg.SetS(mat, hg.Vec3(1, -1, 1)) + hg.DrawText(view_id, + font, + label, shader_font, "u_tex", 0, + mat, hg.Vec3(0, 0, 0), hg.DTHA_Left, hg.DTVA_Top, + [font_color_white], [], text_render_state) + return value + +buttonlist = [[100, res_y - 80]] + +def is_switching(): + for i in buttonlist: + mat = hg.TransformationMat4(hg.Vec3(i[0], i[1], 1), hg.Vec3(0, 0, 0), hg.Vec3(1, 1, 1)) + pos = hg.GetT(mat) + axis_x = hg.GetX(mat) * 56 + axis_y = hg.GetY(mat) * 24 + if (mouse.X() > pos.x - axis_x.x - 10 and mouse.X() < pos.x + axis_x.x + 10 and mouse.Y() > pos.y - axis_y.y - 10 and mouse.Y() < pos.y + axis_y.y + 10 and mouse.Down(hg.MB_0)): # check if mouse is clicked and is within the button area + 10px on every side + return True + return False + def get_v_from_dancing(id_robot): elapsed_time = app_clock *0.5 _amp = 1 @@ -193,7 +266,7 @@ def get_v_from_dancing(id_robot): render_was_reset, res_x, res_y = hg.RenderResetToWindow(win, res_x, res_y, hg.RF_VSync | hg.RF_MSAA4X | hg.RF_MaxAnisotropy) res_y = max(res_y, 16) - if hg.ImGuiIsWindowFocused(): + if hg.ImGuiIsWindowFocused() and not is_switching(): world, cam_rot, cam_tgt, cam_pos = OrbitalController(keyboard, mouse, cam_pos, cam_rot, cam_tgt, dt, res_x, res_y) cam.GetTransform().SetWorld(world) @@ -209,11 +282,34 @@ def get_v_from_dancing(id_robot): url_set_motor_pos = url + "/motors/set/goto/" send_dt = 1/10 # send information to the poppy every send_dt + # get values from the real robot + if compliance_mode: + timer_requests_not_overload += hg.time_to_sec_f(dt) + if timer_requests_not_overload > send_dt: + timer_requests_not_overload = 0 + + if url != "": + try: + r = requests.get(url + "/motors/get/positions").text + for id, m in enumerate(hg_motors): + hg_m = hg_motors[id] + hg_m["v"] = float(r.split(';')[id]) + if id==5: hg_m["v"] = -float(r.split(';')[id]) # send negative value for claw motor angle () + + except: + print("Robot not connected " + url) + # set 3D mesh with the current motor pos for id, m in enumerate(hg_motors): - v = get_v_from_dancing(id) hg_m = hg_motors[id] + # check if we are getting value from the real root + if compliance_mode: + v = hg_m["v"] + else: # sending value to the real robot + if app_status == "dancing": + v = get_v_from_dancing(id) + # led led_actual_color = "off" if led_current_motor_ping_pong == id: @@ -221,11 +317,11 @@ def get_v_from_dancing(id_robot): # send the next position to the robot if id == 5: - url_set_motor_pos_registers += f"m{id + 1}:compliant:False;m{id + 1}:led:{led_actual_color};" - url_set_motor_pos += f"m{id+1}:{-v}:{send_dt};" + url_set_motor_pos_registers += f"m{id + 1}:compliant:{'True' if compliance_mode else 'False'};m{id + 1}:led:{led_actual_color};" + url_set_motor_pos += f"m{id + 1}:{-v}:{send_dt};" else: - url_set_motor_pos_registers += f"m{id + 1}:compliant:False;m{id + 1}:led:{led_actual_color};" - url_set_motor_pos += f"m{id+1}:{v}:{send_dt};" + url_set_motor_pos_registers += f"m{id + 1}:compliant:{'True' if compliance_mode else 'False'};m{id + 1}:led:{led_actual_color};" + url_set_motor_pos += f"m{id + 1}:{v}:{send_dt};" hg_m["acc"] = lerp(0.1, hg_m["acc"], clamp((hg_m["v"] - v) / (hg.time_to_sec_f(dt)**2), -9999, 9999)) @@ -241,16 +337,18 @@ def get_v_from_dancing(id_robot): hg_m["n"].GetTransform().SetRot(rot) - # send value - timer_requests_not_overload += hg.time_to_sec_f(dt) - if timer_requests_not_overload > send_dt: - timer_requests_not_overload = 0 + # check if we are getting value from the real root + if not compliance_mode: + # send value + timer_requests_not_overload += hg.time_to_sec_f(dt) + if timer_requests_not_overload > send_dt: + timer_requests_not_overload = 0 - if url != "": - try: - requests.get(url_set_motor_pos[:-1]) - except: - print("Robot not connected "+url) + if url != "": + try: + requests.get(url_set_motor_pos[:-1]) + except: + print("Robot not connected "+url) # led ping pong led_current_motor_timer_ping_pong += hg.time_to_sec_f(dt) @@ -391,8 +489,11 @@ def get_v_from_dancing(id_robot): mat, hg.Vec3(0, 0, 0), hg.DTHA_Left, hg.DTVA_Top, [font_color], [], text_render_state) + compliance_mode = toggle_button("Compliance Mode ON" if compliance_mode else "Compliance Mode OFF", compliance_mode, 100, res_y - 80) + view_id += 1 + current_frame = hg.Frame() hg.UpdateWindow(win) diff --git a/resources/switch-off.png b/resources/switch-off.png new file mode 100644 index 0000000..dcb0eb3 Binary files /dev/null and b/resources/switch-off.png differ diff --git a/resources/switch-on.png b/resources/switch-on.png new file mode 100644 index 0000000..4dce760 Binary files /dev/null and b/resources/switch-on.png differ