Using U-axis/driver[3] in custom kinematics

koaldesigns

The code compiles and can be flashed onto the duet, the main arm functions work, but the wrist joint doesn't move in relation. It could be possible that the links/calls (?) between the .cpp and .h file are broken or I have the math in the wrong place.

dc42

You've added variable 'beta' but I can't see any code that sets its value.

koaldesigns

The variable beta is supposed to be the offset of the wrist joint angle in relation to the x-axis. What should it be defined as? Also, I believe that the wrist joint should still stay in the same orientation to the x-axis even if beta isn't defined unless I'm understanding the code wrong.

koaldesigns

I have added you to the repo editors so you can change whatever you feel would be incorrect.

dc42

I suggest you get rid of 'float beta' in the .h file because it doesn't need to be stored.

In line 167 of the .cpp file replace 'beta' by 'machinePos[3]'.

In line 194 replace 'beta' by '((float)motorPos[3]/stepsPerMm[3])'.

That doesn't handle enforcing wrist angle limits, but may get movement working.

koaldesigns

Okay, I will add those changes, see what happens, and report back the results. Thank you

koaldesigns

@dc42 I have flashed the firmware to the machine. It compiled with no errors as expected, but the wrist joint still does not operate. I can independently move the wrist joint just using G1 H2 commands on the U-Axis (what I have the wrist defined as currently).

Here is my current M669 if that could be the problem: M669 K4 P376 D376 A-137:110 B0:172 C-1:0:0 X0 Y0 W2 E-180:180

I am going to play with the kinematics some more and see if I can get it to work.

koaldesigns

I tried a simplified movements kinematics for the wrist joint which no longer has the wrist modes. It also does not work. I have no idea why the wrist joint doesn't move when the proximal and distal joints do.

Here is the simplified kinematics which should only keep the wrist in line with the x-axis (it doesn't actually do this).

	motorPos[3] = lrintf((-1 * (theta + psi)) * stepsPerMm[3]);

This is my current config.g if there is anything wrong in that:

M669 K4 P376 D376 A-137:110 B0:172 C-1:0:0 X0 Y0 W2 E-180:180
; Printer class SCARA, proximal/distal length, proximal min/max angle in relation to x-axis, distal min/max angle in relation to proximal arm, crosstalk factor (depends on mechanical setup)
M584 X0 Y1 Z2 W3 E4
M208 X-752:752 Y-752:752 Z0:1270 W-360:360	; Set min/max positions for each axis
M201 X400 Y400 Z200 E800			; Accelerations (mm/s^2)
M203 X6000 Y6000 Z400 E4000			; Maximum speeds (mm/min)
M566 X300 Y300 Z20 E800				; Maximum instant speed changes mm/minute

;Motor Settings

M569 P0 S1					; Drive 0 goes forwards
M569 P1 S0					; Drive 1 goes backwards
M569 P2 S1					; Drive 2 goes forwards
M569 P3 S0					; Drive 3 goes backwards
M569 P4 S1					; Drive 4 goes forwards
M350 X16 Y16 Z16 E16:16 W16 I1			; Set 16x microstepping with interpolation
M92 X262.2222 Y262.2222 Z800 W65.55555		; Set axis steps/degree, steps/mm
M906 X1000 Y1000 Z1000 E800 W500 I60		; Set motor currents (mA) and increase idle current to 60%

;Endstops

M574 X1 S1 P"!xstop"   ; X min active low endstop switch
M574 Y1 S1 P"!ystop"   ; Y min active low endstop switch
;M574 Z2 S1 P"zstop"   ; Z max active high endstop switch
;M574 U2 S1 P"e0stop"  ; U max active high endstop switch

koaldesigns

I have stumbled upon something very odd. I modified the scara kinematics and the wrist joint so that:

motorPos[3] = lrintf(psi * stepsPerMm[3]);

Which should have made the wrist joint move in some fashion... And it stayed completely stationary, the holding torque/current of the wrist motor wasn't even present. The odd thing is that I can still move the wrist using G1 H2.

So, I have absolutely no idea why it isn't working unless there is something actually wrong with motorPos[3] being called.

I'm super confused now... And giving up for the day.

dc42

You need to change line 174 too. Replace XYZ_AXES by XYZ_AXES+1. You will need to make a similar change in the other function too, line 199.

koaldesigns

That seems to have solved the issue of the wrist joint not moving. It doesn't seem to want to stay in line with the x-axis though so I am going to mess with the kinematics of the joint to see if I can get it to work properly.

dc42

Line 194 needs to be changed, to do the inverse of the transformation in the previous function according to the value of wristMode.

koaldesigns

That seems to have fixed it, changing between the different wrist modes also works. The only thing that still must be implemented is the wrist axis homing but that can come later.

One slight peculiarity is that the wrist snaps to its position on its first movement but that's because it hasn't been homed so I am going to fix that right now.

tony73

@koaldesigns
Hi! i followed all the dialogue between you and dc42, how does the firmware work now? is it possible to have the .cpp and the .h with the new changes?

koaldesigns

@dc42 I have modified the Github with the modifications you said to make on line 194, could you see if I implemented them how you said?

koaldesigns

@tony73 Currently with the changes, the wrist joint moves and keeps it's position in relation to the x-axis. There is no homing of the wrist yet but the main movement works.

koaldesigns

Would it be possible to just set the beta angle to equal the machine position?

	if (wristMode == (1 or 2))
		beta = machinePos[3] = (((float)motorPos[3]/stepsPerMm[3]) - theta - psi);
	else
		beta = machinePos[3] = ((float)motorPos[3]/stepsPerMm[3])

I also don't know if the inverse for wristMode 1 and 2 would be the same, or if there even is a needed inverse kinematic?