haddingtondynamics.js

//A sample README for a project that focuses on automating a 3D-printed robotic arm using C and JavaScript in a custom Integrated Development Environment (IDE).

//3D-Printed Robotic Arm Automation

//Overview

//This project demonstrates the automation of a 3D-printed robotic arm, controlled through a custom-developed Integrated Development Environment (IDE). The robotic arm is programmed using C for embedded systems control and JavaScript for the high-level control and automation. This solution integrates the movement of the robotic arm with specific tasks such as picking, placing, and rotating objects.

//Features
//	•	C Programming for Embedded Systems: Low-level control for motor movement and sensor feedback.
//	•	JavaScript for High-Level Automation: Logic for automating tasks like object manipulation.
//	•	Custom IDE: An intuitive environment tailored for both C and JavaScript development.
//	•	3D-Printed Hardware: The robotic arm is fabricated with 3D printing for ease of customization and prototyping.

//Requirements

//Hardware:
//	•	3D-printed robotic arm with actuators and sensors (such as servo motors, stepper motors, and distance sensors).
//	•	Microcontroller (e.g., Arduino, ESP32, or Raspberry Pi) for C programming execution.
//	•	Power supply for the robotic arm and microcontroller.

//Software:
//	•	C Compiler (e.g., GCC or a microcontroller-specific compiler).
//	•	Node.js and npm for JavaScript execution.
//	•	Custom IDE for coding, compiling, and uploading programs to the microcontroller.
//	•	Servo Controller (if applicable).

//Libraries/Dependencies:
//	•	wiringPi (or equivalent) for GPIO control (if using Raspberry Pi or similar).
//	•	Johnny-Five JavaScript library for robotics with Node.js (if using a Node.js server).
//	•	Serial Communication: Communication between the IDE and microcontroller for real-time updates.

//Setup Instructions

//1. Install the Custom IDE
//	1.	Clone the repository:

//git clone https://github.com/yourusername/3d-printed-robotic-arm.git
//cd 3d-printed-robotic-arm


//	2.	Follow the installation guide for the custom IDE:
//	•	Windows: Download the installer from the releases page.
//	•	macOS/Linux: Follow the installation instructions in the INSTALL.md file.
//	3.	Launch the IDE:

//./ide --start



//2. Setup the Microcontroller
//	•	Connect the microcontroller (e.g., Arduino) to your computer via USB.
//	•	Open the robot-arm.c file in the IDE, modify pin assignments if necessary (see configuration section below), and compile/upload the code to the microcontroller.

//3. Running the JavaScript Automation
//	•	Ensure that Node.js is installed:

//node -v
//npm -v


//	•	Install necessary dependencies:

//npm install


//	•	Start the automation script:

//node automation.js



//Configuration

//Microcontroller Pin Setup

//The following pin mappings are used for controlling the motors:

//#define MOTOR_PIN_1 3    // Motor 1 (Base Rotation)
//#define MOTOR_PIN_2 5    // Motor 2 (Shoulder)
//#define MOTOR_PIN_3 6    // Motor 3 (Elbow)
//#define MOTOR_PIN_4 9    // Motor 4 (Wrist)
//#define MOTOR_PIN_5 10   // Motor 5 (Gripper)
//#define SENSOR_PIN_1 A0  // Distance Sensor 1
//#define SENSOR_PIN_2 A1  // Distance Sensor 2

//JavaScript Automation Script Example

//The automation.js file controls the robotic arm to perform a simple pick-and-place operation:

const five = require("johnny-five");
const board = new five.Board();

board.on("ready", function() {
  let arm = new five.Robot({
    pins: [3, 5, 6, 9, 10], // Motor pin assignments
    invert: [false, false, false, false, false]
  });

  let gripper = new five.Servo(10);
  let elbow = new five.Servo(6);

  // Example automation: Move arm to position 1
  arm.move([90, 0, 0, 90, 0]);

  // Pick object
  gripper.to(30); // Close gripper
  elbow.to(45);   // Move arm to angle

  // Move to new position
  arm.move([0, 45, 45, 45, 0]);
  gripper.to(90); // Open gripper
});

Source Code Structure

/3d-printed-robotic-arm
|-- /src
|   |-- robot-arm.c            # C code for microcontroller control
|   |-- automation.js          # JavaScript automation script
|-- /docs
|   |-- INSTALL.md             # Installation guide for the IDE
|   |-- CONFIG.md              # Configuration details
|-- /hardware
|   |-- arm-design.stl         # 3D printable STL files for the robotic arm
|   |-- arm-assembly-guide.pdf # Assembly guide
|-- /ide
|   |-- custom-ide-source-code # Source code for the custom IDE

Testing & Troubleshooting
	•	Motor not moving: Check power connections and ensure correct pin assignments.
	•	Automation not working: Make sure the communication between the microcontroller and the IDE is functioning (e.g., check serial connection).
	•	Gripper malfunctioning: Verify the servo calibration and ensure that the gripper.to(angle) command corresponds to the correct position.

Contributing

We welcome contributions! If you would like to contribute to the project, please fork the repository, create a branch, and submit a pull request with your improvements. Be sure to follow the guidelines provided in CONTRIBUTING.md.

License

This project is licensed under the MIT License - see the LICENSE.md file for details.

Feel free to adjust the details based on specific setup and components.
# Dexter
Software and FPGA code and PCB for Dexter the robot


#include <stddef.h>
#include <stdio.h>
#include <unistd.h>
#include <stdlib.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <time.h>
#include <signal.h>
#include <errno.h>
#include <string.h>
#include <sys/mman.h>
#include <pthread.h>
#include <termios.h>
#include <inttypes.h>


#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>






#define INPUT_OFFSET 11

// Motor position index
#define BASE_POSITION 0
#define END_POSITON 1
#define PIVOT_POSITON 2
#define ANGLE_POSITON 3
#define ROT_POSITON 4

#define ACCELERATION_MAXSPEED 5

// DC bias paramaters
#define BASE_SIN_CENTER 6
#define BASE_COS_CENTER 7
#define END_SIN_CENTER 8
#define END_COS_CENTER 9
#define PIVOT_SIN_CENTER 10
#define PIVOT_COS_CENTER 11
#define ANGLE_SIN_CENTER 12
#define ANGLE_COS_CENTER 13
#define ROT_SIN_CENTER 14
#define ROT_COS_CENTER 15

//PID VALUES
#define PID_DELTATNOT 16
#define PID_DELTAT 17
#define PID_D 18
#define PID_I 19
#define PID_P 20
#define PID_ADDRESS 21

//FORCE PARAMATERS
#define BOUNDRY_BASE 22
#define BOUNDRY_END 23
#define BOUNDRY_PIVOT 24

#define BOUNDRY_ANGLE 25
#define BOUNDRY_ROT 26

#define SPEED_FACTORA 27
#define SPEED_FACTORB 28

#define FRICTION_BASE 29
#define FRICTION_END 30
#define FRICTION_PIVOT 31
#define FRICTION_ANGLE 32
#define FRICTION_ROT 33

#define MOVE_TRHESHOLD 34
#define F_FACTOR 35
#define MAX_ERROR 36

#define FORCE_BIAS_BASE 37
#define FORCE_BIAS_END 38
#define FORCE_BIAS_PIVOT 39
#define FORCE_BIAS_ANGLE 40
#define FORCE_BIAS_ROT 41

// control state register
#define COMMAND_REG 42
#define CMD_CAPCAL_BASE 1
#define CMD_CAPCAL_END 2
#define CMD_CAPCAL_PIVOT 4
#define CMD_MOVEEN 8
#define CMD_MOVEGO 16
#define CMD_ENABLE_LOOP 32
#define CMD_CLEAR_LOOP 64
#define CMD_CALIBRATE_RUN 128
#define CMD_RESET_POSITION 256
#define CMD_RESET_FORCE 512
#define CMD_CAPCAL_ANGLE 1024
#define CMD_CAPCAL_ROT 2048
#define CMD_ANGLE_ENABLE 4096
#define CMD_ROT_ENABLE 8196







//DMA 
#define DMA_READ_ADDRESS 48
#define DMA_READ_PARAMS 47
#define DMA_WRITE_ADDRESS 46
#define DMA_WRITE_PARAMS 45
#define DMA_WRITE_DATA 44
#define DMA_CONTROL 43

// DMA control bits breakdown

#define DMA_WRITE_ENQUEUE 1
#define DMA_WRITE_INITIATE 2
#define DMA_READ_DEQUEUE 4
#define DMA_READ_BLOCK 8
#define DMA_RESET_ALL 16

#define REC_PLAY_CMD 49
#define CMD_RESET_RECORD 1
#define CMD_RECORD 2
#define CMD_RESET_PLAY 4
#define CMD_PLAYBACK 8
#define CMD_RESET_PLAY_POSITION 16



#define REC_PLAY_TIMEBASE 50
#define DIFF_FORCE_TIMEBASE 51
#define DIFF_FORCE_BETA 52
#define DIFF_FORCE_MOVE_THRESHOLD 53
#define DIFF_FORCE_MAX_SPEED 54
#define DIFF_FORCE_SPEED_FACTOR_ANGLE 55
#define DIFF_FORCE_SPEED_FACTOR_ROT 56
#define DIFF_FORCE_ANGLE_COMPENSATE 57

#define FINE_ADJUST_BASE 58
#define FINE_ADJUST_END 59
#define FINE_ADJUST_PIVOT 60
#define FINE_ADJUST_ANGLE 61
#define FINE_ADJUST_ROT 62

#define RECORD_LENGTH 63



#define END_EFFECTOR_IO 64
#define SERVO_SETPOINT_A 65
#define SERVO_SETPOINT_B 66

#define BASE_FORCE_DECAY 67
#define END_FORCE_DECAY 68
#define PIVOT_FORCE_DECAY 69
#define ANGLE_FORCE_DECAY 70
#define ROTATE_FORCE_DECAY 71

#define PID_SCHEDULE_INDEX 72

#define GRIPPER_MOTOR_CONTROL 73
#define GRIPPER_MOTOR_ON_WIDTH 75
#define GRIPPER_MOTOR_OFF_WIDTH 74
#define START_SPEED 76
#define ANGLE_END_RATIO 77





// DMA DATA IN

#define DMA_READ_DATA 30 + INPUT_OFFSET





// READ REGISTER DEFINITIONS

// POSITION REPORT
#define BASE_POSITION_AT 0 + INPUT_OFFSET
#define END_POSITION_AT 1 + INPUT_OFFSET
#define PIVOT_POSITION_AT 2 + INPUT_OFFSET
#define ANGLE_POSITION_AT 3 + INPUT_OFFSET
#define ROT_POSITION_AT 4 + INPUT_OFFSET

//TABLE CALCULATED DELTA

#define BASE_POSITION_DELTA 5 + INPUT_OFFSET
#define END_POSITION_DELTA 6 + INPUT_OFFSET
#define PIVOT_POSITION_DELTA 7 + INPUT_OFFSET
#define ANGLE_POSITION_DELTA 8 + INPUT_OFFSET
#define ROT_POSITION_DELTA 9 + INPUT_OFFSET


//PID CALCULATED DELTA

#define BASE_POSITION_PID_DELTA 10 + INPUT_OFFSET
#define END_POSITION_PID_DELTA 11 + INPUT_OFFSET
#define PIVOT_POSITION_PID_DELTA 12 + INPUT_OFFSET
#define ANGLE_POSITION_PID_DELTA 13 + INPUT_OFFSET
#define ROT_POSITION_PID_DELTA 14 + INPUT_OFFSET


// FORCE CALCULATED POSITION MODIFICATION

#define BASE_POSITION_FORCE_DELTA 15 + INPUT_OFFSET
#define END_POSITION_FORCE_DELTA 16 + INPUT_OFFSET
#define PIVOT_POSITION_FORCE_DELTA 17 + INPUT_OFFSET
#define ANGLE_POSITION_FORCE_DELTA 18 + INPUT_OFFSET
#define ROT_POSITION_FORCE_DELTA 19 + INPUT_OFFSET


// RAW ANALOG TO DIGITAL VALUES

#define BASE_SIN 20 + INPUT_OFFSET
#define BASE_COS 21 + INPUT_OFFSET
#define END_SIN 22 + INPUT_OFFSET
#define END_COS 23 + INPUT_OFFSET
#define PIVOT_SIN 24 + INPUT_OFFSET
#define PIVOT_COS 25 + INPUT_OFFSET
#define ANGLE_SIN 26 + INPUT_OFFSET
#define ANGLE_COS 27 + INPUT_OFFSET
#define ROT_SIN 28 + INPUT_OFFSET
#define ROT_COS 29 + INPUT_OFFSET

// RECORD AND PLAYBACK 

#define RECORD_BLOCK_SIZE 31 + INPUT_OFFSET
#define READ_BLOCK_COUNT 32 + INPUT_OFFSET
#define PLAYBACK_BASE_POSITION 33 + INPUT_OFFSET
#define PLAYBACK_END_POSITION 34 + INPUT_OFFSET
#define PLAYBACK_PIVOT_POSITION 35 + INPUT_OFFSET
#define PLAYBACK_ANGLE_POSITION 36 + INPUT_OFFSET
#define PLAYBACK_ROT_POSITION 37 + INPUT_OFFSET


#define END_EFFECTOR_IO_IN 38 + INPUT_OFFSET

#define SENT_BASE_POSITION 39 + INPUT_OFFSET
#define SENT_END_POSITION 40 + INPUT_OFFSET
#define SENT_PIVOT_POSITION 41 + INPUT_OFFSET
#define SENT_ANGLE_POSITION 42 + INPUT_OFFSET
#define SENT_ROT_POSITION 43 + INPUT_OFFSET

#define SLOPE_BASE_POSITION 44 + INPUT_OFFSET
#define SLOPE_END_POSITION 45 + INPUT_OFFSET
#define SLOPE_PIVOT_POSITION 46 + INPUT_OFFSET
#define SLOPE_ANGLE_POSITION 47 + INPUT_OFFSET
#define SLOPE_ROT_POSITION 48 + INPUT_OFFSET
#define CMD_FIFO_STATE 49 + INPUT_OFFSET








int ADLookUp[5] = {BASE_SIN,END_SIN,PIVOT_SIN,ANGLE_SIN,ROT_SIN};

//commands

#define MOVE_CMD 1
#define READ_CMD 2
#define WRITE_CMD 3
#define EXIT_CMD 4
#define SLOWMOVE_CMD 5
#define MOVEALL_CMD 6
#define DMAREAD_CMD 7
#define DMAWRITE_CMD 8
#define CAPTURE_AD_CMD 9
#define FIND_HOME_CMD 10
#define FIND_HOME_REP_CMD 11
#define LOAD_TABLES 12
#define CAPTURE_POINTS_CMD 14
#define FIND_INDEX_CMD 15
#define SLEEP_CMD 16
#define RECORD_MOVEMENT 17
#define REPLAY_MOVEMENT 18
#define MOVEALL_RELATIVE 19
#define SET_PARAM 20
#define SEND_HEARTBEAT 21
#define SET_FORCE_MOVE_POINT 22
#define HEART_BEAT 23
#define PID_FINEMOVE 24


// modes
#define BLOCKING_MOVE 1
#define NON_BLOCKING_MOVE 2
#define DEFAULT_MOVE_TIMEOUT 0
#define TRUE 1
#define FALSE 0


// defaults
#define DEFAULT_PID_SETTING_XYZ 0x3dcCCCCC
//#define DEFAULT_PID_SETTING_XYZ 0x3f000000
//#define DEFAULT_PID_SETTING_XYZ 0x3f800000
//#define DEFAULT_PID_SETTING_PY 0x3f000000
//#define DEFAULT_PID_SETTING_PY 0x3D4CCCCC
//#define DEFAULT_PID_SETTING_PY 0x3dcCCCCC
#define DEFAULT_PID_SETTING_PY 0x3cf5c28f


#define DEF_SPEED_FACTOR_A 30
#define DEF_SPEED_FACTOR_DIFF 8

#define ACCELERATION_MAXSPEED_DEF 250000+(3<<20)

#define BASE_SIN_LOW 0X760
#define BASE_COS_LOW 0X1
#define BASE_SIN_HIGH 0x83A
#define BASE_COS_HIGH 0X40

#define END_SIN_LOW 0Xc01
#define END_COS_LOW 0X0
#define END_SIN_HIGH 0xd5c
#define END_COS_HIGH 0X70

#define PIVOT_SIN_LOW 0X50
#define PIVOT_COS_LOW 0X880
#define PIVOT_SIN_HIGH 0x100
#define PIVOT_COS_HIGH 0X8d1

#define ANGLE_SIN_LOW 0X900
#define ANGLE_COS_LOW 0X1
#define ANGLE_SIN_HIGH 0x9A0
#define ANGLE_COS_HIGH 0X50

#define ROT_SIN_LOW 0XA90
#define ROT_COS_LOW 0X1
#define ROT_SIN_HIGH 0xcc2
#define ROT_COS_HIGH 0X50


#define DEFAULT_ANGLE_BOUNDRY_HI 3000
#define DEFAULT_ANGLE_BOUNDRY_LOW -3000

#define SERVO_LOW_BOUND 1142000
#define SERVO_HI_BOUND 1355000


#define bool int
#define TRUE 1
#define FALSE 0

int XLowBound[5]={BASE_COS_LOW,END_COS_LOW,PIVOT_COS_LOW,ANGLE_COS_LOW,ROT_COS_LOW};
int XHighBound[5]={BASE_COS_HIGH,END_COS_HIGH,PIVOT_COS_HIGH,ANGLE_COS_HIGH,ROT_COS_HIGH};
int YLowBound[5]={BASE_SIN_LOW,END_SIN_LOW,PIVOT_SIN_LOW,ANGLE_SIN_LOW,ROT_SIN_LOW};
int YHighBound[5]={BASE_SIN_HIGH,END_SIN_HIGH,PIVOT_SIN_HIGH,ANGLE_SIN_HIGH,ROT_SIN_HIGH};
int ForcePossition[5]={0,0,0,0,0};
int ForceDestination[5]={0,0,0,0,0};
int ThreadsExit=1;
int StatusReportIndirection[60]={DMA_READ_DATA,DMA_READ_DATA,RECORD_BLOCK_SIZE,END_EFFECTOR_IO_IN,BASE_POSITION_AT,BASE_POSITION_DELTA,
								BASE_POSITION_PID_DELTA,BASE_POSITION_FORCE_DELTA,BASE_SIN,BASE_COS,PLAYBACK_BASE_POSITION,SENT_BASE_POSITION,
								SLOPE_BASE_POSITION,0,PIVOT_POSITION_AT,PIVOT_POSITION_DELTA,PIVOT_POSITION_PID_DELTA,PIVOT_POSITION_FORCE_DELTA,
								PIVOT_SIN,PIVOT_COS,PLAYBACK_PIVOT_POSITION,SENT_PIVOT_POSITION,SLOPE_PIVOT_POSITION,0,END_POSITION_AT,
								END_POSITION_DELTA,END_POSITION_PID_DELTA,END_POSITION_FORCE_DELTA,END_SIN,END_COS,PLAYBACK_END_POSITION,
								SENT_END_POSITION,SLOPE_END_POSITION,0,ANGLE_POSITION_AT,ANGLE_POSITION_DELTA,ANGLE_POSITION_PID_DELTA,
								ANGLE_POSITION_FORCE_DELTA,ANGLE_SIN,ANGLE_COS,PLAYBACK_ANGLE_POSITION,SENT_ANGLE_POSITION,SLOPE_ANGLE_POSITION,
								0,ROT_POSITION_AT,ROT_POSITION_DELTA,ROT_POSITION_PID_DELTA,ROT_POSITION_FORCE_DELTA,ROT_SIN,ROT_COS,
								PLAYBACK_ROT_POSITION,SENT_ROT_POSITION,SLOPE_ROT_POSITION,0};

void *map_addr,*map_addrCt;
volatile unsigned int *mapped;
volatile unsigned int *CalTables;
volatile unsigned int *RecordTable;
int CmdVal=0,maxSpeed=0,startSpeed=0,coupledAcceleration=0,forceMode=0,fa0=0,fa1=0,fa2=0,fa3=0,fa4=0,RunMode=0,ServerMode=3;
int BaseBoundryHigh,BaseBoundryLow,EndBoundryHigh,EndBoundryLow,PivotBoundryHigh,PivotBoundryLow,AngleBoundryHigh=DEFAULT_ANGLE_BOUNDRY_HI,AngleBoundryLow=DEFAULT_ANGLE_BOUNDRY_LOW,RotateBoundryHigh,RotateBoundryLow;
int Cycle = 0;
struct BotPossition {
	int base;
	int end;
	int pivot;
	int angle;
	int rotate;
	int baseForce;
	int endForce;
	int pivotForce;
	int angleForce;
	int rotateForce;
	int Controlled;
	int ForceMoveState;
};
struct BotSetHomePossition {
	int base;
	int end;
	int pivot;
	int angle;
	int rotate;
};
char RemoteRobotAdd[255];
struct MasterSlaveMoveRatio{
	int base;
	int end;
	int pivot;
	int angle;
	int rotate;	
};
struct MasterSlaveMoveDelta{
	int base;
	int end;
	int pivot;
	int angle;
	int rotate;	
};
struct SlaveBotPossiton{
	int base;
	int end;
	int pivot;
	int angle;
	int rotate;	
};
float DiffCorrectionFactor;
int AngleHIBoundry;
int AngleLOWBoundry;
int controlled=0;

int DexError=0;

struct MasterSlaveMoveRatio MSMoveRatio = {1,1,1,1,1};
struct MasterSlaveMoveDelta MSMoveDelta = {0,0,0,0,0};
struct BotSetHomePossition SetHome = {0,0,0,0,0};
struct SlaveBotPossiton SlaveBotPos = {0,0,0,0,0};
pthread_t tid[10];
struct CaptureArgs {
   char *FileName;
   FILE *fp;
};
struct CaptureArgs CptA,CptMove;
#define MAX_PARAMS 26
#define PARAM_LENGTH 20
//char Params[MAX_PARAMS][PARAM_LENGTH+1] = {"MaxSpeed", "Acceleration", "J1Force","J3Force","J2Force","J4Force","J5Force","J1Friction","J3Friction","J2Friction","J4Friction","J5Friction","J1BoundryHigh","J1BoundryLow","J3BoundryHigh","J3BoundryLow","J2BoundryHigh","J2BoundryLow","J4BoundryHigh","J4BoundryLow","J5BoundryHigh","J5BoundryLow","GripperMotor","EERoll","StartSpeed","EndSpeed","End"};


static struct termios old, new;

int PositionAdjust[5]={0,0,0,0,0};

int ForceLimit[5]={4400,4400,4400,800,800};

int MyBotForce[5]={0,0,0,0,0};

float ForceAdjustPossition[5]={-.01,-.01,-.01,-.01,-.01};
int FroceMoveMode=0;

int LastGoal[5]={0,0,0,0,0};


float JointsCal[5];
int SoftBoundries[5];

void printPosition()
{
	int a1,a2,a3,a4,a5;
	a1=getNormalizedInput(BASE_POSITION_AT)+getNormalizedInput(BASE_POSITION_FORCE_DELTA);
	a2=getNormalizedInput(END_POSITION_AT)+getNormalizedInput(END_POSITION_FORCE_DELTA);
	a3=getNormalizedInput(PIVOT_POSITION_AT)+getNormalizedInput(PIVOT_POSITION_FORCE_DELTA);
	a4=getNormalizedInput(ANGLE_POSITION_AT)+getNormalizedInput(ANGLE_POSITION_FORCE_DELTA);
	a5=getNormalizedInput(ROT_POSITION_AT)+getNormalizedInput(ROT_POSITION_FORCE_DELTA);
	//printf(" %d,%d,%d,%d,%d\n",a1,a2,a3,a4,a5);

}

int sign(int i)
{
	if(i<0)
		return -1;
	if(i>0)
		return 1;
	return 0;
}

void RealtimeMonitor(void *arg)
{
	int* ExitState = arg;
	int i,j,ForceDelta,disTime=0;
	while(*ExitState)
	{
		if(FroceMoveMode==1)
		{
	
	
			
			// do force based movement
			for(i=0;i<5;i++)
			{
				ForceDelta=ForcePossition[i]-getNormalizedInput(BASE_POSITION_FORCE_DELTA+i);
				/*if(abs(ForceDelta)>500)
				{
					ForceDelta=(abs(ForceDelta)-500)*sign(ForceDelta);
				}*/
				//ForceDelta=ForceDelta;//+MyBotForce[i];
				if(disTime==90)
				{
					//for(j=0;j<5;j++)
					//printf(" Force %d ",ForceDelta);
				}
				if(abs(ForceDelta)<ForceLimit[i])
				{
					mapped[FORCE_BIAS_BASE+i]=ForceDelta;
				}
				else
				{
					mapped[FORCE_BIAS_BASE+i]=sign(ForceDelta)*ForceLimit[i];
				}
				//ForcePossition[i]=ForcePossition[i]+(int)((float)ForceDelta*ForceAdjustPossition[i]);
				/*if(ForceDestination[i]>ForcePossition[i])
				{
					ForcePossition[i]=ForcePossition[i]+10;
				}
				else
				{
					ForcePossition[i]=ForcePossition[i]-10;
				}*/
			}
		}
		disTime++;
		if(disTime>100)
		{
			//printPosition();
			disTime = 0;
		}

		usleep(1000);
	}
	//printf("\nMonitor Thread Exiting\n");
}
void SetGripperRoll(int Possition)
{
	int ServoSpan=(SERVO_HI_BOUND-SERVO_LOW_BOUND)/360;
	mapped[END_EFFECTOR_IO]=64;

	mapped[SERVO_SETPOINT_B]=(ServoSpan*Possition)+SERVO_LOW_BOUND;
//	mapped[SERVO_SETPOINT_B]=Possition;
	
}
void SetGripperMotor(int state)
{
	mapped[GRIPPER_MOTOR_ON_WIDTH]=12000;
	mapped[GRIPPER_MOTOR_OFF_WIDTH]=200000;
	mapped[GRIPPER_MOTOR_CONTROL]=state;
}
void StartServerSocketDDE(void *arg)
{
	struct timeval tv;
	tv.tv_sec = 30;  /* 30 Secs Timeout */
	tv.tv_usec = 0;  // Not init'ing this can cause strange errors

	int* ExitState = arg;
	int listenfd = 0, connfd = 0,RLength = 0,SLength = 0;
    struct sockaddr_in serv_addr; 

    char sendBuff[256];
    char recBuff[256];
    time_t ticks; 
	bool SocketLive=0;
	//printf("\n Start DDE Socket Server \n");

    listenfd = socket(AF_INET, SOCK_STREAM, 0);
    memset(&serv_addr, '0', sizeof(serv_addr));
    memset(sendBuff, '0', sizeof(sendBuff)); 

    serv_addr.sin_family = AF_INET;
    serv_addr.sin_addr.s_addr = htonl(INADDR_ANY);
    serv_addr.sin_port = htons(50000); 

    bind(listenfd, (struct sockaddr*)&serv_addr, sizeof(serv_addr)); 

    listen(listenfd, 10); 
	
	setsockopt(listenfd, SOL_SOCKET, SO_RCVTIMEO, (char *)&tv,sizeof(struct timeval));
    while(*ExitState)
    {
        // connfd = accept(listenfd, (struct sockaddr*)NULL, NULL);
        while ( (connfd = accept(listenfd, (struct sockaddr*)NULL, NULL)) < 0)
		{
			//printf("error on accept()! %s\n", strerror(errno));
			////printf("\n Timeout no connect \n");
		} 
		//printf("\n new connect \n");
		
		
		//ticks = time(NULL);
		//strcpy(sendBuff,"Dexter Connect Service Connected\n");
		//sn//printf(sendBuff, sizeof(sendBuff), "%.24s\r\n", ctime(&ticks));
		//write(connfd, sendBuff, strlen(sendBuff)); 
		SocketLive=TRUE;
//		while(SocketLive==TRUE)
		{
			while((errno = 0, (RLength = recv(connfd, recBuff, /*sizeof(recBuff)*/128, 0))>0) || 
			errno == EINTR)
			{
				if(RLength>0)
				{
					////printf("Receive size %i ",RLength);
					//output.append(recBuff, RLength);
				}
					
			 

				if(RLength < 0)
				{
					//printf("%s \n",strerror(errno));
					/* handle error - for example throw an exception*/
					SocketLive = FALSE;
				}
				

//			while ( (RLength = recv (connfd,recBuff,sizeof(recBuff),0 )) > 0)
				{
					//recBuff[RLength]=0;
					ProcessServerReceiveDataDDE(recBuff);
				
					ProcessServerSendDataDDE(sendBuff,recBuff);/*==TRUE)*/
						write (connfd,sendBuff,60*4/*sizeof(sendBuff)*/); 
				}
			}
			//printf("error code %s \n",strerror(errno));			
		}
		//printf("\n Socket Read Zero closing socket\n");
        close(connfd);
        //sleep(1);
     }

}
void StartServerSocket(void *arg)
{
	int* ExitState = arg;
	int listenfd = 0, connfd = 0,RLength = 0,SLength = 0;
    struct sockaddr_in serv_addr; 

    char sendBuff[64];
    char recBuff[64];
    time_t ticks; 
	bool SocketLive=0;

    listenfd = socket(AF_INET, SOCK_STREAM, 0);
    memset(&serv_addr, '0', sizeof(serv_addr));
    memset(sendBuff, '0', sizeof(sendBuff)); 

    serv_addr.sin_family = AF_INET;
    serv_addr.sin_addr.s_addr = htonl(INADDR_ANY);
    serv_addr.sin_port = htons(40000); 

    bind(listenfd, (struct sockaddr*)&serv_addr, sizeof(serv_addr)); 

    listen(listenfd, 10); 

    while(*ExitState)
    {
        connfd = accept(listenfd, (struct sockaddr*)NULL, NULL); 
		//printf("\n new connect \n");
		
		//ticks = time(NULL);
		strcpy(sendBuff,"Dexter Connect Service Connected\n");
		//sn//printf(sendBuff, sizeof(sendBuff), "%.24s\r\n", ctime(&ticks));
		write(connfd, sendBuff, strlen(sendBuff)); 
		SocketLive=TRUE;
		
		while(SocketLive==TRUE)
		{
			while ( (RLength = recv (connfd,recBuff,sizeof(recBuff),0 )) > 0)
			{
				//recBuff[RLength]=0;
				ProcessServerReceiveData(recBuff);
				SocketLive = ProcessServerSendData(sendBuff);
				write (connfd,sendBuff,sizeof(sendBuff)); 
			}
		}
		//printf("\n Socket Read Zero closing socket\n");
        close(connfd);
       
    }

}
int MaxForce(int Max,int Val)
{
	if(abs(Max) > abs(Val))
	{
		return Val;
	}
	else
	{
		return abs(Max)*sign(Val);
	}
}
bool ProcessServerReceiveData(char *recBuff)
{
	struct BotPossition MyBot;
	int MxForce=9800;
	float fScale=1;
	memcpy(&MyBot,recBuff,sizeof(MyBot));
	////printf("Server %d %d %d %d %d \n",MyBot.baseForce,MyBot.endForce,MyBot.pivotForce,MyBot.angleForce,MyBot.rotateForce);
	if(MyBot.Controlled!=0)
	{
		/*MyBotForce[0]=MaxForce(MxForce,MyBot.baseForce);
		MyBotForce[1]=MaxForce(MxForce,MyBot.endForce);
		MyBotForce[2]=MaxForce(MxForce,MyBot.pivotForce);
		MyBotForce[3]=MaxForce(MxForce,MyBot.angleForce);
		MyBotForce[4]=MaxForce(MxForce,MyBot.rotateForce);*/
		if(FroceMoveMode==0)
		{
			mapped[FORCE_BIAS_BASE]=MaxForce(MxForce,(int)((float)MyBot.baseForce)*fScale);
			mapped[FORCE_BIAS_END]=MaxForce(MxForce,(int)((float)MyBot.endForce)*fScale);
			mapped[FORCE_BIAS_PIVOT]=MaxForce(MxForce,(int)((float)MyBot.pivotForce)*fScale);
			mapped[FORCE_BIAS_ANGLE]=MaxForce(MxForce,(int)((float)MyBot.angleForce)*fScale);
			mapped[FORCE_BIAS_ROT]=MaxForce(MxForce,(int)((float)MyBot.rotateForce)*fScale);
		}
		else
		{
			ForcePossition[0]=MyBot.base;
			ForcePossition[1]=MyBot.end;
			ForcePossition[2]=MyBot.pivot;
			ForcePossition[3]=MyBot.angle;
			ForcePossition[4]=MyBot.rotate;
		}
		FroceMoveMode=MyBot.ForceMoveState;
		
	}
	
	//MoveRobot(MyBot.base,MyBot.end,MyBot.pivot,MyBot.angle,MyBot.rotate,BLOCKING_MOVE);
/*	Cycle++;
	if(Cycle>100)
		return FALSE;
	else	
		return TRUE;*/
}
bool ProcessServerSendData(char *sendBuff)
{
	
	struct BotPossition MyBot={0,0,0,0,0,0,0,0,0,0,0,0};
	static int Cycle = 0;
	MyBot.baseForce=getNormalizedInput(BASE_POSITION_DELTA);
	MyBot.endForce=getNormalizedInput(END_POSITION_DELTA);
	MyBot.pivotForce=getNormalizedInput(PIVOT_POSITION_DELTA);
	MyBot.angleForce=getNormalizedInput(ANGLE_POSITION_DELTA);
	MyBot.rotateForce=getNormalizedInput(ROT_POSITION_DELTA);
	MyBot.Controlled=controlled;
	MyBot.base=getNormalizedInput(BASE_POSITION_FORCE_DELTA);
	MyBot.end=getNormalizedInput(END_POSITION_FORCE_DELTA);
	MyBot.pivot=getNormalizedInput(PIVOT_POSITION_FORCE_DELTA);
	MyBot.angle=getNormalizedInput(ANGLE_POSITION_FORCE_DELTA);
	MyBot.rotate=getNormalizedInput(ROT_POSITION_FORCE_DELTA);
	MyBot.ForceMoveState=FroceMoveMode;
	memcpy(sendBuff,&MyBot,sizeof(MyBot));
//	
//	s//printf(sendBuff, "%d \n", Cycle);
//	Cycle++;
	return TRUE;
}
bool ProcessServerSendDataDDE(char *sendBuff,char *recBuff)
{
	int i;
	int *sendBuffReTyped;
	const char delimiters[] = " ,";
	char *token;

/*	for(i=0;i<25;i++)
	{
		//printf("%c",recBuff[i]);
	}*/
//	if(recBuff[0]=="g")
	token = strtok (recBuff, delimiters);

	{
		////printf("returning heartbeat\n");
		sendBuffReTyped=(int *)sendBuff;
		sendBuffReTyped[0]=atoi(token);
		token = strtok (NULL, delimiters);
		////printf("\n %s \n",token);
		sendBuffReTyped[1]=atoi(token);
		token = strtok (NULL, delimiters);
		token = strtok (NULL, delimiters);
		token = strtok (NULL, delimiters);
		sendBuffReTyped[2]=0;//   this shoud be start_time_internal broken into 2 words atoi(token);
		sendBuffReTyped[3]=0;//   this shoud be start_time_internal broken into 2 words atoi(token);
		sendBuffReTyped[4]=token[0];
		sendBuffReTyped[5]=DexError;



		//sendBuffReTyped[1]=token[0];
		//sendBuffReTyped[2]=DexError;
		
		for(i=0;i<59;i++)
		{
			sendBuffReTyped[i+6]=getNormalizedInput(StatusReportIndirection[i]);
			/*if(StatusReportIndirection[i] == BASE_POSITION_PID_DELTA)
			{
				//printf(" PID Delta for L1 = %d", sendBuffReTyped[i+6]);
			}
			if(StatusReportIndirection[i] == BASE_POSITION_DELTA)
			{
				//printf(" TABLE OFFSET Delta for L1 = %d", sendBuffReTyped[i+6]);
			}*/
		}
		return TRUE;
	}
	return FALSE;
}
bool ProcessServerReceiveDataDDE(char *recBuff)
{
	struct BotPossition MyBot;
	char CmdString[255];
	int i,FoundStart=0,j=0;
	bool GotDelim=FALSE;
	int *sendBuffReTyped;
	const char delimiters[] = " ,";
	char *token;
	
	//memcpy(&MyBot,recBuff,sizeof(MyBot));
	////printf("Server %d %d %d %d %d \n",MyBot.base,MyBot.end,MyBot.pivot,MyBot.angle,MyBot.rotate);
	//mapped[FINE_ADJUST_BASE]=MyBot.base;
	//mapped[FINE_ADJUST_END]=MyBot.end;
	//mapped[FINE_ADJUST_PIVOT]=MyBot.pivot;
	//mapped[FINE_ADJUST_ANGLE]=MyBot.angle;
	//mapped[FINE_ADJUST_ROT]=MyBot.rotate;
	
	//MoveRobot(MyBot.base,MyBot.end,MyBot.pivot,MyBot.angle,MyBot.rotate,BLOCKING_MOVE);
	FoundStart=0;
	for(i=0;i<255;i++)
	{
		if(FoundStart >= 4)
		{
			CmdString[j]=recBuff[i];
			j++;
		}
		if(recBuff[i]==' ')
		{
			FoundStart++;
		}
		if(recBuff[i]==0x3b)
		{
			recBuff[i]=0;
			GotDelim=TRUE;
			break;
		}
	}
	if(GotDelim==FALSE)
	{
		DexError=2;
		////printf("%s",recBuff);
		return FALSE;
	}
	CmdString[j-1]=0;
	if(CmdString[0] != 'g')
	{
		//printf("\n%s \n",recBuff);
		//printf("\n%s \n",CmdString);
		
	}
	////printf("\n%s \n",recBuff);
	////printf("\n%s \n",CmdString);
	DexError=ParseInput(CmdString); 
////printf("\nsent parse\n"); 
	return TRUE;
}

void StartClientSocket()
{
    int sockfd = 0, n = 0,j;
    char recvBuff[64];
    char sendBuff[64];
    struct sockaddr_in serv_addr; 
	int Cycle = 100;
	struct BotPossition MyBot;
	float DiffDelta;
	float ForceScale=0;

    memset(recvBuff, '0',sizeof(recvBuff));
    if((sockfd = socket(AF_INET, SOCK_STREAM, 0)) < 0)
    {
        //printf("\n Error : Could not create socket \n");
        return 1;
    } 

    memset(&serv_addr, '0', sizeof(serv_addr)); 

    serv_addr.sin_family = AF_INET;
    serv_addr.sin_port = htons(40000); 
	RemoteRobotAdd[strcspn(RemoteRobotAdd,"\n")]= 0;

    if(inet_pton(AF_INET, RemoteRobotAdd/*"192.168.1.145"*/, &serv_addr.sin_addr)<=0)
    {
        //printf("\n inet_pton error occured\n");
        return 1;
    } 
    if( connect(sockfd, (struct sockaddr *)&serv_addr, sizeof(serv_addr)) < 0)
    {
       //printf("\n Error : Connect Failed \n");
       return 1;
    } 
//	//printf("\n Socket connect\n");
//	while(1)//for(j=0;j<1000;j++)
	{
		////printf("\n next\n");
		while( (n = recv(sockfd, recvBuff, sizeof(recvBuff),0 ) ) > 0)
		{
			//recvBuff[n] = 0;
			////printf("Socket Read\n");
			////printf("%s ",recvBuff);

			if(n < 0)
			{
				//printf("\n Read error \n");
			} 
			/*mapped[FORCE_BIAS_BASE]=(int)(((float)MyBot.base)*ForceScale);
			mapped[FORCE_BIAS_END]=(int)(((float)MyBot.end)*ForceScale);
			mapped[FORCE_BIAS_PIVOT]=(int)(((float)MyBot.pivot)*ForceScale);
			mapped[FORCE_BIAS_ANGLE]=(int)(((float)MyBot.angle)*ForceScale);
			mapped[FORCE_BIAS_ROT]=(int)(((float)MyBot.rotate)*ForceScale);*/
			////printf("%d %d %d %d %d \n",MyBot.base,MyBot.end,MyBot.pivot,MyBot.angle,MyBot.rotate);
#ifdef FOLLOW_POSSITION
			memcpy(&MyBot,recvBuff,sizeof(MyBot));
			DiffDelta=1.5;
			MyBot.base=SlaveBotPos.base + ((GetAxisCurrent(0) - SetHome.base)/MSMoveRatio.base);
			MyBot.end=SlaveBotPos.end + ((GetAxisCurrent(1) - SetHome.end)/MSMoveRatio.end);
			MyBot.pivot=SlaveBotPos.pivot + ((GetAxisCurrent(2) - SetHome.pivot)/MSMoveRatio.pivot);
			MyBot.angle=SlaveBotPos.angle + (((GetAxisCurrent(3) - SetHome.angle)/MSMoveRatio.angle)*DiffDelta);
			MyBot.rotate=SlaveBotPos.rotate + (((GetAxisCurrent(4) - SetHome.rotate)/MSMoveRatio.rotate)*DiffDelta);
/*			MyBot.base=0;
			MyBot.end=0;
			MyBot.pivot=0;
			MyBot.angle=0;
			MyBot.rotate=0;*/

			/*
			SlaveBotPos.base=MyBot.base;
			SlaveBotPos.end=MyBot.base;
			SlaveBotPos.pivot=MyBot.base;
			SlaveBotPos.angle=MyBot.base;
			SlaveBotPos.rotate=MyBot.base;
			*/
			
			//usleep(25000);

			/*MyBot.base=BASE_POSITION_FORCE_DELTA;
			MyBot.end=12300;
			MyBot.pivot=5123;
			MyBot.angle=10;
			MyBot.rotate=15;*/
			memcpy(sendBuff,&MyBot,sizeof(MyBot));
			Cycle++;

#else
			ProcessServerReceiveData(recvBuff);
			ProcessServerSendData(sendBuff);
			
#endif			
			write(sockfd,sendBuff,sizeof(sendBuff));
		}
	}
}
int GetAxisCurrent(int Axis)
{
	return getNormalizedInput(BASE_POSITION_AT+Axis)+getNormalizedInput(BASE_POSITION_FORCE_DELTA+Axis);
}
/* Initialize new terminal i/o settings */
void initTermios(int echo) 
{
  tcgetattr(0, &old); /* grab old terminal i/o settings */
  new = old; /* make new settings same as old settings */
  new.c_lflag &= ~ICANON; /* disable buffered i/o */
  new.c_lflag &= echo ? ECHO : ~ECHO; /* set echo mode */
  tcsetattr(0, TCSANOW, &new); /* use these new terminal i/o settings now */
}

/* Restore old terminal i/o settings */
void resetTermios(void) 
{
  tcsetattr(0, TCSANOW, &old);
}

/* Read 1 character - echo defines echo mode */
char getch_(int echo) 
{
  char ch;
  initTermios(echo);
  ch = getchar();
  resetTermios();
  return ch;
}

/* Read 1 character without echo */
char getch(void) 
{
  return getch_(0);
}

/* Read 1 character with echo */
char getche(void) 
{
  return getch_(1);
}

int FindIndex(int Axis,int Start,int Length,int Delay)
{
	int i,j,k,ADVal,AvgCOS,AvgSIN;
	switch(Axis)
	{
	    case 0  :
		MoveRobot(Start,0,0,0,0,BLOCKING_MOVE);
		break; 
	    case 1  :
		MoveRobot(0,Start,0,0,0,BLOCKING_MOVE);
		break; 
	    case 2  :
		MoveRobot(0,0,Start,0,0,BLOCKING_MOVE);
		break; 
	    case 3  :
		MoveRobot(0,0,0,Start,0,BLOCKING_MOVE);
		break; 
	    case 4  :
		MoveRobot(0,0,0,0,Start,BLOCKING_MOVE);
		break; 
	}
	for(k=0;k<abs(Length);k++) 
	{	
		if(Length>0)
			mapped[Axis]=Start+k;
		else	
			mapped[Axis]=Start-k;
		for(j=0;j<Delay;j++)
		{	
			ADVal=mapped[ADLookUp[Axis]];
			AvgSIN=AvgSIN+ADVal;
			AvgCOS=AvgCOS+mapped[ADLookUp[Axis]+1];
		}	
		AvgSIN=AvgSIN/Delay;
		AvgCOS=AvgCOS/Delay;
		////printf("\n SIN %x COS %x",AvgSIN,AvgCOS);
		if((AvgCOS>XLowBound[Axis]) && (AvgCOS<XHighBound[Axis]) && (AvgSIN>YLowBound[Axis]) && (AvgSIN<YHighBound[Axis]))
		{
			// we found the index 
			return 0;
		}
	}
	return 1; // did not find index
}
void SetNewBotRef()
{
	SlaveBotPos.base=SlaveBotPos.base + ((GetAxisCurrent(0) - SetHome.base)/MSMoveRatio.base);
	SlaveBotPos.end=SlaveBotPos.end + ((GetAxisCurrent(1) - SetHome.end)/MSMoveRatio.end);
	SlaveBotPos.pivot=SlaveBotPos.pivot + ((GetAxisCurrent(2) - SetHome.pivot)/MSMoveRatio.pivot);
	SlaveBotPos.angle=SlaveBotPos.angle + ((GetAxisCurrent(3) - SetHome.angle)/MSMoveRatio.angle);
	SlaveBotPos.rotate=SlaveBotPos.rotate + ((GetAxisCurrent(4) - SetHome.rotate)/MSMoveRatio.rotate);
	SetHome.base=GetAxisCurrent(0);
	SetHome.end=GetAxisCurrent(1);
	SetHome.pivot=GetAxisCurrent(2);
	SetHome.angle=GetAxisCurrent(3);
	SetHome.rotate=GetAxisCurrent(4);
	
}
void* CapturePoints(void *arg)
{
    unsigned long i = 0;
	int a1,a2,a3,a4,a5,f1,f2,f3,f4,f5,BGM=10,EmbedRec=1,RecState=0,Length=0,GripperToggle=0,ServoSet=180;
	const char delimiters[] = ".";

	char c,*FileNameBase,*FileNameExt,*tmpSt,RecordFilename[256],iString[256];
//    pthread_t id = pthread_self();
	initTermios(0);
	f1=0;
	mapped[FINE_ADJUST_BASE]=f1;
	f3 =0;
	mapped[FINE_ADJUST_END]=f3;
	f2=0;
	mapped[FINE_ADJUST_PIVOT]=f2;
	f4=0;
	mapped[FINE_ADJUST_ANGLE]=f4;
	f5=0;
	mapped[FINE_ADJUST_ROT]=f5;
	ForcePossition[0]=0;
	ForcePossition[1]=0;
	ForcePossition[2]=0;
	ForcePossition[3]=0;
	ForcePossition[4]=0;


	mapped[COMMAND_REG]=12448;
	//printf("/n Thread running");
	if(RunMode==1)
		forceMode=3;
	c=' ';
	//mapped[MAX_ERROR]=(2000 ^ 6000);
	while(c!='k') // kill
	{
		mapped[FINE_ADJUST_BASE]=f1;
		mapped[FINE_ADJUST_END]=f3;
		mapped[FINE_ADJUST_PIVOT]=f2;
		mapped[FINE_ADJUST_ANGLE]=f4;
		mapped[FINE_ADJUST_ROT]=f5;
		c=getchar();
		if(c=='l')  // learn movement
		{
			if(RecState==0)
			{
				RecState=1;
				strcpy(iString,CptA.FileName);
				FileNameBase = strtok (iString, delimiters);
				//FileNameExt=strtok (NULL, delimiters);
				strcpy(RecordFilename,FileNameBase);
				strcat(RecordFilename,"ERM");
				asprintf(&tmpSt, "%d", EmbedRec++);
				strcat(RecordFilename,tmpSt);
				free(tmpSt);
				strcat(RecordFilename,".dta");
				a1=getNormalizedInput(BASE_POSITION_AT)+getNormalizedInput(BASE_POSITION_FORCE_DELTA)+f1;
				a3=getNormalizedInput(END_POSITION_AT)+getNormalizedInput(END_POSITION_FORCE_DELTA)+f3;
				a2=getNormalizedInput(PIVOT_POSITION_AT)+getNormalizedInput(PIVOT_POSITION_FORCE_DELTA)+f2;
				a4=getNormalizedInput(ANGLE_POSITION_AT)+getNormalizedInput(ANGLE_POSITION_FORCE_DELTA)+f4;
				a5=getNormalizedInput(ROT_POSITION_AT)+getNormalizedInput(ROT_POSITION_FORCE_DELTA)+f5;
				fprintf(CptA.fp,"a %d,%d,%d,%d,%d\n",a1,a2,a3,a4,a5);
				//printf("\na %d,%d,%d,%d,%d",a1,a2,a3,a4,a5);
				fprintf(CptA.fp,"o %s\n",RecordFilename);
				mapped[REC_PLAY_TIMEBASE]=5;
				mapped[REC_PLAY_CMD]=CMD_RESET_RECORD;
				mapped[REC_PLAY_CMD]=0;				
				mapped[REC_PLAY_CMD]=CMD_RECORD; // start recording

				//InitCaptureMovement(RecordFilename);
			}
			else
			{
				RecState=0;
				a1=getNormalizedInput(BASE_POSITION_AT)+getNormalizedInput(BASE_POSITION_FORCE_DELTA)+f1;
				a3=getNormalizedInput(END_POSITION_AT)+getNormalizedInput(END_POSITION_FORCE_DELTA)+f3;
				a2=getNormalizedInput(PIVOT_POSITION_AT)+getNormalizedInput(PIVOT_POSITION_FORCE_DELTA)+f2;
				a4=getNormalizedInput(ANGLE_POSITION_AT)+getNormalizedInput(ANGLE_POSITION_FORCE_DELTA)+f4;
				a5=getNormalizedInput(ROT_POSITION_AT)+getNormalizedInput(ROT_POSITION_FORCE_DELTA)+f5;
				//f//printf(CptA.fp,"a %d,%d,%d,%d,%d\n",a1,a2,a3,a4,a5);
				//printf("\na %d,%d,%d,%d,%d",a1,a2,a3,a4,a5);
				//f//printf(CptA.fp,"o %s\n",RecordFilename);

				mapped[REC_PLAY_CMD]=0; // stop recording
				Length=mapped[RECORD_BLOCK_SIZE];
				ReadDMA(0x3f000000,Length,RecordFilename);
				mapped[REC_PLAY_CMD]=CMD_RESET_RECORD;
				mapped[REC_PLAY_CMD]=0;
			}
			
		}
		if(c=='h')
		{
			if(FroceMoveMode==0)
			{
				FroceMoveMode=1;
				//printf("/n force mode %d",FroceMoveMode);
			}
			else
			{
				FroceMoveMode=0;
				//printf("/n force mode %d",FroceMoveMode);
			}
		}
		if(c=='+')
		{
			BGM=BGM+10;
			SetNewBotRef();
			/*SetHome.base = SlaveBotPos.base;
			SetHome.end = SlaveBotPos.end;
			SetHome.pivot = SlaveBotPos.pivot;
			SetHome.angle = SlaveBotPos.angle;
			SetHome.rotate = SlaveBotPos.rotate;*/
			
			MSMoveRatio.base++;
			MSMoveRatio.end++;
			MSMoveRatio.pivot++;
			MSMoveRatio.angle++;
			MSMoveRatio.rotate++;
			//printf("\n big move %d MasterSlave Move ratio %d\n",BGM,MSMoveRatio.base);
		}
		if(c=='-')
		{
			BGM=BGM-10;
			if(BGM < 10)
			{
				BGM=10;
			}
			if(MSMoveRatio.base != 1)
			{
				/*SetHome.base = SlaveBotPos.base;
				SetHome.end = SlaveBotPos.end;
				SetHome.pivot = SlaveBotPos.pivot;
				SetHome.angle = SlaveBotPos.angle;
				SetHome.rotate = SlaveBotPos.rotate;*/
				SetNewBotRef();
				MSMoveRatio.base--;
				MSMoveRatio.end--;
				MSMoveRatio.pivot--;
				MSMoveRatio.angle--;
				MSMoveRatio.rotate--;
			}
			//printf("\n big move %d MasterSlave Move ratio %d\n",BGM,MSMoveRatio.base);
		}
		if(c=='h')
		{
			SetNewBotRef();
			/*
			SetHome.base = getNormalizedInput(BASE_POSITION_AT)+getNormalizedInput(BASE_POSITION_FORCE_DELTA);
			SetHome.end = getNormalizedInput(END_POSITION_AT)+getNormalizedInput(END_POSITION_FORCE_DELTA);
			SetHome.pivot = getNormalizedInput(PIVOT_POSITION_AT)+getNormalizedInput(PIVOT_POSITION_FORCE_DELTA);
			SetHome.angle = getNormalizedInput(ANGLE_POSITION_AT)+getNormalizedInput(ANGLE_POSITION_FORCE_DELTA);
			SetHome.rotate = getNormalizedInput(ROT_POSITION_AT)+getNormalizedInput(ROT_POSITION_FORCE_DELTA);
			*/
		}	
		if(c=='p')
		{
			a1=getNormalizedInput(BASE_POSITION_AT)+getNormalizedInput(BASE_POSITION_FORCE_DELTA)+f1;
			a3=getNormalizedInput(END_POSITION_AT)+getNormalizedInput(END_POSITION_FORCE_DELTA)+f3;
			a2=getNormalizedInput(PIVOT_POSITION_AT)+getNormalizedInput(PIVOT_POSITION_FORCE_DELTA)+f2;
			a4=getNormalizedInput(ANGLE_POSITION_AT)+getNormalizedInput(ANGLE_POSITION_FORCE_DELTA)+f4;
			a5=getNormalizedInput(ROT_POSITION_AT)+getNormalizedInput(ROT_POSITION_FORCE_DELTA)+f5;
			fprintf(CptA.fp,"a %d,%d,%d,%d,%d\n",a1,a2,a3,a4,a5);
			fprintf(CptA.fp,"S EERoll,%d\n",ServoSet);
			//printf("\na %d,%d,%d,%d,%d",a1,a2,a3,a4,a5);
		}
		if(c==' ')
		{ 
			fprintf(CptA.fp,"z 500000\n");
			//printf("\n insert dwell .5 seconds\n");
		}
		if(c=='g')
		{ 
			if(GripperToggle==0)
			{
				fprintf(CptA.fp,"S GripperMotor,1\n");
				//printf("\n GripperMotor On\n");
				GripperToggle=1;
				SetGripperMotor(GripperToggle);
			}
			else
			{
				fprintf(CptA.fp,"S GripperMotor,0\n");
				//printf("\n GripperMotor Off\n");
				GripperToggle=0;
				SetGripperMotor(GripperToggle);
			}
		}
		if(c=='t')
		{
			if(forceMode==1)
			{
				mapped[DIFF_FORCE_SPEED_FACTOR_ANGLE]=0;
				mapped[DIFF_FORCE_SPEED_FACTOR_ROT]=0;
				mapped[PID_ADDRESS]=3;
				mapped[PID_P]=DEFAULT_PID_SETTING_PY;
				mapped[PID_ADDRESS]=4;
				mapped[PID_ADDRESS]=0;
				mapped[PID_P]=0;
				mapped[PID_ADDRESS]=1;
				mapped[PID_ADDRESS]=2;
				mapped[SPEED_FACTORA]=DEF_SPEED_FACTOR_A ;
				forceMode=2;
				//printf("\n Move XYZ yo \n");
				controlled=1;
				
			}else if(forceMode==0)
			{

				mapped[DIFF_FORCE_SPEED_FACTOR_ANGLE]=DEF_SPEED_FACTOR_DIFF;
				mapped[DIFF_FORCE_SPEED_FACTOR_ROT]=DEF_SPEED_FACTOR_DIFF;
				mapped[PID_ADDRESS]=3;
				mapped[PID_P]=0;
				mapped[PID_ADDRESS]=4;
				mapped[PID_ADDRESS]=0;
				mapped[PID_P]=DEFAULT_PID_SETTING_XYZ;
				mapped[PID_ADDRESS]=1;
				mapped[PID_ADDRESS]=2;
				mapped[SPEED_FACTORA]=0;
				forceMode=1;
				controlled=0;
				
				//printf("\n Move Pitch and Yaw \n");
				
			}else if(forceMode==2)
			{
				mapped[DIFF_FORCE_SPEED_FACTOR_ANGLE]=0;
				mapped[DIFF_FORCE_SPEED_FACTOR_ROT]=0;
				mapped[PID_ADDRESS]=0;
				mapped[PID_P]=DEFAULT_PID_SETTING_XYZ;
				mapped[PID_ADDRESS]=1;
				mapped[PID_ADDRESS]=2;
				mapped[PID_ADDRESS]=3;
				
//				mapped[PID_P]=0; //DEFAULT_PID_SETTING_PY;
				mapped[PID_P]=DEFAULT_PID_SETTING_PY;
				mapped[PID_ADDRESS]=4;
				mapped[SPEED_FACTORA]=0;
				if(RunMode==1)
					forceMode=3;
				else
					forceMode=0;
				//printf("\n Move fine adjust all \n");
				controlled=0;
			}else if(forceMode==3)
			{
				////printf("\n Move all \n");
				mapped[DIFF_FORCE_SPEED_FACTOR_ANGLE]=DEF_SPEED_FACTOR_DIFF;
				mapped[DIFF_FORCE_SPEED_FACTOR_ROT]=DEF_SPEED_FACTOR_DIFF;
				mapped[PID_P]=0;
				mapped[PID_ADDRESS]=3;
				mapped[PID_ADDRESS]=4;
				mapped[PID_ADDRESS]=0;
				mapped[PID_ADDRESS]=1;
				mapped[PID_ADDRESS]=2;
				mapped[SPEED_FACTORA]=DEF_SPEED_FACTOR_A ;
				forceMode=2;
				controlled=1;
				//printf("\n Move All \n");
			}
		}
		if(c=='q')
		{
			f1--;
		}
		if(c=='w')
		{
			f1++;
		}
		if(c=='a')
		{
			f2--;
		}
		if(c=='s')
		{
			f2++;
		}
		if(c=='z')
		{
			f3--;
		}
		if(c=='x')
		{
			f3++;
		}
		if(c=='e')
		{
			f4--;
		}
		if(c=='r')
		{
			f4++;
		}
		if(c=='d')
		{
			f5--;
		}
		if(c=='f')
		{
			f5++;
		}
		if(c=='Q')
		{
			f1=f1-BGM;
		}
		if(c=='W')
		{
			f1=f1+BGM;
		}
		if(c=='A')
		{
			f2=f2-BGM;
		}
		if(c=='S')
		{
			f2=f2+BGM;
		}
		if(c=='Z')
		{
			f3=f3-BGM;
		}
		if(c=='X')
		{
			f3=f3+BGM;
		}
		if(c=='E')
		{
			f4=f4-BGM;
		}
		if(c=='R')
		{
			f4=f4+BGM;
		}
		if(c=='D')
		{
			f5=f5-BGM;
		}
		if(c=='F')
		{
			f5=f5+BGM;
		}
		if(c==',')
		{
			ServoSet=ServoSet+1;
			SetGripperRoll(ServoSet);
		}
		if(c=='.')
		{
			ServoSet=ServoSet-1;
			SetGripperRoll(ServoSet);
		}
	}
	mapped[MAX_ERROR]=(2000 ^ 4000);
	mapped[DIFF_FORCE_SPEED_FACTOR_ANGLE]=0;
	mapped[DIFF_FORCE_SPEED_FACTOR_ROT]=0;
	mapped[PID_ADDRESS]=0;
	mapped[PID_P]=DEFAULT_PID_SETTING_XYZ;
	mapped[PID_ADDRESS]=1;
	mapped[PID_ADDRESS]=2;
	mapped[PID_ADDRESS]=3;
	mapped[PID_P]=DEFAULT_PID_SETTING_PY;
	mapped[PID_ADDRESS]=4;
	mapped[SPEED_FACTORA]=0;
	
	
	mapped[FINE_ADJUST_BASE]=0;
	mapped[FINE_ADJUST_END]=0;
	mapped[FINE_ADJUST_PIVOT]=0;
	mapped[FINE_ADJUST_ANGLE]=0;
	mapped[FINE_ADJUST_ROT]=0;
        mapped[COMMAND_REG]=3158688;	
	//printf("\nAll done\n");
	controlled=0;
	fprintf(CptA.fp,"x\n");
			
	fclose(CptA.fp);
	resetTermios();
	
    return NULL;
}
int InitCapturePoints(char *FileName)
{
    int i = 0;
    int err;
	CptA.FileName=FileName;
	CptA.fp=fopen(FileName, "w");
	if(CptA.fp!=0)
	{
		CapturePoints((void*)&CptA);
	}

/*		err = pthread_create(&(tid[i]), NULL, &CapturePoints, (void*)&CptA);
		if (err != 0)
		{
			//printf("\ncan't create thread :[%s]", strerror(err));
			return 1;
		}
		else
		{
			//printf("\n Begin Program\n");
		}
    }*/
	return 0;
}


int InitCaptureMovement(char *FileName)
{
    int i = 0,Length=0;
    int err;
	char c;
	//initTermios(0);
//	CptMove.FileName=FileName;
//	CptMove.fp=fopen(FileName, "w");
//	if(CptMove.fp!=0)
	{
		//CaptureMovement((void*)&CptMove);
		mapped[REC_PLAY_CMD]=CMD_RESET_RECORD;
		mapped[REC_PLAY_CMD]=CMD_RECORD; // start recording
		c=' ';
		while(c!='k') // kill
		{
			c=getchar();
			
			
		}
		mapped[REC_PLAY_CMD]=0; // stop recording
		
		Length=mapped[RECORD_BLOCK_SIZE];
		ReadDMA(0x3f000000,Length,FileName);

		
	}
	//resetTermios();
	return 0;
}


int fixedPointCV(float Val,int whole,int fract)
{
	return 0;
}
void setDefaults(int State)
{

	int i,ForceFelt,j,HiBoundry,LowBoundry,BoundryACC;
	char c;
    FILE *CentersFile,*RemoteRobotAddress,*DiffFile;
	int HexValue;
	int IntFloat;
	float *fConvert=(float *)(&IntFloat);
	
	
	
	DiffFile = fopen("DiffCorrectionFactor.txt", "rs");
	if(DiffFile!=NULL)
	{
		fscanf (DiffFile, "%f", &DiffCorrectionFactor);
		//fgets(DiffFile, sizeof(RemoteRobotAdd)+1, RemoteRobotAddress);
		//fscanf(RemoteRobotAddress,"%[^\n]",RemoteRobotAdd);
		//printf("%f \n",DiffCorrectionFactor);
		fclose(DiffFile);
	}
	DiffFile = fopen("AngleBoundry.txt", "rs");
	if(DiffFile!=NULL)
	{
		fscanf (DiffFile, "%i %i", &AngleHIBoundry,&AngleLOWBoundry);
		//fgets(DiffFile, sizeof(RemoteRobotAdd)+1, RemoteRobotAddress);
		//fscanf(RemoteRobotAddress,"%[^\n]",RemoteRobotAdd);
		//printf("Angle High boundry%i    Angle Low Boundry %i \n",AngleHIBoundry,AngleLOWBoundry);
		fclose(DiffFile);
	}

	
	RemoteRobotAddress = fopen("RemoteRobotAddress.txt", "rs");
	if(RemoteRobotAddress!=NULL)
	{
		fgets(RemoteRobotAdd, sizeof(RemoteRobotAdd)+1, RemoteRobotAddress);
		//fscanf(RemoteRobotAddress,"%[^\n]",RemoteRobotAdd);
		//printf("%s \n",RemoteRobotAdd);
		fclose(RemoteRobotAddress);
	}
	mapped[FORCE_BIAS_BASE]=0;
	mapped[FORCE_BIAS_END]=0;
	mapped[FORCE_BIAS_PIVOT]=0;
	mapped[FORCE_BIAS_ANGLE]=0;
	mapped[FORCE_BIAS_ROT]=0;

	mapped[ACCELERATION_MAXSPEED]=ACCELERATION_MAXSPEED_DEF;
	maxSpeed=(ACCELERATION_MAXSPEED_DEF) & 0b00000000000011111111111111111111;
	coupledAcceleration=((ACCELERATION_MAXSPEED_DEF) & 0b00000011111100000000000000000000) >> 20;

	if(State==1)
	{


		mapped[BASE_FORCE_DECAY]=000000;
		mapped[END_FORCE_DECAY]=000000;
		mapped[PIVOT_FORCE_DECAY]=000000;
		mapped[ANGLE_FORCE_DECAY]=000000;
		mapped[ROTATE_FORCE_DECAY]=000000;

		mapped[ACCELERATION_MAXSPEED]=ACCELERATION_MAXSPEED_DEF;
		maxSpeed=(ACCELERATION_MAXSPEED_DEF) & 0b00000000000011111111111111111111;
		coupledAcceleration=((ACCELERATION_MAXSPEED_DEF) & 0b00000011111100000000000000000000) >> 20;
	

		mapped[REC_PLAY_TIMEBASE]=5;
		
    CentersFile = fopen("AdcCenters.txt", "rs");

    //read file into array
	if(CentersFile!=NULL)
	{
		fscanf(CentersFile, "%x", &HexValue);
		//printf("%x \n",HexValue);
		mapped[BASE_SIN_CENTER]=HexValue;
		fscanf(CentersFile, "%x", &HexValue);
		mapped[BASE_COS_CENTER]=HexValue;
		fscanf(CentersFile, "%x", &HexValue);
		mapped[END_SIN_CENTER]=HexValue;
		fscanf(CentersFile, "%x", &HexValue);
		mapped[END_COS_CENTER]=HexValue;
		fscanf(CentersFile, "%x", &HexValue);
		mapped[PIVOT_SIN_CENTER]=HexValue;
		fscanf(CentersFile, "%x", &HexValue);
		mapped[PIVOT_COS_CENTER]=HexValue;
		fscanf(CentersFile, "%x", &HexValue);
		mapped[ANGLE_SIN_CENTER]=HexValue;
		fscanf(CentersFile, "%x", &HexValue);
		mapped[ANGLE_COS_CENTER]=HexValue;
		fscanf(CentersFile, "%x", &HexValue);
		mapped[ROT_SIN_CENTER]=HexValue;
		fscanf(CentersFile, "%x", &HexValue);
		mapped[ROT_COS_CENTER]=HexValue;
		fclose(CentersFile);
	}
	
	CentersFile = fopen("AxisCal.txt", "rs");

    //read file into array
	if(CentersFile!=NULL)
	{
		fscanf(CentersFile, "%f", &JointsCal[0]);
		//printf("%f \n",JointsCal[0]);
		fscanf(CentersFile, "%f", &JointsCal[1]);
		fscanf(CentersFile, "%f", &JointsCal[2]);
		fscanf(CentersFile, "%f", &JointsCal[3]);
		fscanf(CentersFile, "%f", &JointsCal[4]);
		fscanf(CentersFile, "%i", &HexValue);
		mapped[ANGLE_END_RATIO]=HexValue;//((LG_RADIUS/SM_RADIUS * MOTOR_STEPS * MICRO_STEP)/(MOTOR_STEPS*GEAR_RATIO*MICRO_STEP))*2^24
		fclose(CentersFile);
	}
 
    

/*		mapped[BASE_SIN_CENTER]=0x7f80000;
		mapped[BASE_COS_CENTER]=0x7f80000;
		mapped[END_SIN_CENTER]=0x7f80000;
		mapped[END_COS_CENTER]=0x07f80000;
		mapped[PIVOT_SIN_CENTER]=0x4a00000;
		mapped[PIVOT_COS_CENTER]=0x6500000;
		mapped[ANGLE_SIN_CENTER]=0x7890000;
		mapped[ANGLE_COS_CENTER]=0x5080000;
		mapped[ROT_SIN_CENTER]=0x5200000;
		mapped[ROT_COS_CENTER]=0x6f00000;*/

//		mapped[BASE_SIN_CENTER]=0x7f80000;
//		mapped[BASE_COS_CENTER]=0x7f80000;
//		mapped[END_SIN_CENTER]=0x7f80000;
//		mapped[END_COS_CENTER]=0x07f80000;
//		mapped[PIVOT_SIN_CENTER]=0x6500000;
//		mapped[PIVOT_COS_CENTER]=0x4a00000;
//		mapped[ANGLE_SIN_CENTER]=0x7f80000;
//		mapped[ANGLE_COS_CENTER]=0x4900000;
//		mapped[ROT_SIN_CENTER]=0x6f00000;
//		mapped[ROT_COS_CENTER]=0x5200000;


		mapped[DIFF_FORCE_BETA ]=0x0102;

	// set up PID defaults
	
		mapped[PID_P]=DEFAULT_PID_SETTING_XYZ;
		mapped[PID_I]=0;
		mapped[PID_D]=0;
		mapped[PID_DELTATNOT]=0;
		mapped[PID_DELTAT]=0;
		mapped[PID_ADDRESS]=0;
		mapped[PID_ADDRESS]=1;
		mapped[PID_ADDRESS]=2;
		mapped[PID_ADDRESS]=3;
		mapped[PID_P]=DEFAULT_PID_SETTING_PY;
		mapped[PID_ADDRESS]=4;
		
		// set boundries  
		BaseBoundryHigh=90000;
		BaseBoundryLow=-90000;
		EndBoundryHigh=130000;
		EndBoundryLow=-130000;
		PivotBoundryHigh=70000;
		PivotBoundryLow=-70000;
		AngleBoundryHigh=DEFAULT_ANGLE_BOUNDRY_HI;
		AngleBoundryLow=DEFAULT_ANGLE_BOUNDRY_LOW;
		RotateBoundryHigh=3000;
		RotateBoundryLow=-3000;
		
		
		mapped[BOUNDRY_BASE]=0x2bf2d40e; //
		mapped[BOUNDRY_END]=0x3f7ac086;  //
		mapped[BOUNDRY_PIVOT]=0x222eddd2; //1
		HiBoundry=AngleHIBoundry/8;
		LowBoundry=AngleLOWBoundry/8;
		BoundryACC=(HiBoundry << 16) | (0x0000ffff & LowBoundry);
		mapped[BOUNDRY_ANGLE]=BoundryACC;// 0x01f4fe0c; //
		//printf("Angle boundry pack %x ",BoundryACC);
		
		mapped[BOUNDRY_ROT]=((RotateBoundryHigh/8) << 16) | (0X0000FFFF & (RotateBoundryLow/8));

		
		
		
		

		mapped[SPEED_FACTORA]=0;
		mapped[SPEED_FACTORB]=0;
		mapped[MAX_ERROR]=(2000 ^ 6000);
		

		mapped[DIFF_FORCE_TIMEBASE]=140000; // this is currently remapped to MaxSpeed XYZ Force
		
		mapped[DIFF_FORCE_MAX_SPEED]=90000;
//		IntFloat)
		*fConvert=DiffCorrectionFactor;
		
		//printf(" float value %x \n",IntFloat);
		//if(RunMode==1)
			mapped[DIFF_FORCE_ANGLE_COMPENSATE]=0;//IntFloat;
		//else
		//	mapped[DIFF_FORCE_ANGLE_COMPENSATE]=0;//IntFloat;
		
		//mapped[ANGLE_END_RATIO]=645278;//((LG_RADIUS/SM_RADIUS * MOTOR_STEPS * MICRO_STEP)/(MOTOR_STEPS*GEAR_RATIO*MICRO_STEP))*2^24
		




	
		mapped[FRICTION_BASE]=0x130; // fixed point 16 bit == .1
		mapped[FRICTION_END]=0x130; // fixed point 16 bit == .1
		mapped[FRICTION_PIVOT]=0x130; // fixed point 16 bit == .1
		mapped[FRICTION_ANGLE]=0x50; // fixed point 16 bit == .1
		mapped[FRICTION_ROT]=0x50; // fixed point 16 bit == .1
	
		
		
	}
	if(State==2)
	{
		CmdVal=3158688;
		mapped[FINE_ADJUST_BASE]=0;
		mapped[FINE_ADJUST_END]=0;
		mapped[FINE_ADJUST_PIVOT]=0;
		mapped[FINE_ADJUST_ANGLE]=0;
		mapped[FINE_ADJUST_ROT]=0;
	}
	if(State==3)
	{
		
		//printf("\nMonitor Force Started");
		while(1)
		{
			//printf("\nTable Delta ");
			for(i=0;i<5;i++)
			{
				//printf("%6d ",getNormalInput(BASE_POSITION_DELTA+i));
			}
			//printf(" PID_DELTA ");
			for(i=0;i<5;i++)
			{
				//printf("%5d ",getNormalizedInput(BASE_POSITION_PID_DELTA+i));
			}
			////printf("\nForce_DELTA ");
			//for(i=0;i<5;i++)
			//{
			//	//printf("%5d ",getNormalizedInput(BASE_POSITION_FORCE_DELTA+i));
			//}
		}
		
	}
	
	if(State==4)
	{
		//initTermios(0);
		mapped[PID_ADDRESS]=3;
		mapped[PID_P]=0;
		mapped[PID_I]=0;
		mapped[PID_D]=0;
		mapped[PID_DELTATNOT]=0;
		mapped[PID_DELTAT]=0;
		mapped[PID_ADDRESS]=4;
		c=' ';
		mapped[FINE_ADJUST_BASE]=0;
		mapped[FINE_ADJUST_END]=0;
		mapped[FINE_ADJUST_PIVOT]=0;
		mapped[FINE_ADJUST_ANGLE]=0;
		mapped[FINE_ADJUST_ROT]=0;
		
		//printf("\nForce Protect Started");
		while(1) 
//		while(c!='k') // kill
		{
			//c=getchar();
			for(i=0;i<5;i++)
			{
				ForceFelt=getNormalizedInput(BASE_POSITION_PID_DELTA+i)-getNormalizedInput(BASE_POSITION_DELTA+i);
				//printf("%5d ",ForceFelt);
				if(abs(ForceFelt-PositionAdjust[i]) >= ForceLimit[i])
				{
					if((ForceFelt-PositionAdjust[i]) < 0)
					{
						//PositionAdjust[i]=2*(abs((ForceFelt-PositionAdjust[i]))-ForceLimit[i]);						
					}
					else
					{
						//PositionAdjust[i]=2*-(abs((ForceFelt-PositionAdjust[i]))-ForceLimit[i]);
					}
				}
			}
			for(i=0;i<3;i++)
			{
				//mapped[FINE_ADJUST_BASE+i]=PositionAdjust[i];
				//printf("%5d ",PositionAdjust[i]);
			}
			//printf("\n");
			
			usleep(10000);
		}	
	//resetTermios();		
	}
	if(State==5)
	{
		
		//printf("\nMonitor Accelerateion Force Started");
		while(1)
		{
			//printf("\nSpeed Delta ");
			for(i=0;i<5;i++)
			{
				//printf("%5d ",mapped[SLOPE_BASE_POSITION+i]);
			}
		}
		
	}
	if(State==6)
	{
		
		//printf("\nMonitor Force offset Started");
		while(1)
		{
			////printf("\nSpeed Delta ");
			for(i=0;i<5;i++)
			{
				//printf("%6d  ",GetAxisCurrent(i));
			}
			//printf("\n");
		}
		
	}
}

void wait_fifo_flush(void)
{
	while((mapped[CMD_FIFO_STATE] & 0x2) != 2)
	{
		
	}
}
int HashInputCMD(char *s)
{

	if(s[0]=='r')
		return READ_CMD;
	if(s[0]=='w')
	{
		wait_fifo_flush();
		return WRITE_CMD;
	}
	if(s[0]=='x')
		return EXIT_CMD;
	if(s[0]=='s')
		return SLOWMOVE_CMD;
	if(s[0]=='a')
		return MOVEALL_CMD;
	if(s[0]=='d')
		return DMAREAD_CMD;
	if(s[0]=='t')
		return DMAWRITE_CMD;
	if(s[0]=='c')
		return CAPTURE_AD_CMD;
	if(s[0]=='f')
		return FIND_HOME_CMD;
	if(s[0]=='n')
		return FIND_INDEX_CMD;
	if(s[0]=='p')
		return FIND_HOME_REP_CMD;
	if(s[0]=='l')
		return LOAD_TABLES;
	if(s[0]=='i')
		return CAPTURE_POINTS_CMD;
	if(s[0]=='z')
		return SLEEP_CMD;
	if(s[0]=='m')
		return RECORD_MOVEMENT;
	if(s[0]=='o')
		return REPLAY_MOVEMENT;
	if(s[0]=='R')
		return MOVEALL_RELATIVE;
	if(s[0]=='S')
		return SET_PARAM;
	if(s[0]=='g')
		return SEND_HEARTBEAT;
	/*if(s[0]=='F')
		return SET_FORCE_MOVE_POINT;*/
	if(s[0]=='G')
		return HEART_BEAT;
	if(s[0]=='h')
		return HEART_BEAT;
	if(s[0]=='P')
		return PID_FINEMOVE;
	if(s[0]=='F')
	{
		wait_fifo_flush();
		return HEART_BEAT;
	}
	return 0;
}



int isNear(int a,int b,int range)
{
	if(abs(a-b)<=range)
		return TRUE;
	else
		return FALSE;
}
int getNormalizedInput(int param) 
{
	int val;
	float corrF=1;
	val = mapped[param];
	if(param <= ROT_POSITION_FORCE_DELTA)
	{
		corrF = JointsCal[(param-INPUT_OFFSET) % 5];
	}
	if((val & 0x40000)!=0)
	{
		val = (val | 0xfff80000);
	}
	return (int)((float)val / corrF);
}
int getNormalInput(int param) 
{
	int val;
	float corrF=1;
	val = mapped[param];
	if((val & 0x40000)!=0)
	{
		val = (val | 0xfff80000);
	}
	return (int)((float)val / corrF);
}



int WaitMoveGoal(int a1,int a2,int a3,int a4,int a5,int timeout)
{
	int b1,b2,b3,b4,b5;
    long            ms; // Milliseconds
    time_t          oldTime,newTime;  // Seconds
    struct timespec spec;

	b1=getNormalizedInput(BASE_POSITION_AT);
	b3=getNormalizedInput(END_POSITION_AT);
	b2=getNormalizedInput(PIVOT_POSITION_AT);
	b4=getNormalizedInput(ANGLE_POSITION_AT);
	b5=getNormalizedInput(ROT_POSITION_AT);

    clock_gettime(CLOCK_REALTIME, &spec);
	oldTime  = spec.tv_sec;
	////printf("\n%d %d %d %d %d\n",b1,b2,b3,b4,b5);	
	////printf("\n%d %d %d %d %d\n",a1,a2,a3,a4,a5);	
//    //printf("\nStart wait Goal");
	while(!isNear(a1,b1,500) || !isNear(a2,b2,500) || !isNear(a3,b3,500) || !isNear(a4,b4,500) || !isNear(a5,b5,500))
	{
		b1=getNormalizedInput(BASE_POSITION_AT);
		b3=getNormalizedInput(END_POSITION_AT);
		b2=getNormalizedInput(PIVOT_POSITION_AT);
		b4=getNormalizedInput(ANGLE_POSITION_AT);
		b5=getNormalizedInput(ROT_POSITION_AT);
/*		c1=getNormalizedInput(BASE_POSITION_DELTA);
		c3=getNormalizedInput(END_POSITION_DELTA);
		c2=getNormalizedInput(PIVOT_POSITION_DELTA);
		c4=getNormalizedInput(ANGLE_POSITION_DELTA);
		c5=getNormalizedInput(ROT_POSITION_DELTA);*/
		clock_gettime(CLOCK_REALTIME, &spec);
		newTime  = spec.tv_sec;
		if((newTime-oldTime)>20)
			return 0;
		
		////printf("\n%d %d %d %d %d Waiting for %d %d %d %d %d ",b1,b2,b3,b4,b5,a1,a2,a3,a4,a5);	
	}
    ////printf("\nEnd wait Goal");
	return 0;
		
}

int moverobotPID(int a1,int a2,int a3,int a4,int a5)
{
	CheckBoundry(&a1,&a2,&a3,&a4,&a5);	
	a1=(int)((double)a1 * JointsCal[0]);
	a2=(int)((double)a2 * JointsCal[1]);
	a3=(int)((double)a3 * JointsCal[2]);
	a4=(int)((double)a4 * JointsCal[3]);
	a5=(int)((double)a5 * JointsCal[4]);
	mapped[FINE_ADJUST_BASE]=a1;
	mapped[FINE_ADJUST_END]=a3;
	mapped[FINE_ADJUST_PIVOT]=a2;
	mapped[FINE_ADJUST_ANGLE]=a4;
	mapped[FINE_ADJUST_ROT]=a5;
}

int MoveRobot(int a1,int a2,int a3,int a4,int a5, int mode)
{

	CheckBoundry(&a1,&a2,&a3,&a4,&a5);
/*	int b1,b2,b3,b4,b5;
	b1=getNormalizedInput(PLAYBACK_BASE_POSITION);
	b2=getNormalizedInput(PLAYBACK_END_POSITION);
	b3=getNormalizedInput(PLAYBACK_PIVOT_POSITION);
	b4=getNormalizedInput(PLAYBACK_ANGLE_POSITION);
	b5=getNormalizedInput(PLAYBACK_ROT_POSITION);
	a1=a1-b1; // subtract out the playback position 
	a2=a2-b2;
	a3=a3-b3;
	a4=a4-b4;
	a5=a5-b5;
	//printf("\nPlayback position %d %d %d %d %d",b1,b2,b3,b4,b5);
*/
	////printf("\nStart wait Goal");
//	332800
//	166400
//	0.25679012345679012345679012345679
//	0.00987654320987654320987654320988

	//if(mode==BLOCKING_MOVE)
		//WaitMoveGoal(LastGoal[0],LastGoal[1],LastGoal[2],LastGoal[3],LastGoal[4],DEFAULT_MOVE_TIMEOUT);

	

	LastGoal[0]=a1;
	LastGoal[1]=a2;
	LastGoal[2]=a3;
	LastGoal[3]=a4;
	LastGoal[4]=a5;

	a1=(int)((double)a1 * JointsCal[0]);
	a2=(int)((double)a2 * JointsCal[1]);
	a3=(int)((double)a3 * JointsCal[2]);
	a4=(int)((double)a4 * JointsCal[3]);
	a5=(int)((double)a5 * JointsCal[4]);
	////printf("angle 0 result %d ",a1);
	while(mapped[CMD_FIFO_STATE] & 0x01 != 0);
	
	mapped[COMMAND_REG]=CMD_MOVEEN | CmdVal;
	mapped[BASE_POSITION]=a1;
	mapped[END_POSITON]=a3;
	mapped[PIVOT_POSITON]=a2;
	mapped[ANGLE_POSITON]=a4;
	mapped[ROT_POSITON]=a5;
	mapped[COMMAND_REG]=CMD_MOVEEN | CMD_MOVEGO | CmdVal;
	mapped[COMMAND_REG]=CmdVal;
	/*if(mode==BLOCKING_MOVE)
		WaitMoveGoal(LastGoal[0],LastGoal[1],LastGoal[2],LastGoal[3],LastGoal[4],DEFAULT_MOVE_TIMEOUT);*/
	return 0;
}

int CheckBoundry(int* j1, int* j2, int* j3, int* j4, int* j5)
{
	int h1,h2,h3,h4,h5,l1,l2,l3,l4,l5;
	h1=(int)((float)BaseBoundryHigh / fabs(JointsCal[0]));
	h2=(int)((float)PivotBoundryHigh / fabs(JointsCal[1]));
	h3=(int)((float)EndBoundryHigh / fabs(JointsCal[2]));
	h4=(int)((float)AngleBoundryHigh / fabs(JointsCal[3]));
	h5=(int)((float)RotateBoundryHigh / fabs(JointsCal[4]));
	l1=(int)((float)BaseBoundryLow / fabs(JointsCal[0]));
	l2=(int)((float)PivotBoundryLow / fabs(JointsCal[1]));
	l3=(int)((float)EndBoundryLow / fabs(JointsCal[2]));
	l4=(int)((float)AngleBoundryLow / fabs(JointsCal[3]));
	l5=(int)((float)RotateBoundryLow / fabs(JointsCal[4]));
	if(*(j1) >= h1)
	{
		*(j1) = h1;
	}
	if(*(j1) <= l1)
	{
		*(j1) = l1;
	}
	if(*(j2) >= h2)
	{
		*(j2) = h2;
	}
	if(*(j2) <= l2)
	{
		*(j2) = l2;
	}
	if(*(j3) >= h3)
	{
		*(j3) = h3;
	}
	if(*(j3) <= l3)
	{
		*(j3) = l3;
	}
	if(*(j4) >= h4)
	{
		*(j4) = h4;
	}
	if(*(j4) <= l4)
	{
		*(j4) = l4;
	}
	if(*(j5) >= h5)
	{
		*(j5) = h5;
	}
	if(*(j5) <= l5)
	{
		*(j5) = l5;
	}
	////printf(" boundry calc %d %d %f %d",*(j1),BaseBoundryHigh,fabs(JointsCal[0]),(int)((float)BaseBoundryHigh / fabs(JointsCal[0])));
/*
EndBoundryHigh
EndBoundryLow
PivotBoundryHigh
PivotBoundryLow
AngleBoundryHigh
AngleBoundryLow
RotateBoundryHigh
RotateBoundryLow
*/
 if(*(j1)==0)
	 *(j1)=1;
 return 0;
}

int SetParam(char *a1,float fa2,int a3,int a4,int a5)
{
	int i,BDH,BDL,Axis;
	int a2=(int)fa2;
	int fxa2=(a2<<8)+(fa2-a2)*256;
	
	////printf("%s %s %d %d \n",Params[i],a1,a2,i);

	for(i=0;i<MAX_PARAMS;i++)
	{
		////printf("%s %s %d %d \n",Params[i],a1,a2,i);
		if(strcmp(a1,Params[i])==0)
		{
				switch(i)
				{
						case 0:
						////printf("Set Speed\n");
							//set Max Speed
							maxSpeed=a2 & 0b00000000000011111111111111111111;
							mapped[ACCELERATION_MAXSPEED]=maxSpeed + (coupledAcceleration << 20);
							return 0;
						break;
						case 1:
							//set Acceleration
							coupledAcceleration=a2 & 0b111111;
							mapped[ACCELERATION_MAXSPEED]=maxSpeed + (coupledAcceleration << 20);
							return 0;
						break;
						case 2:
							mapped[FORCE_BIAS_BASE]=a2;
							return 0;
						case 3:
							mapped[FORCE_BIAS_END]=a2;
							return 0;
						break;
						case 4:
							mapped[FORCE_BIAS_PIVOT]=a2;
							return 0;
						break;
						case 5:
							mapped[FORCE_BIAS_ANGLE]=a2;
							return 0;
						break;
						case 6:
							mapped[FORCE_BIAS_ROT]=a2;
							return 0;
						break;
						case 7:
							mapped[FRICTION_BASE]=fxa2;
							return 0;
						break;
						case 8:
							mapped[FRICTION_END]=fxa2;
							return 0;
						break;
						case 9:
							mapped[FRICTION_PIVOT]=fxa2;
							return 0;
						break;
						case 10:
							mapped[FRICTION_ANGLE]=fxa2;
							return 0;
						break;
						case 11:
							mapped[FRICTION_ROT]=fxa2;
							return 0;
						break;
						case 12:
							BaseBoundryHigh=(int)((float)a2 * fabs(JointsCal[0]));
							////printf(" boundry set %d %d %f %f ", a2, BaseBoundryHigh, JointsCal[0], fabs(JointsCal[0]));
							BDH=BaseBoundryHigh;
							BDL=BaseBoundryLow;
							Axis=BOUNDRY_BASE;
							mapped[Axis]=((BDH/8) << 16) | (0X0000FFFF & (BDL/8));
							return 0;
						break;
						case 13:
							BaseBoundryLow=(int)((float)a2 * fabs(JointsCal[0]));
							BDH=BaseBoundryHigh;
							BDL=BaseBoundryLow;
							Axis=BOUNDRY_BASE;
							mapped[Axis]=((BDH/8) << 16) | (0X0000FFFF & (BDL/8));
							return 0;
						break;
						case 14:
							EndBoundryHigh=(int)((float)a2 * fabs(JointsCal[2]));
							BDH=EndBoundryHigh;
							BDL=EndBoundryLow;
							Axis=BOUNDRY_END;
							mapped[Axis]=((BDH/8) << 16) | (0X0000FFFF & (BDL/8));
							return 0;
						break;
						case 15:
							EndBoundryLow=(int)((float)a2 * fabs(JointsCal[2]));
							BDH=EndBoundryHigh;
							BDL=EndBoundryLow;
							Axis=BOUNDRY_END;
							mapped[Axis]=((BDH/8) << 16) | (0X0000FFFF & (BDL/8));
							return 0;
						break;
						case 16:
							PivotBoundryHigh=(int)((float)a2 * fabs(JointsCal[1]));
							BDH=PivotBoundryHigh;
							BDL=PivotBoundryLow;
							Axis=BOUNDRY_PIVOT;
							mapped[Axis]=((BDH/8) << 16) | (0X0000FFFF & (BDL/8));
							return 0;
						break;
						case 17:
							PivotBoundryLow=(int)((float)a2 * fabs(JointsCal[1]));
							BDH=PivotBoundryHigh;
							BDL=PivotBoundryLow;
							Axis=BOUNDRY_PIVOT;
							mapped[Axis]=((BDH/8) << 16) | (0X0000FFFF & (BDL/8));
							return 0;
						break;
						case 18:
							AngleBoundryHigh=(int)((float)a2 * fabs(JointsCal[3]));
							BDH=AngleBoundryHigh;
							BDL=AngleBoundryLow;
							Axis=BOUNDRY_ANGLE;
							mapped[Axis]=((BDH/8) << 16) | (0X0000FFFF & (BDL/8));
							return 0;
						break;
						case 19:
							AngleBoundryLow=(int)((float)a2 * fabs(JointsCal[3]));
							BDH=AngleBoundryHigh;
							BDL=AngleBoundryLow;
							Axis=BOUNDRY_ANGLE;
							mapped[Axis]=((BDH/8) << 16) | (0X0000FFFF & (BDL/8));
							return 0;
						break;
						case 20:
							RotateBoundryHigh=(int)((float)a2 * fabs(JointsCal[4]));
							BDH=RotateBoundryHigh;
							BDL=RotateBoundryLow;
							Axis=BOUNDRY_ROT;
							mapped[Axis]=((BDH/8) << 16) | (0X0000FFFF & (BDL/8));
							return 0;
						break;
						case 21:
							RotateBoundryLow=(int)((float)a2 * fabs(JointsCal[4]));
							BDH=RotateBoundryHigh;
							BDL=RotateBoundryLow;
							Axis=BOUNDRY_ROT;
							mapped[Axis]=((BDH/8) << 16) | (0X0000FFFF & (BDL/8));
							return 0;
						break;
						case 22:
							SetGripperMotor(a2);
							//printf("Gripper Motor Set\n");
							return 0;
						break;
						case 23:
							SetGripperRoll(a2);
							//printf("Gripper Roll Set\n");
							return 0;
						break;
						case 24:
							mapped[START_SPEED]=500 ^ a2;
						break;
						case 25:
						break;
						default:
						break;
				}
		
		}
	}
	return 0;

}
int MoveRobotRelative(int a1,int a2,int a3,int a4,int a5, int mode)
{
	int b1,b2,b3,b4,b5;
	b1=getNormalizedInput(BASE_POSITION_AT);
	b3=getNormalizedInput(END_POSITION_AT);
	b2=getNormalizedInput(PIVOT_POSITION_AT);
	b4=getNormalizedInput(ANGLE_POSITION_AT);
	b5=getNormalizedInput(ROT_POSITION_AT);
	
	////printf("\nRelative move %d %d %d %d %d",b1,b2,b3,b4,b5);
	return MoveRobot(a1+b1,a2+b2,a3+b3,a4+b4,a5+b5,mode);
}


int ReadDMA(int p1,int p2,char *p3)
{
	FILE *fp;
	int i,j,blocks,writeSize;
	int dataarray[256];
	fp=fopen(p3, "wb");
	if(fp!=0)
	{
		mapped[DMA_CONTROL]=DMA_RESET_ALL;
		mapped[DMA_CONTROL]=0;
		blocks=p2/256;
		for(j=0;j<blocks;j++) // only do full blocks inside loop
		{	
			mapped[DMA_READ_ADDRESS]=p1+(j*1024);
			mapped[DMA_READ_PARAMS]=(2<<8) | 127;
			mapped[DMA_CONTROL]=DMA_READ_BLOCK;
			mapped[DMA_CONTROL]=0;
			for(i=0;i<256;i++)
			{
				mapped[DMA_CONTROL]=DMA_READ_DEQUEUE;
				dataarray[i]=mapped[DMA_READ_DATA];
				////printf("\n %d",dataarray[i]);
				mapped[DMA_CONTROL]=0;
			}
			writeSize=fwrite((const void *)dataarray,sizeof(int),256,fp);
			//printf("\n write %d iteration %d",writeSize,j);
			mapped[DMA_CONTROL]=0;
		}
		fclose(fp);
	}
	else
	{
		//printf("\n %s",p3);
	}
	return 0;
}
int WriteDMA(int Address,char *FileName)
{
	FILE *fp;
	int i,j,blocks,readSize,Length;
	int dataarray[256];
	fp=fopen(FileName, "rb");
	if(fp!=0)
	{
		
		fseek(fp, 0, SEEK_END);    /* file pointer at the end of file */
		Length = ftell(fp);   /* take a position of file pointer size variable */
		fseek(fp, 0, SEEK_SET);    /* file pointer at the beginning of file */
		mapped[DMA_CONTROL]=DMA_RESET_ALL;
		mapped[DMA_CONTROL]=0;
		blocks=Length/256;
		for(j=0;j<blocks;j++) // only do full blocks inside loop
		{	
			if(readSize=fread((const void *)dataarray,sizeof(int),256,fp)==256)
			{
				mapped[DMA_WRITE_ADDRESS]=Address+(j*1024);
				mapped[DMA_WRITE_PARAMS]=(2<<8) | 127;
				mapped[DMA_CONTROL]=0;
				for(i=0;i<256;i++)
				{
					mapped[DMA_WRITE_DATA]=dataarray[i];
					mapped[DMA_CONTROL]=DMA_WRITE_ENQUEUE;
					mapped[DMA_CONTROL]=0;
				}
				mapped[DMA_CONTROL]=DMA_WRITE_INITIATE;
				mapped[DMA_CONTROL]=0;
				////printf("\n write %d iteration %d",readSize,j);
			}
		}
		fclose(fp);
		//printf("\n Table Loaded: Length %d",Length);
	}
	else
	{
	    //printf("errno = %d.\n", errno);
		//perror(errno);
		//printf("\n failed to open %s",FileName);
	}
	return Length;
}

int CaptureADtoFile(int Axis,int Start,int Length,int Delay,char *FileName)
{
	FILE *fp;
	int i,j,k,blocks,writeSize,AvgSIN,AvgCOS,ADVal;
	int dataarray[512];
	switch(Axis)
	{
	    case 0  :
		MoveRobot(Start,0,0,0,0,BLOCKING_MOVE);
		break; 
	    case 1  :
		MoveRobot(0,Start,0,0,0,BLOCKING_MOVE);
		break; 
	    case 2  :
		MoveRobot(0,0,Start,0,0,BLOCKING_MOVE);
		break; 
	    case 3  :
		MoveRobot(0,0,0,Start,0,BLOCKING_MOVE);
		break; 
	    case 4  :
		MoveRobot(0,0,0,0,Start,BLOCKING_MOVE);
		break; 
	}
	fp=fopen(FileName, "w");
	if(fp!=0)
	{
		blocks=Length/256;
		for(k=0;k<blocks;k++) // only do full blocks inside loop
		{	
			for(i=0;i<256;i++)
			{
				mapped[Axis]=Start+i+(k*256);
				AvgSIN=0;
				AvgCOS=0;
				for(j=0;j<Delay;j++)
				{	
					ADVal=mapped[ADLookUp[Axis]];
					AvgSIN=AvgSIN+ADVal;
					AvgCOS=AvgCOS+mapped[ADLookUp[Axis]+1];
				}			
				dataarray[i*2]=AvgSIN/Delay;
				dataarray[(i*2)+1]=AvgCOS/Delay;
//				//printf("\n SIN %d COS %d",dataarray[i*2],dataarray[(i*2)+1]);
			}
			writeSize=fwrite((const void *)dataarray,sizeof(int),512,fp);
			//printf("\n write %d iteration %d",writeSize,k);
		}
		fclose(fp);
	}
	else
	{
		//printf("\n %s",FileName);
	}
	return 0;
}
int FindHome(int Axis,int Start,int Length,int Delay,char *FileName)
{
	FILE *fp;
	int i,j,k,blocks,AvgSIN,AvgCOS,ADVal,fileLen,ReadSize,MinSIN,MinCOS,MinSINIdx=0,MinCOSIdx=0;
	int RefrenceArray[1000000];
	int CaptureArray[Length*2];
	switch(Axis)
	{
	    case 0  :
		MoveRobot(Start,0,0,0,0,BLOCKING_MOVE);
		break; 
	    case 1  :
		MoveRobot(0,Start,0,0,0,BLOCKING_MOVE);
		break; 
	    case 2  :
		MoveRobot(0,0,Start,0,0,BLOCKING_MOVE);
		break; 
	    case 3  :
		MoveRobot(0,0,0,Start,0,BLOCKING_MOVE);
		break; 
	    case 4  :
		MoveRobot(0,0,0,0,Start,BLOCKING_MOVE);
		break; 
	}
	fp=fopen(FileName, "rb");
	if(fp!=0)
	{
		fseek(fp, 0, SEEK_END);
		fileLen=ftell(fp);
		fseek(fp, 0, SEEK_SET);
		ReadSize=fread((char *)RefrenceArray, fileLen, 1, fp);
		fclose(fp);
	}
	else
	{
		//printf("\n %s",FileName);
		return 1;
	}
	for(k=0;k<Length;k++) 
	{	
		mapped[Axis]=Start+k;
		AvgSIN=0;
		AvgCOS=0;
		for(j=0;j<Delay;j++)
		{	
			ADVal=mapped[ADLookUp[Axis]];
			AvgSIN=AvgSIN+ADVal;
			AvgCOS=AvgCOS+mapped[ADLookUp[Axis]+1];
		}			
		CaptureArray[k*2]=AvgSIN/Delay;
		CaptureArray[(k*2)+1]=AvgCOS/Delay;
//		//printf("\n Query %d Refrence %d",CaptureArray[k*2],RefrenceArray[k*2]);
	}
	MoveRobot(0,0,0,0,0,BLOCKING_MOVE);
	blocks=fileLen/8;
	MinSIN=0x7fffffff;
	MinCOS=0x7fffffff;
	for(i=0;i<blocks;i++)
	{
		AvgSIN=0;
		AvgCOS=0;
		for(k=0;k<Length;k++)
		{
			AvgSIN=AvgSIN+abs(CaptureArray[k*2]-RefrenceArray[(k+i)*2]);
			AvgSIN=AvgSIN+abs(CaptureArray[(k*2)+1]-RefrenceArray[(((k+i)*2)+1)]);
//			AvgCOS=AvgCOS+abs(CaptureArray[(k*2)+1]-RefrenceArray[(((k+i)*2)+1)]);
		}
		if(MinSIN>AvgSIN)
		{
			MinSIN=AvgSIN;
			MinSINIdx=i;
		}
/*		if(MinCOS>AvgCOS)
		{
			MinCOS=AvgCOS;
			MinCOSIdx=i;
		}*/
	}
	//printf("\n SAD %d SINidx %d\n",MinSIN,MinSINIdx);
	switch(Axis)
	{
	    case 0  :
		MoveRobot(30000-MinSINIdx,0,0,0,0,BLOCKING_MOVE);
		break; 
	    case 1  :
		MoveRobot(0,30000-MinSINIdx,0,0,0,BLOCKING_MOVE);
		break; 
	    case 2  :
		MoveRobot(0,0,30000-MinSINIdx,0,0,BLOCKING_MOVE);
		break; 
	    case 3  :
		MoveRobot(0,0,0,30000-MinSINIdx,0,BLOCKING_MOVE);
		break; 
	    case 4  :
		MoveRobot(0,0,0,0,30000-MinSINIdx,BLOCKING_MOVE);
		break; 
	}
	mapped[COMMAND_REG]=256;  //reset home
	mapped[COMMAND_REG]=0;
	return 30000-MinSINIdx;
}


void showPosAt(void)
{
		int b1,b2,b3,b4,b5;
		b1=getNormalizedInput(BASE_POSITION_AT);
		b3=getNormalizedInput(END_POSITION_AT);
		b2=getNormalizedInput(PIVOT_POSITION_AT);
		b4=getNormalizedInput(ANGLE_POSITION_AT);
		b5=getNormalizedInput(ROT_POSITION_AT);
		//printf("\nPos %d %d %d %d %d  ",b1,b2,b3,b4,b5);	
}
void ReplayMovement(char *FileName)
{
	int Length,rbc;
	showPosAt();
	Length=WriteDMA(0x3f000000,FileName);
	mapped[RECORD_LENGTH]=Length/4;
	mapped[REC_PLAY_TIMEBASE]=1;
	mapped[REC_PLAY_CMD]=CMD_RESET_RECORD;
	mapped[REC_PLAY_CMD]=CMD_RESET_PLAY;
	mapped[REC_PLAY_CMD]=0;
	mapped[REC_PLAY_CMD]=CMD_PLAYBACK;
	rbc=mapped[READ_BLOCK_COUNT];
	showPosAt();
	
	//sleep(1);
	while((mapped[READ_BLOCK_COUNT] & 0x003fffff) != 0 )
	{
		if(rbc != mapped[READ_BLOCK_COUNT])
		{
				
			//printf("%d \n",mapped[READ_BLOCK_COUNT] & 0x003fffff);
			rbc=mapped[READ_BLOCK_COUNT];
			showPosAt();
		}
		////printf("%d \n",mapped[READ_BLOCK_COUNT]);
	}
	while((mapped[READ_BLOCK_COUNT] & 0x00400000) != 0 )
		//printf("%d \n",mapped[READ_BLOCK_COUNT]);
	showPosAt();
	mapped[REC_PLAY_CMD]=CMD_RESET_RECORD;
	mapped[REC_PLAY_CMD]=CMD_RESET_PLAY;
	mapped[REC_PLAY_CMD]=0;

	/*fa0=fa0+mapped[PLAYBACK_BASE_POSITION];
	fa1=fa1+mapped[PLAYBACK_END_POSITION];
	fa2=fa2+mapped[PLAYBACK_PIVOT_POSITION];
	fa3=fa3+mapped[PLAYBACK_ANGLE_POSITION];
	fa4=fa4+mapped[PLAYBACK_ROT_POSITION];
	mapped[FINE_ADJUST_BASE]=fa0;
	mapped[FINE_ADJUST_END]=fa1;
	mapped[FINE_ADJUST_PIVOT]=fa2;
	mapped[FINE_ADJUST_ANGLE]=fa3;
	mapped[FINE_ADJUST_ROT]=fa4;*/
	mapped[REC_PLAY_CMD]=CMD_RESET_RECORD;
}
int getInput(void)
{
	char iString[255];
	if(gets(iString)!=NULL)
	{
		return ParseInput(iString);
	}
	
}
int ParseInput(char *iString)
{
	//char iString[255];
	const char delimiters[] = " ,";
	char *token,*p1,*p2,*p3,*p4,*p5;
	int i,j,Add,Start,Length,Delay,Axis,tokenVal;
	////printf("\nStart wait Goal");
	if(iString !=NULL)
	{
		token = strtok (iString, delimiters);
		if (token !=NULL)
		{
			tokenVal=HashInputCMD(token);
			////printf("Token %s TokenVal %i",token,tokenVal);
		}
		else
			return 1;
		if (tokenVal != 0 ){
			switch(tokenVal)
			{	
				case PID_FINEMOVE :
					p1=strtok (NULL, delimiters);
					p2=strtok (NULL, delimiters);
					p3=strtok (NULL, delimiters);
					p4=strtok (NULL, delimiters);
					p5=strtok (NULL, delimiters);
					moverobotPID(atoi(p1),atoi(p2),atoi(p3),atoi(p4),atoi(p5));
				break; 
				case HEART_BEAT :
				break; 
			
				case SET_FORCE_MOVE_POINT :
					p1=strtok (NULL, delimiters);
					p2=strtok (NULL, delimiters);
					p3=strtok (NULL, delimiters);
					p4=strtok (NULL, delimiters);
					p5=strtok (NULL, delimiters);
					ForceDestination[0]=atoi(p1);
					ForceDestination[1]=atoi(p2);
					ForceDestination[2]=atoi(p3);
					ForceDestination[3]=atoi(p4);
					ForceDestination[4]=atoi(p5);
				break; 
	
				case SEND_HEARTBEAT :
					////printf("heartbeat dispatched\n");
				break; 
				case SET_PARAM :
					p1=strtok (NULL, delimiters);
					p2=strtok (NULL, delimiters);
	//				p3=strtok (NULL, delimiters);
	//				p4=strtok (NULL, delimiters);
	//				p5=strtok (NULL, delimiters);
					SetParam(p1,atof(p2),0,0,0);
				break; 
				case MOVEALL_RELATIVE :
					p1=strtok (NULL, delimiters);
					p2=strtok (NULL, delimiters);
					p3=strtok (NULL, delimiters);
					p4=strtok (NULL, delimiters);
					p5=strtok (NULL, delimiters);
					MoveRobotRelative(atoi(p1),atoi(p2),atoi(p3),atoi(p4),atoi(p5),BLOCKING_MOVE);
				break; 
				case REPLAY_MOVEMENT  :
					p1=strtok (NULL, delimiters);
					ReplayMovement(p1);
				break; 
				case SLEEP_CMD  :
					p1=strtok (NULL, delimiters);
					usleep(atoi(p1));
				break; 
				case MOVE_CMD  :
					p1=strtok (NULL, delimiters);
					p2=strtok (NULL, delimiters);
				break; 
				case LOAD_TABLES  :
					WriteDMA(0x3f800000,"/srv/samba/share/a0tbl.dta");
					WriteDMA(0x3f900000,"/srv/samba/share/a1tbl.dta");
					WriteDMA(0x3fa00000,"/srv/samba/share/a2tbl.dta");
				break; 
				case CAPTURE_POINTS_CMD :
					p1=strtok (NULL, delimiters);
					InitCapturePoints(p1);
				break;
				case RECORD_MOVEMENT :
					p1=strtok (NULL, delimiters);
					InitCaptureMovement(p1);
				break;
				case DMAREAD_CMD :
					p1=strtok (NULL, delimiters);
					p2=strtok (NULL, delimiters);
					p3=strtok (NULL, delimiters);
					Add=(int)strtol(p1,0,16);
					Length=(int)strtol(p2,0,16);
					ReadDMA(Add,Length,p3);
				break;
				case DMAWRITE_CMD :
					p1=strtok (NULL, delimiters);
					p2=strtok (NULL, delimiters);
					Add=(int)strtol(p1,0,16);
					WriteDMA(Add,p2);
				break;
				case CAPTURE_AD_CMD :
					p1=strtok (NULL, delimiters);
					p2=strtok (NULL, delimiters);
					p3=strtok (NULL, delimiters);
					p4=strtok (NULL, delimiters);
					p5=strtok (NULL, delimiters);
					Axis=atoi(p1);
					Start=atoi(p2);
					Length=atoi(p3);
					Delay=atoi(p4);
					CaptureADtoFile(Axis,Start,Length,Delay,p5);
				break;
				case FIND_HOME_REP_CMD :
					p1=strtok (NULL, delimiters);
					Axis=atoi(p1);
					Start=0;
					Length=5000;
					Delay=10000;
					switch(Axis)
					{
						case 0  :
							while(abs(FindHome(Axis,Start,Length,Delay,"/srv/samba/share/d0.dta"))>300);
						break; 
						case 1  :
							while(abs(FindHome(Axis,Start,Length,Delay,"/srv/samba/share/d1.dta"))>300);
						break; 
						case 2  :
							while(abs(FindHome(Axis,Start,Length,Delay,"/srv/samba/share/d2.dta"))>300);
						break; 
						case 3  :
							while(abs(FindHome(Axis,Start,Length,Delay,"/srv/samba/share/d3.dta"))>300);
						break; 
						case 4  :
							while(abs(FindHome(Axis,Start,Length,Delay,"/srv/samba/share/d4.dta"))>300);
						break; 
					}
				break; 
					case FIND_HOME_CMD :
					p1=strtok (NULL, delimiters);
					p2=strtok (NULL, delimiters);
					p3=strtok (NULL, delimiters);
					p4=strtok (NULL, delimiters);
					p5=strtok (NULL, delimiters);
					Axis=atoi(p1);
					Start=atoi(p2);
					Length=atoi(p3);
					Delay=atoi(p4);
					FindHome(Axis,Start,Length,Delay,p5);
				break; 
				case FIND_INDEX_CMD :
					p1=strtok (NULL, delimiters);
					p2=strtok (NULL, delimiters);
					p3=strtok (NULL, delimiters);
					p4=strtok (NULL, delimiters);
					Axis=atoi(p1);
					Start=atoi(p2);
					Length=atoi(p3);
					Delay=atoi(p4);
					if(FindIndex(Axis,Start,Length,Delay)==0)
					{
						//printf("/nIndex reached");
						break;
					}
					else
					{
						//printf("/nIndex not found");
					}
				break; 
				case MOVEALL_CMD :
					p1=strtok (NULL, delimiters);
					p2=strtok (NULL, delimiters);
					p3=strtok (NULL, delimiters);
					p4=strtok (NULL, delimiters);
					p5=strtok (NULL, delimiters);
					if(p1!=NULL && p2!=NULL && p3!=NULL && p4!=NULL && p5!=NULL)						
						MoveRobot(atoi(p1),atoi(p2),atoi(p3),atoi(p4),atoi(p5),BLOCKING_MOVE);
				break; 
				case SLOWMOVE_CMD  :
					p1=strtok (NULL, delimiters);
					p2=strtok (NULL, delimiters);
					p3=strtok (NULL, delimiters);
					p4=strtok (NULL, delimiters);
					p5=strtok (NULL, delimiters);
					Add=atoi(p1);
					Length=atoi(p3);
					Start=atoi(p2);
					Delay=atoi(p4);
					mapped[Add]=Start;
					//printf("\n %d %d \n",Add,Length);
					for(i=0;i<Length;i++)
					{
						for(j=0;j<Delay;j++)			
							mapped[Add]=Start+i;
					}
				break;
				case READ_CMD  :
					p1=strtok (NULL, delimiters);
					p2=strtok (NULL, delimiters);
					if((p1==NULL) | (p2==NULL))
					{
						//printf("\n %d %d need addres and length",atoi(p1),atoi(p2));
						break;
					}
					Add=atoi(p1);
					Length=atoi(p2); 
					//printf("\n %d %d \n",Add,Length);
					for(i=0;i<Length;i++)
					{
						
						//printf("\n  %x %d",CalTables[Add+i],CalTables[Add+i]);
						//printf("\n %x ",mapped[Add+i]);
					}
				break; 
				case WRITE_CMD  :
					p1=strtok (NULL, delimiters);
					p2=strtok (NULL, delimiters);
					if((p1==NULL) | (p2==NULL))
					{
						//printf("\n %d %d need addres and data",atoi(p1),atoi(p2));
					}
					//printf("\n %d %d \n",atoi(p1),atoi(p2));
					i=atoi(p1);
					j=atoi(p2);
					if(i==COMMAND_REG)
						CmdVal=j;
					mapped[i]=j;
					if(i==ACCELERATION_MAXSPEED)
					{
						maxSpeed=j & 0b00000000000011111111111111111111;
						coupledAcceleration=(j & 0b00000011111100000000000000000000) >> 20;
						//startSpeed=0,
						
					}
				break; 
				case EXIT_CMD  :
					mapped[FINE_ADJUST_BASE]=0;
					mapped[FINE_ADJUST_END]=0;
					mapped[FINE_ADJUST_PIVOT]=0;
					mapped[FINE_ADJUST_ANGLE]=0;
					mapped[FINE_ADJUST_ROT]=0;
					return 1;
				break; 
				
		
				
				default : 
				  //printf("\nunrecognized command");
				  break;
			}
			return 0;
		}
		else
			return 1;
	}
	return 1;
}

int main(int argc, char *argv[]) {

  int fd,mfd,err;
  
  int size;
  int DefaultMode;
  int CalTblSize = 32*1024*1024; 


  if (argc != 4) {
    fprintf(stderr, "Usage: %s Needs init mode, Master/Slave and RunMode\n", argv[0]);
    exit(1);
  }

  DefaultMode = atoi(argv[1]);
  size = 4095;
  RunMode=atoi(argv[3]);
  ServerMode = atoi(argv[2]);
  if (size <= 0) {
    fprintf(stderr, "Bad size: %d\n", size);
    exit(1);
  }

  fd = open("/dev/uio1", O_RDWR);
  if (fd < 0) {
    perror("Failed to open devfile");
    return 1;
  }

  map_addr = mmap( NULL, size, PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0);

  if (map_addr == MAP_FAILED) {
    perror("Failed to mmap");
    return 1;
  }

  mapped = map_addr;
  
  
  mfd = open("/dev/mem", O_RDWR);
  map_addrCt = mmap(NULL, CalTblSize, PROT_READ | PROT_WRITE, MAP_SHARED, mfd, 0x3f000000);


   if (map_addrCt == MAP_FAILED) {
		printf("\nCant Map Callibration Tables \n");
    perror("Failed to mmap High memory");
    return 1;
  }
  CalTables = map_addrCt;


//  Addr= = atoi(argv[3]);
//  Dta= = atoi(argv[4]);
//  mapped[Addr] = Dta;
	setDefaults(DefaultMode);
//	if(DefaultMode ==2 )
	if(ServerMode==1)
	{
	
		err = pthread_create(&(tid[0]), NULL, &StartServerSocket,  (void*)&ThreadsExit);
		if (err != 0)
		{
			//printf("\ncan't create thread :[%s]", strerror(err));
			return 1;
		}
		else
		{
			//printf("\n Begin Socket Server Thread\n");
		}
	}
	else if(ServerMode==2)
	{
	
		err = pthread_create(&(tid[1]), NULL, &StartClientSocket, NULL);
		if (err != 0)
		{
			//printf("\ncan't create thread :[%s]", strerror(err));
			return 1;
		}
		else
		{
			//printf("\n Begin Socket Client Thread\n");
		}
	}
	else if(ServerMode==3)
	{
		if(mapped[SENT_BASE_POSITION]!=0)
		{
	   		munmap(map_addr, size);
			munmap(map_addrCt, CalTblSize);
 			close(fd);
			close(mfd);
			return 0;   
		}
		mapped[BASE_POSITION]=1;

		MoveRobot(0,0,0,50000,50000,BLOCKING_MOVE);
	    MoveRobot(0,0,0,0,0,BLOCKING_MOVE);
	    MoveRobot(0,0,0,50000,-50000,BLOCKING_MOVE);
	    MoveRobot(0,0,0,0,0,BLOCKING_MOVE);
	    MoveRobot(0,0,0,50000,50000,BLOCKING_MOVE);
	    MoveRobot(0,0,0,0,0,BLOCKING_MOVE);
	    MoveRobot(0,0,0,50000,-50000,BLOCKING_MOVE);
	    MoveRobot(0,0,0,0,0,BLOCKING_MOVE);
		
		err = pthread_create(&(tid[0]), NULL, &StartServerSocketDDE,  (void*)&ThreadsExit);
		if (err != 0)
		{
			//printf("\ncan't create thread :[%s]", strerror(err));
			return 1;
		}
		else
		{
			//printf("\n Begin Socket Server Thread For DDE\n");
		}
	}
	if(RunMode==1 || RunMode==2)
	{
		err = pthread_create(&(tid[2]), NULL, &RealtimeMonitor, (void*)&ThreadsExit );
		if (err != 0)
		{
			//printf("\ncan't create thread :[%s]", strerror(err));
			return 1;
		}
		else
		{
			//printf("\n Begin Realtime Monitor Thread\n");
		}		
	}

	
	
    if(ServerMode==3)
	{
		while(1){} //loop forever
	}
	while(getInput()==0);
	ThreadsExit=0;
	sleep(1);


	//printf("\nExiting \n");

  munmap(map_addr, size);
  munmap(map_addrCt, CalTblSize);
  

  close(fd);
   close(mfd);
  return 0;
}
GNU General Public License v3.0