Issues with solve2
fromanza opened this issue · 2 comments
Great code!
Ive managed to get the 4DOF code working (based on your example) with the Waveshare M1 arm but now and then when the chosen target points fall towards the extremes of the arms reach, the solve2 seems to output angles and positions which push the arm out of its range. Almost like the chain 'flicks' to the alternative solution, which is somewhat expected.
Is there a way to prevent this from occurring by means of a chain reset or similar?
The following is the serial debug output with the same target but different solutions and a simple board reset in between the commands:
10:17:50.918 -> Tool Angle: 0.00Base Rot: 2047. Shoulder: 3369. Elbow: 531 Wrist: 3634 Platform: 0
10:17:50.918 -> X0= 0.00 Y0= 0.00 X1= 167.98 Y1= 18.52 X2= 69.56 Y2= 100.36 X3= 99.56 Y3= 100.36
10:17:50.918 -> OVER MAX: 3200
10:17:50.918 -> UNDER MIN: 2000
10:17:50.918 -> OVER MAX: 3300
10:22:57.266 -> ets Jun 8 2016 00:22:57
10:22:57.266 ->
10:22:57.266 -> rst:0x1 (POWERON_RESET),boot:0x13 (SPI_FAST_FLASH_BOOT)
10:22:57.266 -> configsip: 0, SPIWP:0xee
10:22:57.266 -> clk_drv:0x00,q_drv:0x00,d_drv:0x00,cs0_drv:0x00,hd_drv:0x00,wp_drv:0x00
10:22:57.266 -> mode:DIO, clock div:1
10:22:57.266 -> load:0x3fff0018,len:4
10:22:57.266 -> load:0x3fff001c,len:1216
10:22:57.266 -> ho 0 tail 12 room 4
10:22:57.266 -> load:0x40078000,len:10944
10:22:57.266 -> load:0x40080400,len:6388
10:22:57.266 -> entry 0x400806b4
10:22:58.606 -> Connecting to WiFi..
10:22:59.592 -> Connecting to WiFi..
10:23:00.578 -> Connecting to WiFi..
10:23:01.597 -> Connecting to WiFi..
10:23:01.597 -> 192.168.88.190
10:23:01.597 -> Starting serial
10:23:02.583 -> Sending servos to startup positions with 3s delay
10:23:08.515 -> Received New Position
10:23:08.515 -> Can arm achieve position (1=yes 0=no): 1
10:23:08.515 -> Tool Angle: 0.00Base Rot: 2047. Shoulder: 68. Elbow: 3563 Wrist: 1703 Platform: 0
10:23:08.515 -> X0= 0.00 Y0= 0.00 X1= -39.54 Y1= 164.31 X2= 70.84 Y2= 99.50 X3= 100.84 Y3= 99.50
10:23:08.515 -> UNDER MIN: 200
Hi! The chain is not supposed to flip since it is solving based on its previous position. However, if the target is unreachable, it seems that the solver will reset it to the origin position. I don't think this was intended now when I look at the code.
https://github.com/henriksod/Fabrik2DArduino/blob/master/src/FABRIK2D.cpp line 117
Try removing that line. Does it behave better then?
Stale issue message