diff --git a/dat/Limbs2.m b/dat/Limbs2.m new file mode 100644 index 0000000..9d4eb88 --- /dev/null +++ b/dat/Limbs2.m @@ -0,0 +1,98 @@ +%% 3D actuation of all joints %% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +END = 4; +GAIN = 1; + +GAIN_HIP = GAIN; +GAIN_KNEE = GAIN; +GAIN_ANKLE = GAIN; + +% setup timeline, query points +t = 0 : 1 : END; +q = 0 : 0.001: END; + +% vars starting with tr_ are "tracks", or arrays of unprocessed positions +% over a period of time + +% TRACKS: Left Leg Joints X, Y, Z +tr_LHipY = [0, 0, 0, 0, 0]; +tr_LHipX = [0, 0, 0, 0, 0]; +tr_LHipZ = [0, 0, 0, 0, 0]; + +tr_LKneeY = [0, 0, 0, 0, 0]; +tr_LKneeX = [0, 0, 0, 0, 0]; +tr_LKneeZ = [0, 0, 0, 0, 0]; + +tr_LAnkleY = [0, 0, 0, 0, 0]; +tr_LAnkleX = [0, 0, 0, 0, 0]; +tr_LAnkleZ = [0, 0, 0, 0, 0]; + +% TRACKS: Right Leg Joints X, Y, Z +tr_RHipY = [0, 0, 0, 0, 0]; +tr_RHipX = [0, 0, 0, 0, 0]; +tr_RHipZ = [0, 0, 0, 0, 0]; + +tr_RKneeY = [0, 0, 0, 0, 0]; +tr_RKneeX = [0, 0, 0, 0, 0]; +tr_RKneeZ = [0, 0, 0, 0, 0]; + +tr_RAnkleY = [0, 0, 0, 0, 0]; +tr_RAnkleX = [0, 0, 0, 0, 0]; +tr_RAnkleZ = [0, 0, 0, 0, 0]; + +% interp's, left leg +it_LHipY = interp1(t, tr_LHipY, q, 'spline')'; +it_LHipZ = interp1(t, tr_LHipZ, q, 'spline')'; +it_LHipX = interp1(t, tr_LHipX, q, 'spline')'; + +it_LKneeY = interp1(t, tr_LKneeY, q, 'spline')'; +it_LKneeZ = interp1(t, tr_LKneeZ, q, 'spline')'; +it_LKneeX = interp1(t, tr_LKneeX, q, 'spline')'; + +it_LAnkleY = interp1(t, tr_LAnkleY, q, 'spline')'; +it_LAnkleZ = interp1(t, tr_LAnkleZ, q, 'spline')'; +it_LAnkleX = interp1(t, tr_LAnkleX, q, 'spline')'; + +% interp's, right leg +it_RHipY = interp1(t, tr_RHipY, q, 'spline')'; +it_RHipZ = interp1(t, tr_RHipZ, q, 'spline')'; +it_RHipX = interp1(t, tr_RHipX, q, 'spline')'; + +it_RKneeY = interp1(t, tr_RKneeY, q, 'spline')'; +it_RKneeZ = interp1(t, tr_RKneeZ, q, 'spline')'; +it_RKneeX = interp1(t, tr_RKneeX, q, 'spline')'; + +it_RAnkleY = interp1(t, tr_RAnkleY, q, 'spline')'; +it_RAnkleZ = interp1(t, tr_RAnkleZ, q, 'spline')'; +it_RAnkleX = interp1(t, tr_RAnkleX, q, 'spline')'; + +%% RETURN VALUES %% + +% Left Leg +ret_LHipY = [q', it_LHipY]; +ret_LHipZ = [q', it_LHipZ]; +ret_LHipX = [q', it_LHipX]; + +ret_LKneeY = [q', it_LKneeY]; +ret_LKneeZ = [q', it_LKneeZ]; +ret_LKneeX = [q', it_LKneeX]; + +ret_LAnkleY = [q', it_LAnkleY]; +ret_LAnkleZ = [q', it_LAnkleZ]; +ret_LAnkleX = [q', it_LAnkleX]; + +%%%%%%%%%%%%%%%%%%%% + +% Right Leg +ret_RHipY = [q', it_RHipY]; +ret_RHipZ = [q', it_RHipZ]; +ret_RHipX = [q', it_RHipX]; + +ret_RKneeY = [q', it_RKneeY]; +ret_RKneeZ = [q', it_RKneeZ]; +ret_RKneeX = [q', it_RKneeX]; + +ret_RAnkleY = [q', it_RAnkleY]; +ret_RAnkleZ = [q', it_RAnkleZ]; +ret_RAnkleX = [q', it_RAnkleX];