1  // This is a simple differential "kinematics" component for LinuxCNC HAL.
 2  // Copyright 2015-2016 Sebastian Kuzminsky <seb@highlab.com>
 3  //
 4  // This program is free software; you can redistribute it and/or modify
 5  // it under the terms of the GNU General Public License as published by
 6  // the Free Software Foundation; either version 2 of the License, or
 7  // (at your option) any later version.
 8  //
 9  // This program is distributed in the hope that it will be useful,
10  // but WITHOUT ANY WARRANTY; without even the implied warranty of
11  // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
12  // GNU General Public License for more details.
13  //
14  // You should have received a copy of the GNU General Public License
15  // along with this program; if not, write to the Free Software
16  // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
17  
18  component differential "kinematics for a differential transmission";
19  
20  pin in float roll-cmd  "position command for roll (in degrees)";
21  pin in float pitch-cmd "position command for pitch (in degrees)";
22  
23  pin out float roll-fb  "position feedback for roll (in degrees)";
24  pin out float pitch-fb "position feedback for pitch (in degrees)";
25  
26  pin out float motor0-cmd "position command to motor0 (based on roll & pitch inputs)";
27  pin out float motor1-cmd "position command to motor1 (based on roll & pitch inputs)";
28  
29  pin in float motor0-fb "position feedback from motor0";
30  pin in float motor1-fb "position feedback from motor1";
31  
32  function _;
33  license "GPL";
34  
35  ;;
36  
37  FUNCTION(_) {
38      motor0_cmd = roll_cmd + pitch_cmd;
39      motor1_cmd = roll_cmd - pitch_cmd;
40  
41      roll_fb = (motor0_fb + motor1_fb)/2;
42      pitch_fb = (motor0_fb - motor1_fb)/2;
43  }