component x200_vfd "Hitachi x200 modbus driver";
param rw unsigned mbslaveaddr     "Modbus slave address";
pin in float commanded_frequency  "Frequency of vfd";
pin in bit reverse                "1 when reverse 0 when forward";
pin in bit run                    "run the vfd";
pin in bit enable                 "1 to enable the vfd. 0 will remote trip the vfd, thereby disabling it.";
pin out bit is_running            "1 when running";
pin out bit is_at_speed           "1 when running at assigned frequency";
pin out bit is_ready              "1 when vfd is ready to run";
pin out bit is_alarm              "1 when vfd alarm is set";
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.";
option userspace;
option userinit yes;
license "GPLv2 or greater";
option extra_compile_args "-I/usr/local/include/modbus -std=c99";
option extra_link_args "-L/usr/local/lib -lmodbus";
;;
/*
  Userspace HAL component to control a Hitatchi X200 series VFD

  Written by Curtis Dutton, inspired by vfs11_vfd.c in linuxcnc

  Copyright (C) 2012 Curtis Dutton, OK Computers LLC

  This program is free software; you can redistribute it and/or
  modify it under the terms of the GNU Lesser General Public
  License as published by the Free Software Foundation, version 2.

  This program is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  General Public License for more details.

  You should have received a copy of the GNU Lesser General Public
  License along with this program; if not, write to the Free Software
  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307  USA.

  see 'man x200_vfd' and the X200 section in the Drivers manual.
*/
#include<stdio.h>
#include<errno.h>
#include<getopt.h>
#include<stdbool.h>
#include<math.h>
#include<modbus.h>
#include<unistd.h>
#include<ctype.h>

typedef struct
{
        uint8_t running;
        uint8_t ready;
        uint8_t direction;
        uint8_t at_speed;
        uint8_t alarm;
        uint16_t frequency;
} x200_status;

/*sets the operating frequency of the vfd*/
bool x200_setFrequency(modbus_t* ctx, uint16_t frequency)
{
        return modbus_write_registers(ctx, 0x001, 1, &frequency) > 0;
}

/*resets the trip status of the VFD*/
bool x200_reset(modbus_t* ctx)
{
        /*after the reset, the x200 vfd seem to need a second
        before it will reply to more modbus commands*/
        int rc = modbus_write_bit(ctx, 0x003, TRUE);

        sleep(1);

        return rc > 0;
}

bool x200_setDirection(modbus_t* ctx, bool direction)
{
	return modbus_write_bit(ctx, 0x001, direction) > 0;
	//return modbus_write_bit(ctx, 0x001, FALSE) > 0;
}

bool x200_trip(modbus_t* ctx)
{
        return modbus_write_bit(ctx, 0x002, TRUE) > 0;
}

bool x200_run(modbus_t* ctx, bool runBit)
{
        return modbus_write_bit(ctx, 0x000, runBit) > 0;
}

bool x200_getStatus(modbus_t* ctx, x200_status* status)
{
        int rc;
        uint8_t bits[12]={1,1,1,1,1,1,1,1,1,1,1,1};
 uint8_t iit[5];
        uint16_t registers[2];

        /*read coils 0x000E thru 0x0018 in one step*/
        rc = modbus_read_bits(ctx, 0x000D, 12, bits);

        if(rc < 0)
        {
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",
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]);
                return false;
        }

sleep(0.2);

        /*read the first 2 registers*/
        rc = modbus_read_registers(ctx, 0x000, 2, registers);

        if(rc < 0)
        {
fprintf(stderr,"Readreg fail\n");
                return false;
        }

sleep(0.2);

        status->running = bits[0];
        status->direction = bits[1];
        status->ready = bits[2];
        status->alarm = bits[6];
        status->at_speed = bits[10];
        status->frequency = registers[1];

fprintf(stderr, "Bits: RUN%0d DIR%0d READY%0d %0d %0d %0d ALARM%0d %0d %0d %0d AT_SPEED%0d %0d Freq%0d\n",
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]);

        return true;
}

void print_modbus_error(struct __comp_state *__comp_inst, const char* msg)
{
	fprintf(stderr,
		"Error: x200_vfd slave(%d): %s - Modbus error (%d) -  %s\n",
		mbslaveaddr,
		msg,
		errno,
		modbus_strerror(errno));
}


/* modbus connection settings*/
char *device = "/dev/ttyS0";
int baud = 9600;
char parity = 'N';
int data_bits = 8;
int stop_bits = 1;
modbus_t *ctx;

void userinit(int argc, char **argv)
{
	int opt_index = 0;
	int c = 0;

	static struct option options[] = {
		{"baud",   required_argument, 0, 0 },
		{"parity", required_argument, 0, 0 },
		{"databits", required_argument, 0, 0 },
		{"stopbits", required_argument, 0, 0 },
		{0, 0, 0, 0}
	};

	while(1) {
		c = getopt_long(argc, argv, "", options, &opt_index);

		if(c == -1)
			break;

		switch(opt_index) {
			case 0:
				baud = atoi(optarg);

				if(baud == 0)
				{
					fprintf(stderr,
						"Invalid argument: baud must be a number. Given '%s'\n",
						optarg);
					exit(1);
				}
				break;

			case 1:
				parity = toupper(optarg[0]);

				if(parity != 'Y' && parity != 'N')
				{
					fprintf(stderr,
						"Invalid argument: parity must be 'y' or 'n'. Given '%s'\n",
						optarg);
					exit(1);
				}
				break;

			case 2:
				data_bits = atoi(optarg);

				if(data_bits == 0)
				{
					fprintf(stderr,
						"Invalid argument: databits must be a number. Given '%s'\n",
						optarg);
					exit(1);
				}
				break;

			case 3:
				stop_bits = atoi(optarg);

				if(stop_bits == 0)
				{
					fprintf(stderr,
						"Invalid argument: stopbits must be a number. Given '%s'\n",
						optarg);
					exit(1);
				}
				break;

			default:
				exit(1);
		}

	}

	ctx = modbus_new_rtu(device, baud, parity, data_bits, stop_bits);

	if (ctx == NULL) {
		fprintf(stderr,
			"ERROR: x200_vfd unable to create libmodbus context. - %s\n",
			modbus_strerror(errno));
		fprintf(stderr, "Check your commandline!\n");
		exit(1);
	}



// Set response timeout to 1.1 seconds (1 second, 100000 usec)
if (modbus_set_response_timeout(ctx, 1, 100000) == -1) {
	fprintf(
            stderr,
            "ERROR: x200-vfd unable to set modbus resposne timeout. - %s\n",
            modbus_strerror(errno)
        );
        exit(1);
}

// Set error recovery
if (modbus_set_error_recovery(ctx, MODBUS_ERROR_RECOVERY_LINK) == -1) {
	fprintf(
            stderr,
            "ERROR: x200-vfd unable to set error recovery. - %s\n",
            modbus_strerror(errno)
        );
        exit(1);
}

	if (modbus_connect(ctx)) {
		fprintf(stderr,
			"ERROR: x200_vfd unable to create libmodbus connection. - %s\n",
			modbus_strerror(errno));
		exit(1);
	}


}

void user_mainloop(void) {
	x200_status status;
	uint16_t calculated_frequency;

	while(1) {
	FOR_ALL_INSTS() {
		/*
                  until the params are set we just wait a bit
		  and then skip to the next instance.

		  if every instance does not get a slave address,
                  this could cause bad behavior
		*/
		if(mbslaveaddr == 0) {
			sleep(1);
			continue;
		}

		modbus_set_slave(ctx, mbslaveaddr);

		/*
                  for each slave, receive info from the slave,
		  update our output pins based upon vfd status,
		  then set the vfd according to our input pins

		  if we hit an error we just re-loop. The watchdog
		  pin won't change until we make it all the way through
		  the loop.
                */

		if(!x200_getStatus(ctx, &status)) {
			print_modbus_error(__comp_inst, "failed to get status");
			continue;
		}
sleep(0.2);

		is_running = status.running;
		is_at_speed = status.at_speed;
		is_ready = status.ready;
		is_alarm = status.alarm;

		if(!status.alarm && !enable && !x200_trip(ctx)) {
			print_modbus_error(__comp_inst, "failed to trip");
			continue;
		}
		else if(status.alarm && enable && !x200_reset(ctx)) {
			print_modbus_error(__comp_inst, "failed to reset");
			continue;
		}
		else {
	//		calculated_frequency = (uint16_t)(fabs(commanded_frequency) * 100);
                        calculated_frequency =(uint16_t)(fabs(commanded_frequency) * 10);
			if(calculated_frequency != status.frequency) {
				fprintf(stderr, "CALC NOT EQ STAT");

			   if (!x200_setFrequency(ctx, calculated_frequency)) {
				print_modbus_error(__comp_inst, "failed to set frequency");
				continue;
			}
sleep(0.2);
}

			if(reverse != status.direction && !x200_setDirection(ctx, !reverse)) {
				print_modbus_error(__comp_inst, "failed to set direction");
				continue;
			}
sleep(0.2);

			if(status.running ^ run && !x200_run(ctx, run)) {
				print_modbus_error(__comp_inst, "failed to run");
				continue;
			}
sleep(0.2);

			watchdog_out = !watchdog_out;
		}
sleep(0.2);
	}
	}
}