# PUMA560 Custom HAL (Final No-BS Version)

loadrt hal_gpio outputs=GPIO25,GPIO2,GPIO17,GPIO18,GPIO12,GPIO23,GPIO16,GPIO11,GPIO20,GPIO10,GPIO24,GPIO19,GPIO21,GPIO22,GPIO13 inputs=GPIO26,GPIO4,GPIO27,GPIO6,GPIO3,GPIO5


loadrt stepgen step_type=0,0,0,0,0,0
loadrt pwmgen output_type=1
loadrt not count=1
addf not.0 servo-thread

addf pwmgen.make-pulses servo-thread
addf hal_gpio.read  servo-thread
addf stepgen.make-pulses servo-thread
# addf charge-pump base-thread
addf hal_gpio.write servo-thread
addf stepgen.capture-position servo-thread
#addf motion-command-handler servo-thread
#addf motion-controller servo-thread
addf stepgen.update-freq servo-thread
addf pwmgen.update servo-thread






unlinkp joint.0.home-sw-in
net joint0-home hal_gpio.GPIO5-in => joint.0.home-sw-in

unlinkp joint.1.home-sw-in
net joint1-home hal_gpio.GPIO27-in => joint.1.home-sw-in

unlinkp joint.2.home-sw-in
net joint2-home hal_gpio.GPIO4-in => joint.2.home-sw-in

unlinkp joint.3.home-sw-in
net joint3-home hal_gpio.GPIO3-in-not => joint.3.home-sw-in

unlinkp joint.4.home-sw-in
net joint4-home hal_gpio.GPIO26-in => joint.4.home-sw-in

unlinkp joint.5.home-sw-in
net joint5-home hal_gpio.GPIO6-in => joint.5.home-sw-in




# === JOINT 5 (X) ===
setp stepgen.0.position-scale [JOINT_5]SCALE
setp stepgen.0.steplen 2000
setp stepgen.0.stepspace 2000
setp stepgen.0.dirhold 5000
setp stepgen.0.dirsetup 5000
setp stepgen.0.maxaccel [JOINT_5]STEPGEN_MAXACCEL
net J5:pos-cmd => stepgen.0.position-cmd
net stepgen0-fb stepgen.0.position-fb
net J5:enable => stepgen.0.enable
net J5:step stepgen.0.step => hal_gpio.GPIO12-out
net J5:dir  stepgen.0.dir  => hal_gpio.GPIO13-out
#net J5:dir => not.0.in
#net not0-out not.0.out => hal_gpio.GPIO13-out

# === JOINT 4 (Y) ===
setp stepgen.1.position-scale [JOINT_4]SCALE
setp stepgen.1.steplen 2000
setp stepgen.1.stepspace 2000
setp stepgen.1.dirhold 5000
setp stepgen.1.dirsetup 5000
setp stepgen.1.maxaccel [JOINT_4]STEPGEN_MAXACCEL
net J4:pos-cmd => stepgen.1.position-cmd
net stepgen1-fb stepgen.1.position-fb
net J4:enable => stepgen.1.enable
net J4:step stepgen.1.step => hal_gpio.GPIO16-out
net J4:dir  stepgen.1.dir  => hal_gpio.GPIO17-out

# === JOINT 3 (Z) ===
setp stepgen.2.position-scale [JOINT_3]SCALE
setp stepgen.2.steplen 2000
setp stepgen.2.stepspace 2000
setp stepgen.2.dirhold 5000
setp stepgen.2.dirsetup 5000
setp stepgen.2.maxaccel [JOINT_3]STEPGEN_MAXACCEL
net J3:pos-cmd => stepgen.2.position-cmd
net stepgen2-fb stepgen.2.position-fb
net J3:enable => stepgen.2.enable
net J3:step stepgen.2.step => hal_gpio.GPIO18-out
net J3:dir  stepgen.2.dir  => hal_gpio.GPIO19-out

# === JOINT 2 (A) ===
setp stepgen.3.position-scale [JOINT_2]SCALE
setp stepgen.3.steplen 2000
setp stepgen.3.stepspace 2000
setp stepgen.3.dirhold 5000
setp stepgen.3.dirsetup 5000
setp stepgen.3.maxaccel [JOINT_2]STEPGEN_MAXACCEL
net J2:pos-cmd => stepgen.3.position-cmd
net stepgen3-fb stepgen.3.position-fb
net J2:enable => stepgen.3.enable
net J2:step stepgen.3.step => hal_gpio.GPIO20-out
net J2:dir  stepgen.3.dir  => hal_gpio.GPIO21-out

# === JOINT 1 (B) ===
setp stepgen.4.position-scale [JOINT_1]SCALE
setp stepgen.4.steplen 2000
setp stepgen.4.stepspace 2000
setp stepgen.4.dirhold 5000
setp stepgen.4.dirsetup 5000
setp stepgen.4.maxaccel [JOINT_1]STEPGEN_MAXACCEL
net J1:pos-cmd => stepgen.4.position-cmd
net stepgen4-fb stepgen.4.position-fb
net J1:enable => stepgen.4.enable
net J1:step stepgen.4.step => hal_gpio.GPIO22-out
net J1:dir  stepgen.4.dir  => hal_gpio.GPIO23-out

# === JOINT 0 (C) ===
setp stepgen.5.position-scale [JOINT_0]SCALE
setp stepgen.5.steplen 2000
setp stepgen.5.stepspace 2000
setp stepgen.5.dirhold 5000
setp stepgen.5.dirsetup 5000
setp stepgen.5.maxaccel [JOINT_0]STEPGEN_MAXACCEL
net J0:pos-cmd => stepgen.5.position-cmd
net stepgen5-fb stepgen.5.position-fb
net J0:enable => stepgen.5.enable
net J0:step stepgen.5.step => hal_gpio.GPIO24-out
net J0:dir  stepgen.5.dir  => hal_gpio.GPIO25-out








setp genserkins.A-0       23.42      
setp genserkins.A-1      180         
setp genserkins.A-2     -43.5        
setp genserkins.A-3       0
setp genserkins.A-4       0
setp genserkins.A-5     -45.25      

setp genserkins.ALPHA-0  -1.57079632679   
setp genserkins.ALPHA-1   3.14159265359  
setp genserkins.ALPHA-2   1.57079632679   
setp genserkins.ALPHA-3  -1.57079632679   
setp genserkins.ALPHA-4   1.57079632679   
setp genserkins.ALPHA-5   3.14159265359  

setp genserkins.D-0     110.5       
setp genserkins.D-1       0
setp genserkins.D-2       0
setp genserkins.D-3    -176.35      
setp genserkins.D-4       0
setp genserkins.D-5     -62.8