SHARE
TWEET

Untitled

a guest Sep 19th, 2019 113 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. component x200_vfd "Hitachi x200 modbus driver";
  2. param rw unsigned mbslaveaddr     "Modbus slave address";
  3. pin in float commanded_frequency  "Frequency of vfd";
  4. pin in bit reverse                "1 when reverse 0 when forward";
  5. pin in bit run                    "run the vfd";
  6. pin in bit enable                 "1 to enable the vfd. 0 will remote trip the vfd, thereby disabling it.";
  7. pin out bit is_running            "1 when running";
  8. pin out bit is_at_speed           "1 when running at assigned frequency";
  9. pin out bit is_ready              "1 when vfd is ready to run";
  10. pin out bit is_alarm              "1 when vfd alarm is set";
  11. pin out bit watchdog_out          "Alternates between 1 and 0 after every update cycle. Feed into a watchdog component to ensure vfd driver is communicating with the vfd properly.";
  12. option userspace;
  13. option userinit yes;
  14. license "GPLv2 or greater";
  15. option extra_compile_args "-I/usr/local/include/modbus -std=c99";
  16. option extra_link_args "-L/usr/local/lib -lmodbus";
  17. ;;
  18. /*
  19.   Userspace HAL component to control a Hitatchi X200 series VFD
  20.  
  21.   Written by Curtis Dutton, inspired by vfs11_vfd.c in linuxcnc
  22.  
  23.   Copyright (C) 2012 Curtis Dutton, OK Computers LLC
  24.  
  25.   This program is free software; you can redistribute it and/or
  26.   modify it under the terms of the GNU Lesser General Public
  27.   License as published by the Free Software Foundation, version 2.
  28.  
  29.   This program is distributed in the hope that it will be useful,
  30.   but WITHOUT ANY WARRANTY; without even the implied warranty of
  31.   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  32.   General Public License for more details.
  33.  
  34.   You should have received a copy of the GNU Lesser General Public
  35.   License along with this program; if not, write to the Free Software
  36.   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307  USA.
  37.  
  38.   see 'man x200_vfd' and the X200 section in the Drivers manual.
  39. */
  40. #include<stdio.h>
  41. #include<errno.h>
  42. #include<getopt.h>
  43. #include<stdbool.h>
  44. #include<math.h>
  45. #include<modbus.h>
  46. #include<unistd.h>
  47. #include<ctype.h>
  48.  
  49. typedef struct
  50. {
  51.         uint8_t running;
  52.         uint8_t ready;
  53.         uint8_t direction;
  54.         uint8_t at_speed;
  55.         uint8_t alarm;
  56.         uint16_t frequency;
  57. } x200_status;
  58.  
  59. /*sets the operating frequency of the vfd*/
  60. bool x200_setFrequency(modbus_t* ctx, uint16_t frequency)
  61. {
  62.         return modbus_write_registers(ctx, 0x001, 1, &frequency) > 0;
  63. }
  64.  
  65. /*resets the trip status of the VFD*/
  66. bool x200_reset(modbus_t* ctx)
  67. {
  68.         /*after the reset, the x200 vfd seem to need a second
  69.         before it will reply to more modbus commands*/
  70.         int rc = modbus_write_bit(ctx, 0x003, TRUE);
  71.  
  72.         sleep(1);
  73.  
  74.         return rc > 0;
  75. }
  76.  
  77. bool x200_setDirection(modbus_t* ctx, bool direction)
  78. {
  79.     return modbus_write_bit(ctx, 0x001, direction) > 0;
  80.     //return modbus_write_bit(ctx, 0x001, FALSE) > 0;
  81. }
  82.  
  83. bool x200_trip(modbus_t* ctx)
  84. {
  85.         return modbus_write_bit(ctx, 0x002, TRUE) > 0;
  86. }
  87.  
  88. bool x200_run(modbus_t* ctx, bool runBit)
  89. {
  90.         return modbus_write_bit(ctx, 0x000, runBit) > 0;
  91. }
  92.  
  93. bool x200_getStatus(modbus_t* ctx, x200_status* status)
  94. {
  95.         int rc;
  96.         uint8_t bits[12]={1,1,1,1,1,1,1,1,1,1,1,1};
  97.  uint8_t iit[5];
  98.         uint16_t registers[2];
  99.  
  100.         /*read coils 0x000E thru 0x0018 in one step*/
  101.         rc = modbus_read_bits(ctx, 0x000D, 12, bits);
  102.  
  103.         if(rc < 0)
  104.         {
  105. fprintf(stderr,"Readbits fail: RUN%0d DIR%0d READY%0d %0d %0d %0d ALARM%0d %0d %0d %0d AT_SPEED%0d %0d Freq%0d\n",
  106. bits[0],bits[1],bits[2],bits[3],bits[4],bits[5],bits[6],bits[7],bits[8],bits[9],bits[10],bits[11],registers[1]);
  107.                 return false;
  108.         }
  109.  
  110. sleep(0.2);
  111.  
  112.         /*read the first 2 registers*/
  113.         rc = modbus_read_registers(ctx, 0x000, 2, registers);
  114.  
  115.         if(rc < 0)
  116.         {
  117. fprintf(stderr,"Readreg fail\n");
  118.                 return false;
  119.         }
  120.  
  121. sleep(0.2);
  122.  
  123.         status->running = bits[0];
  124.         status->direction = bits[1];
  125.         status->ready = bits[2];
  126.         status->alarm = bits[6];
  127.         status->at_speed = bits[10];
  128.         status->frequency = registers[1];
  129.  
  130. fprintf(stderr, "Bits: RUN%0d DIR%0d READY%0d %0d %0d %0d ALARM%0d %0d %0d %0d AT_SPEED%0d %0d Freq%0d\n",
  131. bits[0],bits[1],bits[2],bits[3],bits[4],bits[5],bits[6],bits[7],bits[8],bits[9],bits[10],bits[11],registers[1]);
  132.  
  133.         return true;
  134. }
  135.  
  136. void print_modbus_error(struct __comp_state *__comp_inst, const char* msg)
  137. {
  138.     fprintf(stderr,
  139.         "Error: x200_vfd slave(%d): %s - Modbus error (%d) -  %s\n",
  140.         mbslaveaddr,
  141.         msg,
  142.         errno,
  143.         modbus_strerror(errno));
  144. }
  145.  
  146.  
  147. /* modbus connection settings*/
  148. char *device = "/dev/ttyS0";
  149. int baud = 9600;
  150. char parity = 'N';
  151. int data_bits = 8;
  152. int stop_bits = 1;
  153. modbus_t *ctx;
  154.  
  155. void userinit(int argc, char **argv)
  156. {
  157.     int opt_index = 0;
  158.     int c = 0;
  159.  
  160.     static struct option options[] = {
  161.         {"baud",   required_argument, 0, 0 },
  162.         {"parity", required_argument, 0, 0 },
  163.         {"databits", required_argument, 0, 0 },
  164.         {"stopbits", required_argument, 0, 0 },
  165.         {0, 0, 0, 0}
  166.     }; 
  167.  
  168.     while(1) {
  169.         c = getopt_long(argc, argv, "", options, &opt_index);
  170.  
  171.         if(c == -1)
  172.             break;
  173.  
  174.         switch(opt_index) {
  175.             case 0:
  176.                 baud = atoi(optarg);
  177.  
  178.                 if(baud == 0)
  179.                 {
  180.                     fprintf(stderr,
  181.                         "Invalid argument: baud must be a number. Given '%s'\n",
  182.                         optarg);
  183.                     exit(1);
  184.                 }
  185.                 break;
  186.  
  187.             case 1:
  188.                 parity = toupper(optarg[0]);
  189.  
  190.                 if(parity != 'Y' && parity != 'N')
  191.                 {
  192.                     fprintf(stderr,
  193.                         "Invalid argument: parity must be 'y' or 'n'. Given '%s'\n",
  194.                         optarg);
  195.                     exit(1);
  196.                 }
  197.                 break;
  198.  
  199.             case 2:
  200.                 data_bits = atoi(optarg);
  201.  
  202.                 if(data_bits == 0)
  203.                 {
  204.                     fprintf(stderr,
  205.                         "Invalid argument: databits must be a number. Given '%s'\n",
  206.                         optarg);
  207.                     exit(1);
  208.                 }
  209.                 break;
  210.  
  211.             case 3:
  212.                 stop_bits = atoi(optarg);
  213.        
  214.                 if(stop_bits == 0)
  215.                 {
  216.                     fprintf(stderr,
  217.                         "Invalid argument: stopbits must be a number. Given '%s'\n",
  218.                         optarg);
  219.                     exit(1);
  220.                 }
  221.                 break;
  222.  
  223.             default:
  224.                 exit(1);
  225.         }
  226.  
  227.     }
  228.    
  229.     ctx = modbus_new_rtu(device, baud, parity, data_bits, stop_bits);
  230.  
  231.     if (ctx == NULL) {
  232.         fprintf(stderr,
  233.             "ERROR: x200_vfd unable to create libmodbus context. - %s\n",
  234.             modbus_strerror(errno));
  235.         fprintf(stderr, "Check your commandline!\n");
  236.         exit(1);   
  237.     }
  238.  
  239.  
  240.  
  241. // Set response timeout to 1.1 seconds (1 second, 100000 usec)
  242. if (modbus_set_response_timeout(ctx, 1, 100000) == -1) {
  243.     fprintf(
  244.             stderr,
  245.             "ERROR: x200-vfd unable to set modbus resposne timeout. - %s\n",
  246.             modbus_strerror(errno)
  247.         );
  248.         exit(1);
  249. }
  250.  
  251. // Set error recovery
  252. if (modbus_set_error_recovery(ctx, MODBUS_ERROR_RECOVERY_LINK) == -1) {
  253.     fprintf(
  254.             stderr,
  255.             "ERROR: x200-vfd unable to set error recovery. - %s\n",
  256.             modbus_strerror(errno)
  257.         );
  258.         exit(1);
  259. }
  260.  
  261.     if (modbus_connect(ctx)) {
  262.         fprintf(stderr,
  263.             "ERROR: x200_vfd unable to create libmodbus connection. - %s\n",
  264.             modbus_strerror(errno));
  265.         exit(1);
  266.     }
  267.  
  268.  
  269. }
  270.  
  271. void user_mainloop(void) {
  272.     x200_status status;
  273.     uint16_t calculated_frequency;
  274.  
  275.     while(1) {
  276.     FOR_ALL_INSTS() {
  277.         /*
  278.                   until the params are set we just wait a bit
  279.           and then skip to the next instance.
  280.  
  281.           if every instance does not get a slave address,
  282.                   this could cause bad behavior
  283.         */
  284.         if(mbslaveaddr == 0) {
  285.             sleep(1);
  286.             continue;
  287.         }
  288.  
  289.         modbus_set_slave(ctx, mbslaveaddr);
  290.  
  291.         /*
  292.                   for each slave, receive info from the slave,
  293.           update our output pins based upon vfd status,
  294.           then set the vfd according to our input pins
  295.  
  296.           if we hit an error we just re-loop. The watchdog
  297.           pin won't change until we make it all the way through
  298.           the loop.
  299.                 */
  300.  
  301.         if(!x200_getStatus(ctx, &status)) {
  302.             print_modbus_error(__comp_inst, "failed to get status");
  303.             continue;
  304.         }
  305. sleep(0.2);
  306.  
  307.         is_running = status.running;
  308.         is_at_speed = status.at_speed;
  309.         is_ready = status.ready;
  310.         is_alarm = status.alarm;
  311.  
  312.         if(!status.alarm && !enable && !x200_trip(ctx)) {
  313.             print_modbus_error(__comp_inst, "failed to trip");
  314.             continue;
  315.         }
  316.         else if(status.alarm && enable && !x200_reset(ctx)) {
  317.             print_modbus_error(__comp_inst, "failed to reset");
  318.             continue;
  319.         }
  320.         else {
  321.     //      calculated_frequency = (uint16_t)(fabs(commanded_frequency) * 100);
  322.                         calculated_frequency =(uint16_t)(fabs(commanded_frequency) * 10);
  323.             if(calculated_frequency != status.frequency) {
  324.                 fprintf(stderr, "CALC NOT EQ STAT");
  325.  
  326.                if (!x200_setFrequency(ctx, calculated_frequency)) {
  327.                 print_modbus_error(__comp_inst, "failed to set frequency");
  328.                 continue;
  329.             }
  330. sleep(0.2);
  331. }
  332.  
  333.             if(reverse != status.direction && !x200_setDirection(ctx, !reverse)) {
  334.                 print_modbus_error(__comp_inst, "failed to set direction");
  335.                 continue;
  336.             }
  337. sleep(0.2);
  338.  
  339.             if(status.running ^ run && !x200_run(ctx, run)) {
  340.                 print_modbus_error(__comp_inst, "failed to run");
  341.                 continue;
  342.             }
  343. sleep(0.2);
  344.  
  345.             watchdog_out = !watchdog_out;
  346.         }
  347. sleep(0.2);
  348.     }
  349.     }
  350. }
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
 
Top