
/* SIMPLE       to read ISA card                                      */
/* Reads ISA card, writes load data in the units programmed in sensor */
/* to the screen continuously until ESC key is hit                    */

#include <conio.h>
#include <stdio.h>
#include <i86.h>
#include "jr3.h"


#define esc 27            /* escape key                               */
#define fulls_add 0x0080  /* start of full scale data                 */
#define f0add 0x0090      /* start of filter 0 (unfiltered) data      */
#define f1add 0x0098      /* start of filter 1 (500 Hz) data          */
#define f2add 0x00A0      /* start of filter 2 (125 Hz) data          */
#define f3add 0x00A8      /* start of filter 3 (31.25 Hz) data        */
#define f4add 0x00B0      /* start of filter 4 (7.81 Hz) data         */
#define f5add 0x00B8      /* start of filter 5 (1.95 Hz) data         */
#define f6add 0x00C0      /* start of filter 6 (0.49 Hz) data         */
#define fadd_for_rate 0x00e2 /* the force address to use for rate     */
#define rateadd 0x00c8    /* start of rate data                       */
#define rate_divisor 0x00e3  /* how often the rate is being calculated*/
#define error_count  0x00ee  /* explanation is in the .h file         */
#define warning_bits_add 0x00f0
#define error_bits_add   0x00f1

#define board1 0x0200     /* address of isa board # 1                 */

#define comm0 0x00E7      /* command word 0 location                  */
#define cpright 0x0040    /* copyright location                       */
#define ver 0x00f5        /* DSP version number location              */

void Force_sensor_data:: init()
{ printf("\n test \n");
}

force_array Force_sensor_data:: getForces(int filteredNum)
{
  int axload;
  force_array filtered_forces;
  int force_address;
  int i;
  
  force_address = f0add + 0x0008*filteredNum ;

		for (i = 0; i <= 5; i++)            
		{                                   
			axload = getdata(board1, force_address+i);    
			filtered_forces.axis[i] = full_scale.axis[i] * ((float)axload/16384);  
		}
  return filtered_forces;
}


force_array Force_sensor_data:: getDerivativeForces(int filteredNum)
{
  int axrate;
  force_array filtered_rate;
  int rate_address;
  int i;

  rate_address = f0add + 0x0008*filteredNum ;
  
  outpw(board1,fadd_for_rate );     /*we want to write to fadd_for_rate*/
  delay1();                 /*delay for bus*/
  outpw(board1+2, rate_address);    /*write rate_address*/


		for (i = 0; i <= 5; i++)            
		{                                   
			axrate = getdata(board1, rateadd+i);    
			filtered_rate.axis[i] = full_scale.axis[i] * ((float)axrate/16384);  
		}
  return filtered_rate;

}


void Force_sensor_data:: delay1(void)      /* Many ISA bus computers do not respond properly   */
{                      /* to repeated sequential I/O port reads and writes.*/
  int j;
  int k=0;
  for (j=0; j<1000; j++) k++;
//asm nop;             /* This inline assembler procedure forces a NOP in  */
}                      /* .exe file to allow correct operation             */
                       /* Some compilers will require optimization to be   */
                       /* turned off, for the NOP to be included           */


void Force_sensor_data::zerooffs()
{
  outpw(board1, comm0);     /*we want to write to command word 0*/
  delay1();                 /*delay for bus*/
  outpw(board1+2, 0x0800);  /*command to zero offsets is $800*/
}

void Force_sensor_data::setdata(int baseadd, int addr, unsigned int data)
{
  outpw(baseadd, addr);     /*we want to write to command word 0*/
  delay1();                 /*delay for bus*/
  outpw(baseadd+2, data);  /*command to zero offsets is $800*/
}

int Force_sensor_data:: getdata (int baseadd, int addr)
{
  unsigned int data;

  outpw(baseadd, addr);     /*write location to read*/
  delay1();                 /*delay for bus*/

  data= (inpw(baseadd+2));  /*read data at addr*/

  if (data > 32767)                 /*inpw returns unsigned integer    */
	return(data - 65536);     /*we have to convert it to integer */
  else
	return(data);
}


void Force_sensor_data:: get_full_scales(void)
{
  int i;
  for (i = 0; i <= 5; i++)
   full_scale.axis[i] = getdata(board1, fulls_add+i);
}

unsigned int Force_sensor_data::getRateDivisor()
{
  return getdata(board1,rate_divisor);
}

void Force_sensor_data::setRateDivisor(unsigned int divisor)
{
  setdata(board1,rate_divisor,divisor);
}

unsigned int Force_sensor_data::getErrorCount()
{
  return getdata(board1,error_count);
}

unsigned int Force_sensor_data::getSaturationWarning()
{
  return getdata(board1,warning_bits_add);
}

unsigned int Force_sensor_data::getErrorMessages()
{
  return getdata(board1,error_bits_add);
}


