{"id":23870109,"url":"https://github.com/engineermichael/-robotic-arm---haddington-dynamics-robotics-engineering-","last_synced_at":"2025-09-08T20:32:10.162Z","repository":{"id":260214017,"uuid":"119200731","full_name":"EngineerMichael/-Robotic-Arm---Haddington-Dynamics-Robotics-Engineering-","owner":"EngineerMichael","description":"⎔ Automation in 3D-Printed Robotics","archived":false,"fork":false,"pushed_at":"2024-12-22T18:25:13.000Z","size":4125,"stargazers_count":4,"open_issues_count":0,"forks_count":0,"subscribers_count":3,"default_branch":"master","last_synced_at":"2024-12-22T19:33:39.535Z","etag":null,"topics":["3d-printing","ai","automation","c","fpga-programming","fpga-soc","javascript","orcad","pcb","robotic-arm","software","software-engineering","ui-design"],"latest_commit_sha":null,"homepage":"","language":"C","has_issues":true,"has_wiki":null,"has_pages":null,"mirror_url":null,"source_name":null,"license":"gpl-3.0","status":null,"scm":"git","pull_requests_enabled":true,"icon_url":"https://github.com/EngineerMichael.png","metadata":{"files":{"readme":"README.md","changelog":null,"contributing":null,"funding":null,"license":"LICENSE","code_of_conduct":null,"threat_model":null,"audit":null,"citation":null,"codeowners":null,"security":null,"support":null,"governance":null,"roadmap":null,"authors":null,"dei":null,"publiccode":null,"codemeta":null}},"created_at":"2018-01-27T20:38:39.000Z","updated_at":"2024-12-22T18:25:16.000Z","dependencies_parsed_at":"2024-10-30T07:22:36.445Z","dependency_job_id":null,"html_url":"https://github.com/EngineerMichael/-Robotic-Arm---Haddington-Dynamics-Robotics-Engineering-","commit_stats":null,"previous_names":["engineermichael/-robotic-arm---haddington-dynamics-robotics-engineering-"],"tags_count":0,"template":false,"template_full_name":null,"repository_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/EngineerMichael%2F-Robotic-Arm---Haddington-Dynamics-Robotics-Engineering-","tags_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/EngineerMichael%2F-Robotic-Arm---Haddington-Dynamics-Robotics-Engineering-/tags","releases_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/EngineerMichael%2F-Robotic-Arm---Haddington-Dynamics-Robotics-Engineering-/releases","manifests_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/EngineerMichael%2F-Robotic-Arm---Haddington-Dynamics-Robotics-Engineering-/manifests","owner_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/owners/EngineerMichael","download_url":"https://codeload.github.com/EngineerMichael/-Robotic-Arm---Haddington-Dynamics-Robotics-Engineering-/tar.gz/refs/heads/master","host":{"name":"GitHub","url":"https://github.com","kind":"github","repositories_count":232346051,"owners_count":18509029,"icon_url":"https://github.com/github.png","version":null,"created_at":"2022-05-30T11:31:42.601Z","updated_at":"2022-07-04T15:15:14.044Z","host_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub","repositories_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories","repository_names_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repository_names","owners_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/owners"}},"keywords":["3d-printing","ai","automation","c","fpga-programming","fpga-soc","javascript","orcad","pcb","robotic-arm","software","software-engineering","ui-design"],"created_at":"2025-01-03T13:52:17.922Z","updated_at":"2025-01-03T13:52:18.609Z","avatar_url":"https://github.com/EngineerMichael.png","language":"C","funding_links":[],"categories":[],"sub_categories":[],"readme":"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).\n\n3D-Printed Robotic Arm Automation\n\nOverview\n\nThis 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.\n\nFeatures\n\t•\tC Programming for Embedded Systems: Low-level control for motor movement and sensor feedback.\n\t•\tJavaScript for High-Level Automation: Logic for automating tasks like object manipulation.\n\t•\tCustom IDE: An intuitive environment tailored for both C and JavaScript development.\n\t•\t3D-Printed Hardware: The robotic arm is fabricated with 3D printing for ease of customization and prototyping.\n\nRequirements\n\nHardware:\n\t•\t3D-printed robotic arm with actuators and sensors (such as servo motors, stepper motors, and distance sensors).\n\t•\tMicrocontroller (e.g., Arduino, ESP32, or Raspberry Pi) for C programming execution.\n\t•\tPower supply for the robotic arm and microcontroller.\n\nSoftware:\n\t•\tC Compiler (e.g., GCC or a microcontroller-specific compiler).\n\t•\tNode.js and npm for JavaScript execution.\n\t•\tCustom IDE for coding, compiling, and uploading programs to the microcontroller.\n\t•\tServo Controller (if applicable).\n\nLibraries/Dependencies:\n\t•\twiringPi (or equivalent) for GPIO control (if using Raspberry Pi or similar).\n\t•\tJohnny-Five JavaScript library for robotics with Node.js (if using a Node.js server).\n\t•\tSerial Communication: Communication between the IDE and microcontroller for real-time updates.\n\nSetup Instructions\n\n1. Install the Custom IDE\n\t1.\tClone the repository:\n\ngit clone https://github.com/yourusername/3d-printed-robotic-arm.git\ncd 3d-printed-robotic-arm\n\n\n\t2.\tFollow the installation guide for the custom IDE:\n\t•\tWindows: Download the installer from the releases page.\n\t•\tmacOS/Linux: Follow the installation instructions in the INSTALL.md file.\n\t3.\tLaunch the IDE:\n\n./ide --start\n\n\n\n2. Setup the Microcontroller\n\t•\tConnect the microcontroller (e.g., Arduino) to your computer via USB.\n\t•\tOpen 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.\n\n3. Running the JavaScript Automation\n\t•\tEnsure that Node.js is installed:\n\nnode -v\nnpm -v\n\n\n\t•\tInstall necessary dependencies:\n\nnpm install\n\n\n\t•\tStart the automation script:\n\nnode automation.js\n\n\n\nConfiguration\n\nMicrocontroller Pin Setup\n\nThe following pin mappings are used for controlling the motors:\n\n#define MOTOR_PIN_1 3    // Motor 1 (Base Rotation)\n#define MOTOR_PIN_2 5    // Motor 2 (Shoulder)\n#define MOTOR_PIN_3 6    // Motor 3 (Elbow)\n#define MOTOR_PIN_4 9    // Motor 4 (Wrist)\n#define MOTOR_PIN_5 10   // Motor 5 (Gripper)\n#define SENSOR_PIN_1 A0  // Distance Sensor 1\n#define SENSOR_PIN_2 A1  // Distance Sensor 2\n\nJavaScript Automation Script Example\n\nThe automation.js file controls the robotic arm to perform a simple pick-and-place operation:\n\nconst five = require(\"johnny-five\");\nconst board = new five.Board();\n\nboard.on(\"ready\", function() {\n  let arm = new five.Robot({\n    pins: [3, 5, 6, 9, 10], // Motor pin assignments\n    invert: [false, false, false, false, false]\n  });\n\n  let gripper = new five.Servo(10);\n  let elbow = new five.Servo(6);\n\n  // Example automation: Move arm to position 1\n  arm.move([90, 0, 0, 90, 0]);\n\n  // Pick object\n  gripper.to(30); // Close gripper\n  elbow.to(45);   // Move arm to angle\n\n  // Move to new position\n  arm.move([0, 45, 45, 45, 0]);\n  gripper.to(90); // Open gripper\n});\n\nSource Code Structure\n\n/3d-printed-robotic-arm\n|-- /src\n|   |-- robot-arm.c            # C code for microcontroller control\n|   |-- automation.js          # JavaScript automation script\n|-- /docs\n|   |-- INSTALL.md             # Installation guide for the IDE\n|   |-- CONFIG.md              # Configuration details\n|-- /hardware\n|   |-- arm-design.stl         # 3D printable STL files for the robotic arm\n|   |-- arm-assembly-guide.pdf # Assembly guide\n|-- /ide\n|   |-- custom-ide-source-code # Source code for the custom IDE\n\nTesting \u0026 Troubleshooting\n\t•\tMotor not moving: Check power connections and ensure correct pin assignments.\n\t•\tAutomation not working: Make sure the communication between the microcontroller and the IDE is functioning (e.g., check serial connection).\n\t•\tGripper malfunctioning: Verify the servo calibration and ensure that the gripper.to(angle) command corresponds to the correct position.\n\nContributing\n\nWe 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.\n\nLicense\n\nThis project is licensed under the MIT License - see the LICENSE.md file for details.\n\nFeel free to adjust the details based on specific setup and components.\n# Dexter\nSoftware and FPGA code and PCB for Dexter the robot\n\n\n#include \u003cstddef.h\u003e\n#include \u003cstdio.h\u003e\n#include \u003cunistd.h\u003e\n#include \u003cstdlib.h\u003e\n#include \u003csys/types.h\u003e\n#include \u003csys/stat.h\u003e\n#include \u003cfcntl.h\u003e\n#include \u003ctime.h\u003e\n#include \u003csignal.h\u003e\n#include \u003cerrno.h\u003e\n#include \u003cstring.h\u003e\n#include \u003csys/mman.h\u003e\n#include \u003cpthread.h\u003e\n#include \u003ctermios.h\u003e\n#include \u003cinttypes.h\u003e\n\n\n#include \u003csys/socket.h\u003e\n#include \u003cnetinet/in.h\u003e\n#include \u003carpa/inet.h\u003e\n\n\n\n\n\n\n#define INPUT_OFFSET 11\n\n// Motor position index\n#define BASE_POSITION 0\n#define END_POSITON 1\n#define PIVOT_POSITON 2\n#define ANGLE_POSITON 3\n#define ROT_POSITON 4\n\n#define ACCELERATION_MAXSPEED 5\n\n// DC bias paramaters\n#define BASE_SIN_CENTER 6\n#define BASE_COS_CENTER 7\n#define END_SIN_CENTER 8\n#define END_COS_CENTER 9\n#define PIVOT_SIN_CENTER 10\n#define PIVOT_COS_CENTER 11\n#define ANGLE_SIN_CENTER 12\n#define ANGLE_COS_CENTER 13\n#define ROT_SIN_CENTER 14\n#define ROT_COS_CENTER 15\n\n//PID VALUES\n#define PID_DELTATNOT 16\n#define PID_DELTAT 17\n#define PID_D 18\n#define PID_I 19\n#define PID_P 20\n#define PID_ADDRESS 21\n\n//FORCE PARAMATERS\n#define BOUNDRY_BASE 22\n#define BOUNDRY_END 23\n#define BOUNDRY_PIVOT 24\n\n#define BOUNDRY_ANGLE 25\n#define BOUNDRY_ROT 26\n\n#define SPEED_FACTORA 27\n#define SPEED_FACTORB 28\n\n#define FRICTION_BASE 29\n#define FRICTION_END 30\n#define FRICTION_PIVOT 31\n#define FRICTION_ANGLE 32\n#define FRICTION_ROT 33\n\n#define MOVE_TRHESHOLD 34\n#define F_FACTOR 35\n#define MAX_ERROR 36\n\n#define FORCE_BIAS_BASE 37\n#define FORCE_BIAS_END 38\n#define FORCE_BIAS_PIVOT 39\n#define FORCE_BIAS_ANGLE 40\n#define FORCE_BIAS_ROT 41\n\n// control state register\n#define COMMAND_REG 42\n#define CMD_CAPCAL_BASE 1\n#define CMD_CAPCAL_END 2\n#define CMD_CAPCAL_PIVOT 4\n#define CMD_MOVEEN 8\n#define CMD_MOVEGO 16\n#define CMD_ENABLE_LOOP 32\n#define CMD_CLEAR_LOOP 64\n#define CMD_CALIBRATE_RUN 128\n#define CMD_RESET_POSITION 256\n#define CMD_RESET_FORCE 512\n#define CMD_CAPCAL_ANGLE 1024\n#define CMD_CAPCAL_ROT 2048\n#define CMD_ANGLE_ENABLE 4096\n#define CMD_ROT_ENABLE 8196\n\n\n\n\n\n\n\n//DMA \n#define DMA_READ_ADDRESS 48\n#define DMA_READ_PARAMS 47\n#define DMA_WRITE_ADDRESS 46\n#define DMA_WRITE_PARAMS 45\n#define DMA_WRITE_DATA 44\n#define DMA_CONTROL 43\n\n// DMA control bits breakdown\n\n#define DMA_WRITE_ENQUEUE 1\n#define DMA_WRITE_INITIATE 2\n#define DMA_READ_DEQUEUE 4\n#define DMA_READ_BLOCK 8\n#define DMA_RESET_ALL 16\n\n#define REC_PLAY_CMD 49\n#define CMD_RESET_RECORD 1\n#define CMD_RECORD 2\n#define CMD_RESET_PLAY 4\n#define CMD_PLAYBACK 8\n#define CMD_RESET_PLAY_POSITION 16\n\n\n\n#define REC_PLAY_TIMEBASE 50\n#define DIFF_FORCE_TIMEBASE 51\n#define DIFF_FORCE_BETA 52\n#define DIFF_FORCE_MOVE_THRESHOLD 53\n#define DIFF_FORCE_MAX_SPEED 54\n#define DIFF_FORCE_SPEED_FACTOR_ANGLE 55\n#define DIFF_FORCE_SPEED_FACTOR_ROT 56\n#define DIFF_FORCE_ANGLE_COMPENSATE 57\n\n#define FINE_ADJUST_BASE 58\n#define FINE_ADJUST_END 59\n#define FINE_ADJUST_PIVOT 60\n#define FINE_ADJUST_ANGLE 61\n#define FINE_ADJUST_ROT 62\n\n#define RECORD_LENGTH 63\n\n\n\n#define END_EFFECTOR_IO 64\n#define SERVO_SETPOINT_A 65\n#define SERVO_SETPOINT_B 66\n\n#define BASE_FORCE_DECAY 67\n#define END_FORCE_DECAY 68\n#define PIVOT_FORCE_DECAY 69\n#define ANGLE_FORCE_DECAY 70\n#define ROTATE_FORCE_DECAY 71\n\n#define PID_SCHEDULE_INDEX 72\n\n#define GRIPPER_MOTOR_CONTROL 73\n#define GRIPPER_MOTOR_ON_WIDTH 75\n#define GRIPPER_MOTOR_OFF_WIDTH 74\n#define START_SPEED 76\n#define ANGLE_END_RATIO 77\n\n\n\n\n\n// DMA DATA IN\n\n#define DMA_READ_DATA 30 + INPUT_OFFSET\n\n\n\n\n\n// READ REGISTER DEFINITIONS\n\n// POSITION REPORT\n#define BASE_POSITION_AT 0 + INPUT_OFFSET\n#define END_POSITION_AT 1 + INPUT_OFFSET\n#define PIVOT_POSITION_AT 2 + INPUT_OFFSET\n#define ANGLE_POSITION_AT 3 + INPUT_OFFSET\n#define ROT_POSITION_AT 4 + INPUT_OFFSET\n\n//TABLE CALCULATED DELTA\n\n#define BASE_POSITION_DELTA 5 + INPUT_OFFSET\n#define END_POSITION_DELTA 6 + INPUT_OFFSET\n#define PIVOT_POSITION_DELTA 7 + INPUT_OFFSET\n#define ANGLE_POSITION_DELTA 8 + INPUT_OFFSET\n#define ROT_POSITION_DELTA 9 + INPUT_OFFSET\n\n\n//PID CALCULATED DELTA\n\n#define BASE_POSITION_PID_DELTA 10 + INPUT_OFFSET\n#define END_POSITION_PID_DELTA 11 + INPUT_OFFSET\n#define PIVOT_POSITION_PID_DELTA 12 + INPUT_OFFSET\n#define ANGLE_POSITION_PID_DELTA 13 + INPUT_OFFSET\n#define ROT_POSITION_PID_DELTA 14 + INPUT_OFFSET\n\n\n// FORCE CALCULATED POSITION MODIFICATION\n\n#define BASE_POSITION_FORCE_DELTA 15 + INPUT_OFFSET\n#define END_POSITION_FORCE_DELTA 16 + INPUT_OFFSET\n#define PIVOT_POSITION_FORCE_DELTA 17 + INPUT_OFFSET\n#define ANGLE_POSITION_FORCE_DELTA 18 + INPUT_OFFSET\n#define ROT_POSITION_FORCE_DELTA 19 + INPUT_OFFSET\n\n\n// RAW ANALOG TO DIGITAL VALUES\n\n#define BASE_SIN 20 + INPUT_OFFSET\n#define BASE_COS 21 + INPUT_OFFSET\n#define END_SIN 22 + INPUT_OFFSET\n#define END_COS 23 + INPUT_OFFSET\n#define PIVOT_SIN 24 + INPUT_OFFSET\n#define PIVOT_COS 25 + INPUT_OFFSET\n#define ANGLE_SIN 26 + INPUT_OFFSET\n#define ANGLE_COS 27 + INPUT_OFFSET\n#define ROT_SIN 28 + INPUT_OFFSET\n#define ROT_COS 29 + INPUT_OFFSET\n\n// RECORD AND PLAYBACK \n\n#define RECORD_BLOCK_SIZE 31 + INPUT_OFFSET\n#define READ_BLOCK_COUNT 32 + INPUT_OFFSET\n#define PLAYBACK_BASE_POSITION 33 + INPUT_OFFSET\n#define PLAYBACK_END_POSITION 34 + INPUT_OFFSET\n#define PLAYBACK_PIVOT_POSITION 35 + INPUT_OFFSET\n#define PLAYBACK_ANGLE_POSITION 36 + INPUT_OFFSET\n#define PLAYBACK_ROT_POSITION 37 + INPUT_OFFSET\n\n\n#define END_EFFECTOR_IO_IN 38 + INPUT_OFFSET\n\n#define SENT_BASE_POSITION 39 + INPUT_OFFSET\n#define SENT_END_POSITION 40 + INPUT_OFFSET\n#define SENT_PIVOT_POSITION 41 + INPUT_OFFSET\n#define SENT_ANGLE_POSITION 42 + INPUT_OFFSET\n#define SENT_ROT_POSITION 43 + INPUT_OFFSET\n\n#define SLOPE_BASE_POSITION 44 + INPUT_OFFSET\n#define SLOPE_END_POSITION 45 + INPUT_OFFSET\n#define SLOPE_PIVOT_POSITION 46 + INPUT_OFFSET\n#define SLOPE_ANGLE_POSITION 47 + INPUT_OFFSET\n#define SLOPE_ROT_POSITION 48 + INPUT_OFFSET\n#define CMD_FIFO_STATE 49 + INPUT_OFFSET\n\n\n\n\n\n\n\n\nint ADLookUp[5] = {BASE_SIN,END_SIN,PIVOT_SIN,ANGLE_SIN,ROT_SIN};\n\n//commands\n\n#define MOVE_CMD 1\n#define READ_CMD 2\n#define WRITE_CMD 3\n#define EXIT_CMD 4\n#define SLOWMOVE_CMD 5\n#define MOVEALL_CMD 6\n#define DMAREAD_CMD 7\n#define DMAWRITE_CMD 8\n#define CAPTURE_AD_CMD 9\n#define FIND_HOME_CMD 10\n#define FIND_HOME_REP_CMD 11\n#define LOAD_TABLES 12\n#define CAPTURE_POINTS_CMD 14\n#define FIND_INDEX_CMD 15\n#define SLEEP_CMD 16\n#define RECORD_MOVEMENT 17\n#define REPLAY_MOVEMENT 18\n#define MOVEALL_RELATIVE 19\n#define SET_PARAM 20\n#define SEND_HEARTBEAT 21\n#define SET_FORCE_MOVE_POINT 22\n#define HEART_BEAT 23\n#define PID_FINEMOVE 24\n\n\n// modes\n#define BLOCKING_MOVE 1\n#define NON_BLOCKING_MOVE 2\n#define DEFAULT_MOVE_TIMEOUT 0\n#define TRUE 1\n#define FALSE 0\n\n\n// defaults\n#define DEFAULT_PID_SETTING_XYZ 0x3dcCCCCC\n//#define DEFAULT_PID_SETTING_XYZ 0x3f000000\n//#define DEFAULT_PID_SETTING_XYZ 0x3f800000\n//#define DEFAULT_PID_SETTING_PY 0x3f000000\n//#define DEFAULT_PID_SETTING_PY 0x3D4CCCCC\n//#define DEFAULT_PID_SETTING_PY 0x3dcCCCCC\n#define DEFAULT_PID_SETTING_PY 0x3cf5c28f\n\n\n#define DEF_SPEED_FACTOR_A 30\n#define DEF_SPEED_FACTOR_DIFF 8\n\n#define ACCELERATION_MAXSPEED_DEF 250000+(3\u003c\u003c20)\n\n#define BASE_SIN_LOW 0X760\n#define BASE_COS_LOW 0X1\n#define BASE_SIN_HIGH 0x83A\n#define BASE_COS_HIGH 0X40\n\n#define END_SIN_LOW 0Xc01\n#define END_COS_LOW 0X0\n#define END_SIN_HIGH 0xd5c\n#define END_COS_HIGH 0X70\n\n#define PIVOT_SIN_LOW 0X50\n#define PIVOT_COS_LOW 0X880\n#define PIVOT_SIN_HIGH 0x100\n#define PIVOT_COS_HIGH 0X8d1\n\n#define ANGLE_SIN_LOW 0X900\n#define ANGLE_COS_LOW 0X1\n#define ANGLE_SIN_HIGH 0x9A0\n#define ANGLE_COS_HIGH 0X50\n\n#define ROT_SIN_LOW 0XA90\n#define ROT_COS_LOW 0X1\n#define ROT_SIN_HIGH 0xcc2\n#define ROT_COS_HIGH 0X50\n\n\n#define DEFAULT_ANGLE_BOUNDRY_HI 3000\n#define DEFAULT_ANGLE_BOUNDRY_LOW -3000\n\n#define SERVO_LOW_BOUND 1142000\n#define SERVO_HI_BOUND 1355000\n\n\n#define bool int\n#define TRUE 1\n#define FALSE 0\n\nint XLowBound[5]={BASE_COS_LOW,END_COS_LOW,PIVOT_COS_LOW,ANGLE_COS_LOW,ROT_COS_LOW};\nint XHighBound[5]={BASE_COS_HIGH,END_COS_HIGH,PIVOT_COS_HIGH,ANGLE_COS_HIGH,ROT_COS_HIGH};\nint YLowBound[5]={BASE_SIN_LOW,END_SIN_LOW,PIVOT_SIN_LOW,ANGLE_SIN_LOW,ROT_SIN_LOW};\nint YHighBound[5]={BASE_SIN_HIGH,END_SIN_HIGH,PIVOT_SIN_HIGH,ANGLE_SIN_HIGH,ROT_SIN_HIGH};\nint ForcePossition[5]={0,0,0,0,0};\nint ForceDestination[5]={0,0,0,0,0};\nint ThreadsExit=1;\nint StatusReportIndirection[60]={DMA_READ_DATA,DMA_READ_DATA,RECORD_BLOCK_SIZE,END_EFFECTOR_IO_IN,BASE_POSITION_AT,BASE_POSITION_DELTA,\n\t\t\t\t\t\t\t\tBASE_POSITION_PID_DELTA,BASE_POSITION_FORCE_DELTA,BASE_SIN,BASE_COS,PLAYBACK_BASE_POSITION,SENT_BASE_POSITION,\n\t\t\t\t\t\t\t\tSLOPE_BASE_POSITION,0,PIVOT_POSITION_AT,PIVOT_POSITION_DELTA,PIVOT_POSITION_PID_DELTA,PIVOT_POSITION_FORCE_DELTA,\n\t\t\t\t\t\t\t\tPIVOT_SIN,PIVOT_COS,PLAYBACK_PIVOT_POSITION,SENT_PIVOT_POSITION,SLOPE_PIVOT_POSITION,0,END_POSITION_AT,\n\t\t\t\t\t\t\t\tEND_POSITION_DELTA,END_POSITION_PID_DELTA,END_POSITION_FORCE_DELTA,END_SIN,END_COS,PLAYBACK_END_POSITION,\n\t\t\t\t\t\t\t\tSENT_END_POSITION,SLOPE_END_POSITION,0,ANGLE_POSITION_AT,ANGLE_POSITION_DELTA,ANGLE_POSITION_PID_DELTA,\n\t\t\t\t\t\t\t\tANGLE_POSITION_FORCE_DELTA,ANGLE_SIN,ANGLE_COS,PLAYBACK_ANGLE_POSITION,SENT_ANGLE_POSITION,SLOPE_ANGLE_POSITION,\n\t\t\t\t\t\t\t\t0,ROT_POSITION_AT,ROT_POSITION_DELTA,ROT_POSITION_PID_DELTA,ROT_POSITION_FORCE_DELTA,ROT_SIN,ROT_COS,\n\t\t\t\t\t\t\t\tPLAYBACK_ROT_POSITION,SENT_ROT_POSITION,SLOPE_ROT_POSITION,0};\n\nvoid *map_addr,*map_addrCt;\nvolatile unsigned int *mapped;\nvolatile unsigned int *CalTables;\nvolatile unsigned int *RecordTable;\nint CmdVal=0,maxSpeed=0,startSpeed=0,coupledAcceleration=0,forceMode=0,fa0=0,fa1=0,fa2=0,fa3=0,fa4=0,RunMode=0,ServerMode=3;\nint BaseBoundryHigh,BaseBoundryLow,EndBoundryHigh,EndBoundryLow,PivotBoundryHigh,PivotBoundryLow,AngleBoundryHigh=DEFAULT_ANGLE_BOUNDRY_HI,AngleBoundryLow=DEFAULT_ANGLE_BOUNDRY_LOW,RotateBoundryHigh,RotateBoundryLow;\nint Cycle = 0;\nstruct BotPossition {\n\tint base;\n\tint end;\n\tint pivot;\n\tint angle;\n\tint rotate;\n\tint baseForce;\n\tint endForce;\n\tint pivotForce;\n\tint angleForce;\n\tint rotateForce;\n\tint Controlled;\n\tint ForceMoveState;\n};\nstruct BotSetHomePossition {\n\tint base;\n\tint end;\n\tint pivot;\n\tint angle;\n\tint rotate;\n};\nchar RemoteRobotAdd[255];\nstruct MasterSlaveMoveRatio{\n\tint base;\n\tint end;\n\tint pivot;\n\tint angle;\n\tint rotate;\t\n};\nstruct MasterSlaveMoveDelta{\n\tint base;\n\tint end;\n\tint pivot;\n\tint angle;\n\tint rotate;\t\n};\nstruct SlaveBotPossiton{\n\tint base;\n\tint end;\n\tint pivot;\n\tint angle;\n\tint rotate;\t\n};\nfloat DiffCorrectionFactor;\nint AngleHIBoundry;\nint AngleLOWBoundry;\nint controlled=0;\n\nint DexError=0;\n\nstruct MasterSlaveMoveRatio MSMoveRatio = {1,1,1,1,1};\nstruct MasterSlaveMoveDelta MSMoveDelta = {0,0,0,0,0};\nstruct BotSetHomePossition SetHome = {0,0,0,0,0};\nstruct SlaveBotPossiton SlaveBotPos = {0,0,0,0,0};\npthread_t tid[10];\nstruct CaptureArgs {\n   char *FileName;\n   FILE *fp;\n};\nstruct CaptureArgs CptA,CptMove;\n#define MAX_PARAMS 26\n#define PARAM_LENGTH 20\nchar 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\"};\n\n\nstatic struct termios old, new;\n\nint PositionAdjust[5]={0,0,0,0,0};\n\nint ForceLimit[5]={4400,4400,4400,800,800};\n\nint MyBotForce[5]={0,0,0,0,0};\n\nfloat ForceAdjustPossition[5]={-.01,-.01,-.01,-.01,-.01};\nint FroceMoveMode=0;\n\nint LastGoal[5]={0,0,0,0,0};\n\n\nfloat JointsCal[5];\nint SoftBoundries[5];\n\nvoid printPosition()\n{\n\tint a1,a2,a3,a4,a5;\n\ta1=getNormalizedInput(BASE_POSITION_AT)+getNormalizedInput(BASE_POSITION_FORCE_DELTA);\n\ta2=getNormalizedInput(END_POSITION_AT)+getNormalizedInput(END_POSITION_FORCE_DELTA);\n\ta3=getNormalizedInput(PIVOT_POSITION_AT)+getNormalizedInput(PIVOT_POSITION_FORCE_DELTA);\n\ta4=getNormalizedInput(ANGLE_POSITION_AT)+getNormalizedInput(ANGLE_POSITION_FORCE_DELTA);\n\ta5=getNormalizedInput(ROT_POSITION_AT)+getNormalizedInput(ROT_POSITION_FORCE_DELTA);\n\t//printf(\" %d,%d,%d,%d,%d\\n\",a1,a2,a3,a4,a5);\n\n}\n\nint sign(int i)\n{\n\tif(i\u003c0)\n\t\treturn -1;\n\tif(i\u003e0)\n\t\treturn 1;\n\treturn 0;\n}\n\nvoid RealtimeMonitor(void *arg)\n{\n\tint* ExitState = arg;\n\tint i,j,ForceDelta,disTime=0;\n\twhile(*ExitState)\n\t{\n\t\tif(FroceMoveMode==1)\n\t\t{\n\t\n\t\n\t\t\t\n\t\t\t// do force based movement\n\t\t\tfor(i=0;i\u003c5;i++)\n\t\t\t{\n\t\t\t\tForceDelta=ForcePossition[i]-getNormalizedInput(BASE_POSITION_FORCE_DELTA+i);\n\t\t\t\t/*if(abs(ForceDelta)\u003e500)\n\t\t\t\t{\n\t\t\t\t\tForceDelta=(abs(ForceDelta)-500)*sign(ForceDelta);\n\t\t\t\t}*/\n\t\t\t\t//ForceDelta=ForceDelta;//+MyBotForce[i];\n\t\t\t\tif(disTime==90)\n\t\t\t\t{\n\t\t\t\t\t//for(j=0;j\u003c5;j++)\n\t\t\t\t\t//printf(\" Force %d \",ForceDelta);\n\t\t\t\t}\n\t\t\t\tif(abs(ForceDelta)\u003cForceLimit[i])\n\t\t\t\t{\n\t\t\t\t\tmapped[FORCE_BIAS_BASE+i]=ForceDelta;\n\t\t\t\t}\n\t\t\t\telse\n\t\t\t\t{\n\t\t\t\t\tmapped[FORCE_BIAS_BASE+i]=sign(ForceDelta)*ForceLimit[i];\n\t\t\t\t}\n\t\t\t\t//ForcePossition[i]=ForcePossition[i]+(int)((float)ForceDelta*ForceAdjustPossition[i]);\n\t\t\t\t/*if(ForceDestination[i]\u003eForcePossition[i])\n\t\t\t\t{\n\t\t\t\t\tForcePossition[i]=ForcePossition[i]+10;\n\t\t\t\t}\n\t\t\t\telse\n\t\t\t\t{\n\t\t\t\t\tForcePossition[i]=ForcePossition[i]-10;\n\t\t\t\t}*/\n\t\t\t}\n\t\t}\n\t\tdisTime++;\n\t\tif(disTime\u003e100)\n\t\t{\n\t\t\t//printPosition();\n\t\t\tdisTime = 0;\n\t\t}\n\n\t\tusleep(1000);\n\t}\n\t//printf(\"\\nMonitor Thread Exiting\\n\");\n}\nvoid SetGripperRoll(int Possition)\n{\n\tint ServoSpan=(SERVO_HI_BOUND-SERVO_LOW_BOUND)/360;\n\tmapped[END_EFFECTOR_IO]=64;\n\n\tmapped[SERVO_SETPOINT_B]=(ServoSpan*Possition)+SERVO_LOW_BOUND;\n//\tmapped[SERVO_SETPOINT_B]=Possition;\n\t\n}\nvoid SetGripperMotor(int state)\n{\n\tmapped[GRIPPER_MOTOR_ON_WIDTH]=12000;\n\tmapped[GRIPPER_MOTOR_OFF_WIDTH]=200000;\n\tmapped[GRIPPER_MOTOR_CONTROL]=state;\n}\nvoid StartServerSocketDDE(void *arg)\n{\n\tstruct timeval tv;\n\ttv.tv_sec = 30;  /* 30 Secs Timeout */\n\ttv.tv_usec = 0;  // Not init'ing this can cause strange errors\n\n\tint* ExitState = arg;\n\tint listenfd = 0, connfd = 0,RLength = 0,SLength = 0;\n    struct sockaddr_in serv_addr; \n\n    char sendBuff[256];\n    char recBuff[256];\n    time_t ticks; \n\tbool SocketLive=0;\n\t//printf(\"\\n Start DDE Socket Server \\n\");\n\n    listenfd = socket(AF_INET, SOCK_STREAM, 0);\n    memset(\u0026serv_addr, '0', sizeof(serv_addr));\n    memset(sendBuff, '0', sizeof(sendBuff)); \n\n    serv_addr.sin_family = AF_INET;\n    serv_addr.sin_addr.s_addr = htonl(INADDR_ANY);\n    serv_addr.sin_port = htons(50000); \n\n    bind(listenfd, (struct sockaddr*)\u0026serv_addr, sizeof(serv_addr)); \n\n    listen(listenfd, 10); \n\t\n\tsetsockopt(listenfd, SOL_SOCKET, SO_RCVTIMEO, (char *)\u0026tv,sizeof(struct timeval));\n    while(*ExitState)\n    {\n        // connfd = accept(listenfd, (struct sockaddr*)NULL, NULL);\n        while ( (connfd = accept(listenfd, (struct sockaddr*)NULL, NULL)) \u003c 0)\n\t\t{\n\t\t\t//printf(\"error on accept()! %s\\n\", strerror(errno));\n\t\t\t////printf(\"\\n Timeout no connect \\n\");\n\t\t} \n\t\t//printf(\"\\n new connect \\n\");\n\t\t\n\t\t\n\t\t//ticks = time(NULL);\n\t\t//strcpy(sendBuff,\"Dexter Connect Service Connected\\n\");\n\t\t//sn//printf(sendBuff, sizeof(sendBuff), \"%.24s\\r\\n\", ctime(\u0026ticks));\n\t\t//write(connfd, sendBuff, strlen(sendBuff)); \n\t\tSocketLive=TRUE;\n//\t\twhile(SocketLive==TRUE)\n\t\t{\n\t\t\twhile((errno = 0, (RLength = recv(connfd, recBuff, /*sizeof(recBuff)*/128, 0))\u003e0) || \n\t\t\terrno == EINTR)\n\t\t\t{\n\t\t\t\tif(RLength\u003e0)\n\t\t\t\t{\n\t\t\t\t\t////printf(\"Receive size %i \",RLength);\n\t\t\t\t\t//output.append(recBuff, RLength);\n\t\t\t\t}\n\t\t\t\t\t\n\t\t\t \n\n\t\t\t\tif(RLength \u003c 0)\n\t\t\t\t{\n\t\t\t\t\t//printf(\"%s \\n\",strerror(errno));\n\t\t\t\t\t/* handle error - for example throw an exception*/\n\t\t\t\t\tSocketLive = FALSE;\n\t\t\t\t}\n\t\t\t\t\n\n//\t\t\twhile ( (RLength = recv (connfd,recBuff,sizeof(recBuff),0 )) \u003e 0)\n\t\t\t\t{\n\t\t\t\t\t//recBuff[RLength]=0;\n\t\t\t\t\tProcessServerReceiveDataDDE(recBuff);\n\t\t\t\t\n\t\t\t\t\tProcessServerSendDataDDE(sendBuff,recBuff);/*==TRUE)*/\n\t\t\t\t\t\twrite (connfd,sendBuff,60*4/*sizeof(sendBuff)*/); \n\t\t\t\t}\n\t\t\t}\n\t\t\t//printf(\"error code %s \\n\",strerror(errno));\t\t\t\n\t\t}\n\t\t//printf(\"\\n Socket Read Zero closing socket\\n\");\n        close(connfd);\n        //sleep(1);\n     }\n\n}\nvoid StartServerSocket(void *arg)\n{\n\tint* ExitState = arg;\n\tint listenfd = 0, connfd = 0,RLength = 0,SLength = 0;\n    struct sockaddr_in serv_addr; \n\n    char sendBuff[64];\n    char recBuff[64];\n    time_t ticks; \n\tbool SocketLive=0;\n\n    listenfd = socket(AF_INET, SOCK_STREAM, 0);\n    memset(\u0026serv_addr, '0', sizeof(serv_addr));\n    memset(sendBuff, '0', sizeof(sendBuff)); \n\n    serv_addr.sin_family = AF_INET;\n    serv_addr.sin_addr.s_addr = htonl(INADDR_ANY);\n    serv_addr.sin_port = htons(40000); \n\n    bind(listenfd, (struct sockaddr*)\u0026serv_addr, sizeof(serv_addr)); \n\n    listen(listenfd, 10); \n\n    while(*ExitState)\n    {\n        connfd = accept(listenfd, (struct sockaddr*)NULL, NULL); \n\t\t//printf(\"\\n new connect \\n\");\n\t\t\n\t\t//ticks = time(NULL);\n\t\tstrcpy(sendBuff,\"Dexter Connect Service Connected\\n\");\n\t\t//sn//printf(sendBuff, sizeof(sendBuff), \"%.24s\\r\\n\", ctime(\u0026ticks));\n\t\twrite(connfd, sendBuff, strlen(sendBuff)); \n\t\tSocketLive=TRUE;\n\t\t\n\t\twhile(SocketLive==TRUE)\n\t\t{\n\t\t\twhile ( (RLength = recv (connfd,recBuff,sizeof(recBuff),0 )) \u003e 0)\n\t\t\t{\n\t\t\t\t//recBuff[RLength]=0;\n\t\t\t\tProcessServerReceiveData(recBuff);\n\t\t\t\tSocketLive = ProcessServerSendData(sendBuff);\n\t\t\t\twrite (connfd,sendBuff,sizeof(sendBuff)); \n\t\t\t}\n\t\t}\n\t\t//printf(\"\\n Socket Read Zero closing socket\\n\");\n        close(connfd);\n       \n    }\n\n}\nint MaxForce(int Max,int Val)\n{\n\tif(abs(Max) \u003e abs(Val))\n\t{\n\t\treturn Val;\n\t}\n\telse\n\t{\n\t\treturn abs(Max)*sign(Val);\n\t}\n}\nbool ProcessServerReceiveData(char *recBuff)\n{\n\tstruct BotPossition MyBot;\n\tint MxForce=9800;\n\tfloat fScale=1;\n\tmemcpy(\u0026MyBot,recBuff,sizeof(MyBot));\n\t////printf(\"Server %d %d %d %d %d \\n\",MyBot.baseForce,MyBot.endForce,MyBot.pivotForce,MyBot.angleForce,MyBot.rotateForce);\n\tif(MyBot.Controlled!=0)\n\t{\n\t\t/*MyBotForce[0]=MaxForce(MxForce,MyBot.baseForce);\n\t\tMyBotForce[1]=MaxForce(MxForce,MyBot.endForce);\n\t\tMyBotForce[2]=MaxForce(MxForce,MyBot.pivotForce);\n\t\tMyBotForce[3]=MaxForce(MxForce,MyBot.angleForce);\n\t\tMyBotForce[4]=MaxForce(MxForce,MyBot.rotateForce);*/\n\t\tif(FroceMoveMode==0)\n\t\t{\n\t\t\tmapped[FORCE_BIAS_BASE]=MaxForce(MxForce,(int)((float)MyBot.baseForce)*fScale);\n\t\t\tmapped[FORCE_BIAS_END]=MaxForce(MxForce,(int)((float)MyBot.endForce)*fScale);\n\t\t\tmapped[FORCE_BIAS_PIVOT]=MaxForce(MxForce,(int)((float)MyBot.pivotForce)*fScale);\n\t\t\tmapped[FORCE_BIAS_ANGLE]=MaxForce(MxForce,(int)((float)MyBot.angleForce)*fScale);\n\t\t\tmapped[FORCE_BIAS_ROT]=MaxForce(MxForce,(int)((float)MyBot.rotateForce)*fScale);\n\t\t}\n\t\telse\n\t\t{\n\t\t\tForcePossition[0]=MyBot.base;\n\t\t\tForcePossition[1]=MyBot.end;\n\t\t\tForcePossition[2]=MyBot.pivot;\n\t\t\tForcePossition[3]=MyBot.angle;\n\t\t\tForcePossition[4]=MyBot.rotate;\n\t\t}\n\t\tFroceMoveMode=MyBot.ForceMoveState;\n\t\t\n\t}\n\t\n\t//MoveRobot(MyBot.base,MyBot.end,MyBot.pivot,MyBot.angle,MyBot.rotate,BLOCKING_MOVE);\n/*\tCycle++;\n\tif(Cycle\u003e100)\n\t\treturn FALSE;\n\telse\t\n\t\treturn TRUE;*/\n}\nbool ProcessServerSendData(char *sendBuff)\n{\n\t\n\tstruct BotPossition MyBot={0,0,0,0,0,0,0,0,0,0,0,0};\n\tstatic int Cycle = 0;\n\tMyBot.baseForce=getNormalizedInput(BASE_POSITION_DELTA);\n\tMyBot.endForce=getNormalizedInput(END_POSITION_DELTA);\n\tMyBot.pivotForce=getNormalizedInput(PIVOT_POSITION_DELTA);\n\tMyBot.angleForce=getNormalizedInput(ANGLE_POSITION_DELTA);\n\tMyBot.rotateForce=getNormalizedInput(ROT_POSITION_DELTA);\n\tMyBot.Controlled=controlled;\n\tMyBot.base=getNormalizedInput(BASE_POSITION_FORCE_DELTA);\n\tMyBot.end=getNormalizedInput(END_POSITION_FORCE_DELTA);\n\tMyBot.pivot=getNormalizedInput(PIVOT_POSITION_FORCE_DELTA);\n\tMyBot.angle=getNormalizedInput(ANGLE_POSITION_FORCE_DELTA);\n\tMyBot.rotate=getNormalizedInput(ROT_POSITION_FORCE_DELTA);\n\tMyBot.ForceMoveState=FroceMoveMode;\n\tmemcpy(sendBuff,\u0026MyBot,sizeof(MyBot));\n//\t\n//\ts//printf(sendBuff, \"%d \\n\", Cycle);\n//\tCycle++;\n\treturn TRUE;\n}\nbool ProcessServerSendDataDDE(char *sendBuff,char *recBuff)\n{\n\tint i;\n\tint *sendBuffReTyped;\n\tconst char delimiters[] = \" ,\";\n\tchar *token;\n\n/*\tfor(i=0;i\u003c25;i++)\n\t{\n\t\t//printf(\"%c\",recBuff[i]);\n\t}*/\n//\tif(recBuff[0]==\"g\")\n\ttoken = strtok (recBuff, delimiters);\n\n\t{\n\t\t////printf(\"returning heartbeat\\n\");\n\t\tsendBuffReTyped=(int *)sendBuff;\n\t\tsendBuffReTyped[0]=atoi(token);\n\t\ttoken = strtok (NULL, delimiters);\n\t\t////printf(\"\\n %s \\n\",token);\n\t\tsendBuffReTyped[1]=atoi(token);\n\t\ttoken = strtok (NULL, delimiters);\n\t\ttoken = strtok (NULL, delimiters);\n\t\ttoken = strtok (NULL, delimiters);\n\t\tsendBuffReTyped[2]=0;//   this shoud be start_time_internal broken into 2 words atoi(token);\n\t\tsendBuffReTyped[3]=0;//   this shoud be start_time_internal broken into 2 words atoi(token);\n\t\tsendBuffReTyped[4]=token[0];\n\t\tsendBuffReTyped[5]=DexError;\n\n\n\n\t\t//sendBuffReTyped[1]=token[0];\n\t\t//sendBuffReTyped[2]=DexError;\n\t\t\n\t\tfor(i=0;i\u003c59;i++)\n\t\t{\n\t\t\tsendBuffReTyped[i+6]=getNormalizedInput(StatusReportIndirection[i]);\n\t\t\t/*if(StatusReportIndirection[i] == BASE_POSITION_PID_DELTA)\n\t\t\t{\n\t\t\t\t//printf(\" PID Delta for L1 = %d\", sendBuffReTyped[i+6]);\n\t\t\t}\n\t\t\tif(StatusReportIndirection[i] == BASE_POSITION_DELTA)\n\t\t\t{\n\t\t\t\t//printf(\" TABLE OFFSET Delta for L1 = %d\", sendBuffReTyped[i+6]);\n\t\t\t}*/\n\t\t}\n\t\treturn TRUE;\n\t}\n\treturn FALSE;\n}\nbool ProcessServerReceiveDataDDE(char *recBuff)\n{\n\tstruct BotPossition MyBot;\n\tchar CmdString[255];\n\tint i,FoundStart=0,j=0;\n\tbool GotDelim=FALSE;\n\tint *sendBuffReTyped;\n\tconst char delimiters[] = \" ,\";\n\tchar *token;\n\t\n\t//memcpy(\u0026MyBot,recBuff,sizeof(MyBot));\n\t////printf(\"Server %d %d %d %d %d \\n\",MyBot.base,MyBot.end,MyBot.pivot,MyBot.angle,MyBot.rotate);\n\t//mapped[FINE_ADJUST_BASE]=MyBot.base;\n\t//mapped[FINE_ADJUST_END]=MyBot.end;\n\t//mapped[FINE_ADJUST_PIVOT]=MyBot.pivot;\n\t//mapped[FINE_ADJUST_ANGLE]=MyBot.angle;\n\t//mapped[FINE_ADJUST_ROT]=MyBot.rotate;\n\t\n\t//MoveRobot(MyBot.base,MyBot.end,MyBot.pivot,MyBot.angle,MyBot.rotate,BLOCKING_MOVE);\n\tFoundStart=0;\n\tfor(i=0;i\u003c255;i++)\n\t{\n\t\tif(FoundStart \u003e= 4)\n\t\t{\n\t\t\tCmdString[j]=recBuff[i];\n\t\t\tj++;\n\t\t}\n\t\tif(recBuff[i]==' ')\n\t\t{\n\t\t\tFoundStart++;\n\t\t}\n\t\tif(recBuff[i]==0x3b)\n\t\t{\n\t\t\trecBuff[i]=0;\n\t\t\tGotDelim=TRUE;\n\t\t\tbreak;\n\t\t}\n\t}\n\tif(GotDelim==FALSE)\n\t{\n\t\tDexError=2;\n\t\t////printf(\"%s\",recBuff);\n\t\treturn FALSE;\n\t}\n\tCmdString[j-1]=0;\n\tif(CmdString[0] != 'g')\n\t{\n\t\t//printf(\"\\n%s \\n\",recBuff);\n\t\t//printf(\"\\n%s \\n\",CmdString);\n\t\t\n\t}\n\t////printf(\"\\n%s \\n\",recBuff);\n\t////printf(\"\\n%s \\n\",CmdString);\n\tDexError=ParseInput(CmdString); \n////printf(\"\\nsent parse\\n\"); \n\treturn TRUE;\n}\n\nvoid StartClientSocket()\n{\n    int sockfd = 0, n = 0,j;\n    char recvBuff[64];\n    char sendBuff[64];\n    struct sockaddr_in serv_addr; \n\tint Cycle = 100;\n\tstruct BotPossition MyBot;\n\tfloat DiffDelta;\n\tfloat ForceScale=0;\n\n    memset(recvBuff, '0',sizeof(recvBuff));\n    if((sockfd = socket(AF_INET, SOCK_STREAM, 0)) \u003c 0)\n    {\n        //printf(\"\\n Error : Could not create socket \\n\");\n        return 1;\n    } \n\n    memset(\u0026serv_addr, '0', sizeof(serv_addr)); \n\n    serv_addr.sin_family = AF_INET;\n    serv_addr.sin_port = htons(40000); \n\tRemoteRobotAdd[strcspn(RemoteRobotAdd,\"\\n\")]= 0;\n\n    if(inet_pton(AF_INET, RemoteRobotAdd/*\"192.168.1.145\"*/, \u0026serv_addr.sin_addr)\u003c=0)\n    {\n        //printf(\"\\n inet_pton error occured\\n\");\n        return 1;\n    } \n    if( connect(sockfd, (struct sockaddr *)\u0026serv_addr, sizeof(serv_addr)) \u003c 0)\n    {\n       //printf(\"\\n Error : Connect Failed \\n\");\n       return 1;\n    } \n//\t//printf(\"\\n Socket connect\\n\");\n//\twhile(1)//for(j=0;j\u003c1000;j++)\n\t{\n\t\t////printf(\"\\n next\\n\");\n\t\twhile( (n = recv(sockfd, recvBuff, sizeof(recvBuff),0 ) ) \u003e 0)\n\t\t{\n\t\t\t//recvBuff[n] = 0;\n\t\t\t////printf(\"Socket Read\\n\");\n\t\t\t////printf(\"%s \",recvBuff);\n\n\t\t\tif(n \u003c 0)\n\t\t\t{\n\t\t\t\t//printf(\"\\n Read error \\n\");\n\t\t\t} \n\t\t\t/*mapped[FORCE_BIAS_BASE]=(int)(((float)MyBot.base)*ForceScale);\n\t\t\tmapped[FORCE_BIAS_END]=(int)(((float)MyBot.end)*ForceScale);\n\t\t\tmapped[FORCE_BIAS_PIVOT]=(int)(((float)MyBot.pivot)*ForceScale);\n\t\t\tmapped[FORCE_BIAS_ANGLE]=(int)(((float)MyBot.angle)*ForceScale);\n\t\t\tmapped[FORCE_BIAS_ROT]=(int)(((float)MyBot.rotate)*ForceScale);*/\n\t\t\t////printf(\"%d %d %d %d %d \\n\",MyBot.base,MyBot.end,MyBot.pivot,MyBot.angle,MyBot.rotate);\n#ifdef FOLLOW_POSSITION\n\t\t\tmemcpy(\u0026MyBot,recvBuff,sizeof(MyBot));\n\t\t\tDiffDelta=1.5;\n\t\t\tMyBot.base=SlaveBotPos.base + ((GetAxisCurrent(0) - SetHome.base)/MSMoveRatio.base);\n\t\t\tMyBot.end=SlaveBotPos.end + ((GetAxisCurrent(1) - SetHome.end)/MSMoveRatio.end);\n\t\t\tMyBot.pivot=SlaveBotPos.pivot + ((GetAxisCurrent(2) - SetHome.pivot)/MSMoveRatio.pivot);\n\t\t\tMyBot.angle=SlaveBotPos.angle + (((GetAxisCurrent(3) - SetHome.angle)/MSMoveRatio.angle)*DiffDelta);\n\t\t\tMyBot.rotate=SlaveBotPos.rotate + (((GetAxisCurrent(4) - SetHome.rotate)/MSMoveRatio.rotate)*DiffDelta);\n/*\t\t\tMyBot.base=0;\n\t\t\tMyBot.end=0;\n\t\t\tMyBot.pivot=0;\n\t\t\tMyBot.angle=0;\n\t\t\tMyBot.rotate=0;*/\n\n\t\t\t/*\n\t\t\tSlaveBotPos.base=MyBot.base;\n\t\t\tSlaveBotPos.end=MyBot.base;\n\t\t\tSlaveBotPos.pivot=MyBot.base;\n\t\t\tSlaveBotPos.angle=MyBot.base;\n\t\t\tSlaveBotPos.rotate=MyBot.base;\n\t\t\t*/\n\t\t\t\n\t\t\t//usleep(25000);\n\n\t\t\t/*MyBot.base=BASE_POSITION_FORCE_DELTA;\n\t\t\tMyBot.end=12300;\n\t\t\tMyBot.pivot=5123;\n\t\t\tMyBot.angle=10;\n\t\t\tMyBot.rotate=15;*/\n\t\t\tmemcpy(sendBuff,\u0026MyBot,sizeof(MyBot));\n\t\t\tCycle++;\n\n#else\n\t\t\tProcessServerReceiveData(recvBuff);\n\t\t\tProcessServerSendData(sendBuff);\n\t\t\t\n#endif\t\t\t\n\t\t\twrite(sockfd,sendBuff,sizeof(sendBuff));\n\t\t}\n\t}\n}\nint GetAxisCurrent(int Axis)\n{\n\treturn getNormalizedInput(BASE_POSITION_AT+Axis)+getNormalizedInput(BASE_POSITION_FORCE_DELTA+Axis);\n}\n/* Initialize new terminal i/o settings */\nvoid initTermios(int echo) \n{\n  tcgetattr(0, \u0026old); /* grab old terminal i/o settings */\n  new = old; /* make new settings same as old settings */\n  new.c_lflag \u0026= ~ICANON; /* disable buffered i/o */\n  new.c_lflag \u0026= echo ? ECHO : ~ECHO; /* set echo mode */\n  tcsetattr(0, TCSANOW, \u0026new); /* use these new terminal i/o settings now */\n}\n\n/* Restore old terminal i/o settings */\nvoid resetTermios(void) \n{\n  tcsetattr(0, TCSANOW, \u0026old);\n}\n\n/* Read 1 character - echo defines echo mode */\nchar getch_(int echo) \n{\n  char ch;\n  initTermios(echo);\n  ch = getchar();\n  resetTermios();\n  return ch;\n}\n\n/* Read 1 character without echo */\nchar getch(void) \n{\n  return getch_(0);\n}\n\n/* Read 1 character with echo */\nchar getche(void) \n{\n  return getch_(1);\n}\n\nint FindIndex(int Axis,int Start,int Length,int Delay)\n{\n\tint i,j,k,ADVal,AvgCOS,AvgSIN;\n\tswitch(Axis)\n\t{\n\t    case 0  :\n\t\tMoveRobot(Start,0,0,0,0,BLOCKING_MOVE);\n\t\tbreak; \n\t    case 1  :\n\t\tMoveRobot(0,Start,0,0,0,BLOCKING_MOVE);\n\t\tbreak; \n\t    case 2  :\n\t\tMoveRobot(0,0,Start,0,0,BLOCKING_MOVE);\n\t\tbreak; \n\t    case 3  :\n\t\tMoveRobot(0,0,0,Start,0,BLOCKING_MOVE);\n\t\tbreak; \n\t    case 4  :\n\t\tMoveRobot(0,0,0,0,Start,BLOCKING_MOVE);\n\t\tbreak; \n\t}\n\tfor(k=0;k\u003cabs(Length);k++) \n\t{\t\n\t\tif(Length\u003e0)\n\t\t\tmapped[Axis]=Start+k;\n\t\telse\t\n\t\t\tmapped[Axis]=Start-k;\n\t\tfor(j=0;j\u003cDelay;j++)\n\t\t{\t\n\t\t\tADVal=mapped[ADLookUp[Axis]];\n\t\t\tAvgSIN=AvgSIN+ADVal;\n\t\t\tAvgCOS=AvgCOS+mapped[ADLookUp[Axis]+1];\n\t\t}\t\n\t\tAvgSIN=AvgSIN/Delay;\n\t\tAvgCOS=AvgCOS/Delay;\n\t\t////printf(\"\\n SIN %x COS %x\",AvgSIN,AvgCOS);\n\t\tif((AvgCOS\u003eXLowBound[Axis]) \u0026\u0026 (AvgCOS\u003cXHighBound[Axis]) \u0026\u0026 (AvgSIN\u003eYLowBound[Axis]) \u0026\u0026 (AvgSIN\u003cYHighBound[Axis]))\n\t\t{\n\t\t\t// we found the index \n\t\t\treturn 0;\n\t\t}\n\t}\n\treturn 1; // did not find index\n}\nvoid SetNewBotRef()\n{\n\tSlaveBotPos.base=SlaveBotPos.base + ((GetAxisCurrent(0) - SetHome.base)/MSMoveRatio.base);\n\tSlaveBotPos.end=SlaveBotPos.end + ((GetAxisCurrent(1) - SetHome.end)/MSMoveRatio.end);\n\tSlaveBotPos.pivot=SlaveBotPos.pivot + ((GetAxisCurrent(2) - SetHome.pivot)/MSMoveRatio.pivot);\n\tSlaveBotPos.angle=SlaveBotPos.angle + ((GetAxisCurrent(3) - SetHome.angle)/MSMoveRatio.angle);\n\tSlaveBotPos.rotate=SlaveBotPos.rotate + ((GetAxisCurrent(4) - SetHome.rotate)/MSMoveRatio.rotate);\n\tSetHome.base=GetAxisCurrent(0);\n\tSetHome.end=GetAxisCurrent(1);\n\tSetHome.pivot=GetAxisCurrent(2);\n\tSetHome.angle=GetAxisCurrent(3);\n\tSetHome.rotate=GetAxisCurrent(4);\n\t\n}\nvoid* CapturePoints(void *arg)\n{\n    unsigned long i = 0;\n\tint a1,a2,a3,a4,a5,f1,f2,f3,f4,f5,BGM=10,EmbedRec=1,RecState=0,Length=0,GripperToggle=0,ServoSet=180;\n\tconst char delimiters[] = \".\";\n\n\tchar c,*FileNameBase,*FileNameExt,*tmpSt,RecordFilename[256],iString[256];\n//    pthread_t id = pthread_self();\n\tinitTermios(0);\n\tf1=0;\n\tmapped[FINE_ADJUST_BASE]=f1;\n\tf3 =0;\n\tmapped[FINE_ADJUST_END]=f3;\n\tf2=0;\n\tmapped[FINE_ADJUST_PIVOT]=f2;\n\tf4=0;\n\tmapped[FINE_ADJUST_ANGLE]=f4;\n\tf5=0;\n\tmapped[FINE_ADJUST_ROT]=f5;\n\tForcePossition[0]=0;\n\tForcePossition[1]=0;\n\tForcePossition[2]=0;\n\tForcePossition[3]=0;\n\tForcePossition[4]=0;\n\n\n\tmapped[COMMAND_REG]=12448;\n\t//printf(\"/n Thread running\");\n\tif(RunMode==1)\n\t\tforceMode=3;\n\tc=' ';\n\t//mapped[MAX_ERROR]=(2000 ^ 6000);\n\twhile(c!='k') // kill\n\t{\n\t\tmapped[FINE_ADJUST_BASE]=f1;\n\t\tmapped[FINE_ADJUST_END]=f3;\n\t\tmapped[FINE_ADJUST_PIVOT]=f2;\n\t\tmapped[FINE_ADJUST_ANGLE]=f4;\n\t\tmapped[FINE_ADJUST_ROT]=f5;\n\t\tc=getchar();\n\t\tif(c=='l')  // learn movement\n\t\t{\n\t\t\tif(RecState==0)\n\t\t\t{\n\t\t\t\tRecState=1;\n\t\t\t\tstrcpy(iString,CptA.FileName);\n\t\t\t\tFileNameBase = strtok (iString, delimiters);\n\t\t\t\t//FileNameExt=strtok (NULL, delimiters);\n\t\t\t\tstrcpy(RecordFilename,FileNameBase);\n\t\t\t\tstrcat(RecordFilename,\"ERM\");\n\t\t\t\tasprintf(\u0026tmpSt, \"%d\", EmbedRec++);\n\t\t\t\tstrcat(RecordFilename,tmpSt);\n\t\t\t\tfree(tmpSt);\n\t\t\t\tstrcat(RecordFilename,\".dta\");\n\t\t\t\ta1=getNormalizedInput(BASE_POSITION_AT)+getNormalizedInput(BASE_POSITION_FORCE_DELTA)+f1;\n\t\t\t\ta3=getNormalizedInput(END_POSITION_AT)+getNormalizedInput(END_POSITION_FORCE_DELTA)+f3;\n\t\t\t\ta2=getNormalizedInput(PIVOT_POSITION_AT)+getNormalizedInput(PIVOT_POSITION_FORCE_DELTA)+f2;\n\t\t\t\ta4=getNormalizedInput(ANGLE_POSITION_AT)+getNormalizedInput(ANGLE_POSITION_FORCE_DELTA)+f4;\n\t\t\t\ta5=getNormalizedInput(ROT_POSITION_AT)+getNormalizedInput(ROT_POSITION_FORCE_DELTA)+f5;\n\t\t\t\tfprintf(CptA.fp,\"a %d,%d,%d,%d,%d\\n\",a1,a2,a3,a4,a5);\n\t\t\t\t//printf(\"\\na %d,%d,%d,%d,%d\",a1,a2,a3,a4,a5);\n\t\t\t\tfprintf(CptA.fp,\"o %s\\n\",RecordFilename);\n\t\t\t\tmapped[REC_PLAY_TIMEBASE]=5;\n\t\t\t\tmapped[REC_PLAY_CMD]=CMD_RESET_RECORD;\n\t\t\t\tmapped[REC_PLAY_CMD]=0;\t\t\t\t\n\t\t\t\tmapped[REC_PLAY_CMD]=CMD_RECORD; // start recording\n\n\t\t\t\t//InitCaptureMovement(RecordFilename);\n\t\t\t}\n\t\t\telse\n\t\t\t{\n\t\t\t\tRecState=0;\n\t\t\t\ta1=getNormalizedInput(BASE_POSITION_AT)+getNormalizedInput(BASE_POSITION_FORCE_DELTA)+f1;\n\t\t\t\ta3=getNormalizedInput(END_POSITION_AT)+getNormalizedInput(END_POSITION_FORCE_DELTA)+f3;\n\t\t\t\ta2=getNormalizedInput(PIVOT_POSITION_AT)+getNormalizedInput(PIVOT_POSITION_FORCE_DELTA)+f2;\n\t\t\t\ta4=getNormalizedInput(ANGLE_POSITION_AT)+getNormalizedInput(ANGLE_POSITION_FORCE_DELTA)+f4;\n\t\t\t\ta5=getNormalizedInput(ROT_POSITION_AT)+getNormalizedInput(ROT_POSITION_FORCE_DELTA)+f5;\n\t\t\t\t//f//printf(CptA.fp,\"a %d,%d,%d,%d,%d\\n\",a1,a2,a3,a4,a5);\n\t\t\t\t//printf(\"\\na %d,%d,%d,%d,%d\",a1,a2,a3,a4,a5);\n\t\t\t\t//f//printf(CptA.fp,\"o %s\\n\",RecordFilename);\n\n\t\t\t\tmapped[REC_PLAY_CMD]=0; // stop recording\n\t\t\t\tLength=mapped[RECORD_BLOCK_SIZE];\n\t\t\t\tReadDMA(0x3f000000,Length,RecordFilename);\n\t\t\t\tmapped[REC_PLAY_CMD]=CMD_RESET_RECORD;\n\t\t\t\tmapped[REC_PLAY_CMD]=0;\n\t\t\t}\n\t\t\t\n\t\t}\n\t\tif(c=='h')\n\t\t{\n\t\t\tif(FroceMoveMode==0)\n\t\t\t{\n\t\t\t\tFroceMoveMode=1;\n\t\t\t\t//printf(\"/n force mode %d\",FroceMoveMode);\n\t\t\t}\n\t\t\telse\n\t\t\t{\n\t\t\t\tFroceMoveMode=0;\n\t\t\t\t//printf(\"/n force mode %d\",FroceMoveMode);\n\t\t\t}\n\t\t}\n\t\tif(c=='+')\n\t\t{\n\t\t\tBGM=BGM+10;\n\t\t\tSetNewBotRef();\n\t\t\t/*SetHome.base = SlaveBotPos.base;\n\t\t\tSetHome.end = SlaveBotPos.end;\n\t\t\tSetHome.pivot = SlaveBotPos.pivot;\n\t\t\tSetHome.angle = SlaveBotPos.angle;\n\t\t\tSetHome.rotate = SlaveBotPos.rotate;*/\n\t\t\t\n\t\t\tMSMoveRatio.base++;\n\t\t\tMSMoveRatio.end++;\n\t\t\tMSMoveRatio.pivot++;\n\t\t\tMSMoveRatio.angle++;\n\t\t\tMSMoveRatio.rotate++;\n\t\t\t//printf(\"\\n big move %d MasterSlave Move ratio %d\\n\",BGM,MSMoveRatio.base);\n\t\t}\n\t\tif(c=='-')\n\t\t{\n\t\t\tBGM=BGM-10;\n\t\t\tif(BGM \u003c 10)\n\t\t\t{\n\t\t\t\tBGM=10;\n\t\t\t}\n\t\t\tif(MSMoveRatio.base != 1)\n\t\t\t{\n\t\t\t\t/*SetHome.base = SlaveBotPos.base;\n\t\t\t\tSetHome.end = SlaveBotPos.end;\n\t\t\t\tSetHome.pivot = SlaveBotPos.pivot;\n\t\t\t\tSetHome.angle = SlaveBotPos.angle;\n\t\t\t\tSetHome.rotate = SlaveBotPos.rotate;*/\n\t\t\t\tSetNewBotRef();\n\t\t\t\tMSMoveRatio.base--;\n\t\t\t\tMSMoveRatio.end--;\n\t\t\t\tMSMoveRatio.pivot--;\n\t\t\t\tMSMoveRatio.angle--;\n\t\t\t\tMSMoveRatio.rotate--;\n\t\t\t}\n\t\t\t//printf(\"\\n big move %d MasterSlave Move ratio %d\\n\",BGM,MSMoveRatio.base);\n\t\t}\n\t\tif(c=='h')\n\t\t{\n\t\t\tSetNewBotRef();\n\t\t\t/*\n\t\t\tSetHome.base = getNormalizedInput(BASE_POSITION_AT)+getNormalizedInput(BASE_POSITION_FORCE_DELTA);\n\t\t\tSetHome.end = getNormalizedInput(END_POSITION_AT)+getNormalizedInput(END_POSITION_FORCE_DELTA);\n\t\t\tSetHome.pivot = getNormalizedInput(PIVOT_POSITION_AT)+getNormalizedInput(PIVOT_POSITION_FORCE_DELTA);\n\t\t\tSetHome.angle = getNormalizedInput(ANGLE_POSITION_AT)+getNormalizedInput(ANGLE_POSITION_FORCE_DELTA);\n\t\t\tSetHome.rotate = getNormalizedInput(ROT_POSITION_AT)+getNormalizedInput(ROT_POSITION_FORCE_DELTA);\n\t\t\t*/\n\t\t}\t\n\t\tif(c=='p')\n\t\t{\n\t\t\ta1=getNormalizedInput(BASE_POSITION_AT)+getNormalizedInput(BASE_POSITION_FORCE_DELTA)+f1;\n\t\t\ta3=getNormalizedInput(END_POSITION_AT)+getNormalizedInput(END_POSITION_FORCE_DELTA)+f3;\n\t\t\ta2=getNormalizedInput(PIVOT_POSITION_AT)+getNormalizedInput(PIVOT_POSITION_FORCE_DELTA)+f2;\n\t\t\ta4=getNormalizedInput(ANGLE_POSITION_AT)+getNormalizedInput(ANGLE_POSITION_FORCE_DELTA)+f4;\n\t\t\ta5=getNormalizedInput(ROT_POSITION_AT)+getNormalizedInput(ROT_POSITION_FORCE_DELTA)+f5;\n\t\t\tfprintf(CptA.fp,\"a %d,%d,%d,%d,%d\\n\",a1,a2,a3,a4,a5);\n\t\t\tfprintf(CptA.fp,\"S EERoll,%d\\n\",ServoSet);\n\t\t\t//printf(\"\\na %d,%d,%d,%d,%d\",a1,a2,a3,a4,a5);\n\t\t}\n\t\tif(c==' ')\n\t\t{ \n\t\t\tfprintf(CptA.fp,\"z 500000\\n\");\n\t\t\t//printf(\"\\n insert dwell .5 seconds\\n\");\n\t\t}\n\t\tif(c=='g')\n\t\t{ \n\t\t\tif(GripperToggle==0)\n\t\t\t{\n\t\t\t\tfprintf(CptA.fp,\"S GripperMotor,1\\n\");\n\t\t\t\t//printf(\"\\n GripperMotor On\\n\");\n\t\t\t\tGripperToggle=1;\n\t\t\t\tSetGripperMotor(GripperToggle);\n\t\t\t}\n\t\t\telse\n\t\t\t{\n\t\t\t\tfprintf(CptA.fp,\"S GripperMotor,0\\n\");\n\t\t\t\t//printf(\"\\n GripperMotor Off\\n\");\n\t\t\t\tGripperToggle=0;\n\t\t\t\tSetGripperMotor(GripperToggle);\n\t\t\t}\n\t\t}\n\t\tif(c=='t')\n\t\t{\n\t\t\tif(forceMode==1)\n\t\t\t{\n\t\t\t\tmapped[DIFF_FORCE_SPEED_FACTOR_ANGLE]=0;\n\t\t\t\tmapped[DIFF_FORCE_SPEED_FACTOR_ROT]=0;\n\t\t\t\tmapped[PID_ADDRESS]=3;\n\t\t\t\tmapped[PID_P]=DEFAULT_PID_SETTING_PY;\n\t\t\t\tmapped[PID_ADDRESS]=4;\n\t\t\t\tmapped[PID_ADDRESS]=0;\n\t\t\t\tmapped[PID_P]=0;\n\t\t\t\tmapped[PID_ADDRESS]=1;\n\t\t\t\tmapped[PID_ADDRESS]=2;\n\t\t\t\tmapped[SPEED_FACTORA]=DEF_SPEED_FACTOR_A ;\n\t\t\t\tforceMode=2;\n\t\t\t\t//printf(\"\\n Move XYZ yo \\n\");\n\t\t\t\tcontrolled=1;\n\t\t\t\t\n\t\t\t}else if(forceMode==0)\n\t\t\t{\n\n\t\t\t\tmapped[DIFF_FORCE_SPEED_FACTOR_ANGLE]=DEF_SPEED_FACTOR_DIFF;\n\t\t\t\tmapped[DIFF_FORCE_SPEED_FACTOR_ROT]=DEF_SPEED_FACTOR_DIFF;\n\t\t\t\tmapped[PID_ADDRESS]=3;\n\t\t\t\tmapped[PID_P]=0;\n\t\t\t\tmapped[PID_ADDRESS]=4;\n\t\t\t\tmapped[PID_ADDRESS]=0;\n\t\t\t\tmapped[PID_P]=DEFAULT_PID_SETTING_XYZ;\n\t\t\t\tmapped[PID_ADDRESS]=1;\n\t\t\t\tmapped[PID_ADDRESS]=2;\n\t\t\t\tmapped[SPEED_FACTORA]=0;\n\t\t\t\tforceMode=1;\n\t\t\t\tcontrolled=0;\n\t\t\t\t\n\t\t\t\t//printf(\"\\n Move Pitch and Yaw \\n\");\n\t\t\t\t\n\t\t\t}else if(forceMode==2)\n\t\t\t{\n\t\t\t\tmapped[DIFF_FORCE_SPEED_FACTOR_ANGLE]=0;\n\t\t\t\tmapped[DIFF_FORCE_SPEED_FACTOR_ROT]=0;\n\t\t\t\tmapped[PID_ADDRESS]=0;\n\t\t\t\tmapped[PID_P]=DEFAULT_PID_SETTING_XYZ;\n\t\t\t\tmapped[PID_ADDRESS]=1;\n\t\t\t\tmapped[PID_ADDRESS]=2;\n\t\t\t\tmapped[PID_ADDRESS]=3;\n\t\t\t\t\n//\t\t\t\tmapped[PID_P]=0; //DEFAULT_PID_SETTING_PY;\n\t\t\t\tmapped[PID_P]=DEFAULT_PID_SETTING_PY;\n\t\t\t\tmapped[PID_ADDRESS]=4;\n\t\t\t\tmapped[SPEED_FACTORA]=0;\n\t\t\t\tif(RunMode==1)\n\t\t\t\t\tforceMode=3;\n\t\t\t\telse\n\t\t\t\t\tforceMode=0;\n\t\t\t\t//printf(\"\\n Move fine adjust all \\n\");\n\t\t\t\tcontrolled=0;\n\t\t\t}else if(forceMode==3)\n\t\t\t{\n\t\t\t\t////printf(\"\\n Move all \\n\");\n\t\t\t\tmapped[DIFF_FORCE_SPEED_FACTOR_ANGLE]=DEF_SPEED_FACTOR_DIFF;\n\t\t\t\tmapped[DIFF_FORCE_SPEED_FACTOR_ROT]=DEF_SPEED_FACTOR_DIFF;\n\t\t\t\tmapped[PID_P]=0;\n\t\t\t\tmapped[PID_ADDRESS]=3;\n\t\t\t\tmapped[PID_ADDRESS]=4;\n\t\t\t\tmapped[PID_ADDRESS]=0;\n\t\t\t\tmapped[PID_ADDRESS]=1;\n\t\t\t\tmapped[PID_ADDRESS]=2;\n\t\t\t\tmapped[SPEED_FACTORA]=DEF_SPEED_FACTOR_A ;\n\t\t\t\tforceMode=2;\n\t\t\t\tcontrolled=1;\n\t\t\t\t//printf(\"\\n Move All \\n\");\n\t\t\t}\n\t\t}\n\t\tif(c=='q')\n\t\t{\n\t\t\tf1--;\n\t\t}\n\t\tif(c=='w')\n\t\t{\n\t\t\tf1++;\n\t\t}\n\t\tif(c=='a')\n\t\t{\n\t\t\tf2--;\n\t\t}\n\t\tif(c=='s')\n\t\t{\n\t\t\tf2++;\n\t\t}\n\t\tif(c=='z')\n\t\t{\n\t\t\tf3--;\n\t\t}\n\t\tif(c=='x')\n\t\t{\n\t\t\tf3++;\n\t\t}\n\t\tif(c=='e')\n\t\t{\n\t\t\tf4--;\n\t\t}\n\t\tif(c=='r')\n\t\t{\n\t\t\tf4++;\n\t\t}\n\t\tif(c=='d')\n\t\t{\n\t\t\tf5--;\n\t\t}\n\t\tif(c=='f')\n\t\t{\n\t\t\tf5++;\n\t\t}\n\t\tif(c=='Q')\n\t\t{\n\t\t\tf1=f1-BGM;\n\t\t}\n\t\tif(c=='W')\n\t\t{\n\t\t\tf1=f1+BGM;\n\t\t}\n\t\tif(c=='A')\n\t\t{\n\t\t\tf2=f2-BGM;\n\t\t}\n\t\tif(c=='S')\n\t\t{\n\t\t\tf2=f2+BGM;\n\t\t}\n\t\tif(c=='Z')\n\t\t{\n\t\t\tf3=f3-BGM;\n\t\t}\n\t\tif(c=='X')\n\t\t{\n\t\t\tf3=f3+BGM;\n\t\t}\n\t\tif(c=='E')\n\t\t{\n\t\t\tf4=f4-BGM;\n\t\t}\n\t\tif(c=='R')\n\t\t{\n\t\t\tf4=f4+BGM;\n\t\t}\n\t\tif(c=='D')\n\t\t{\n\t\t\tf5=f5-BGM;\n\t\t}\n\t\tif(c=='F')\n\t\t{\n\t\t\tf5=f5+BGM;\n\t\t}\n\t\tif(c==',')\n\t\t{\n\t\t\tServoSet=ServoSet+1;\n\t\t\tSetGripperRoll(ServoSet);\n\t\t}\n\t\tif(c=='.')\n\t\t{\n\t\t\tServoSet=ServoSet-1;\n\t\t\tSetGripperRoll(ServoSet);\n\t\t}\n\t}\n\tmapped[MAX_ERROR]=(2000 ^ 4000);\n\tmapped[DIFF_FORCE_SPEED_FACTOR_ANGLE]=0;\n\tmapped[DIFF_FORCE_SPEED_FACTOR_ROT]=0;\n\tmapped[PID_ADDRESS]=0;\n\tmapped[PID_P]=DEFAULT_PID_SETTING_XYZ;\n\tmapped[PID_ADDRESS]=1;\n\tmapped[PID_ADDRESS]=2;\n\tmapped[PID_ADDRESS]=3;\n\tmapped[PID_P]=DEFAULT_PID_SETTING_PY;\n\tmapped[PID_ADDRESS]=4;\n\tmapped[SPEED_FACTORA]=0;\n\t\n\t\n\tmapped[FINE_ADJUST_BASE]=0;\n\tmapped[FINE_ADJUST_END]=0;\n\tmapped[FINE_ADJUST_PIVOT]=0;\n\tmapped[FINE_ADJUST_ANGLE]=0;\n\tmapped[FINE_ADJUST_ROT]=0;\n        mapped[COMMAND_REG]=3158688;\t\n\t//printf(\"\\nAll done\\n\");\n\tcontrolled=0;\n\tfprintf(CptA.fp,\"x\\n\");\n\t\t\t\n\tfclose(CptA.fp);\n\tresetTermios();\n\t\n    return NULL;\n}\nint InitCapturePoints(char *FileName)\n{\n    int i = 0;\n    int err;\n\tCptA.FileName=FileName;\n\tCptA.fp=fopen(FileName, \"w\");\n\tif(CptA.fp!=0)\n\t{\n\t\tCapturePoints((void*)\u0026CptA);\n\t}\n\n/*\t\terr = pthread_create(\u0026(tid[i]), NULL, \u0026CapturePoints, (void*)\u0026CptA);\n\t\tif (err != 0)\n\t\t{\n\t\t\t//printf(\"\\ncan't create thread :[%s]\", strerror(err));\n\t\t\treturn 1;\n\t\t}\n\t\telse\n\t\t{\n\t\t\t//printf(\"\\n Begin Program\\n\");\n\t\t}\n    }*/\n\treturn 0;\n}\n\n\nint InitCaptureMovement(char *FileName)\n{\n    int i = 0,Length=0;\n    int err;\n\tchar c;\n\t//initTermios(0);\n//\tCptMove.FileName=FileName;\n//\tCptMove.fp=fopen(FileName, \"w\");\n//\tif(CptMove.fp!=0)\n\t{\n\t\t//CaptureMovement((void*)\u0026CptMove);\n\t\tmapped[REC_PLAY_CMD]=CMD_RESET_RECORD;\n\t\tmapped[REC_PLAY_CMD]=CMD_RECORD; // start recording\n\t\tc=' ';\n\t\twhile(c!='k') // kill\n\t\t{\n\t\t\tc=getchar();\n\t\t\t\n\t\t\t\n\t\t}\n\t\tmapped[REC_PLAY_CMD]=0; // stop recording\n\t\t\n\t\tLength=mapped[RECORD_BLOCK_SIZE];\n\t\tReadDMA(0x3f000000,Length,FileName);\n\n\t\t\n\t}\n\t//resetTermios();\n\treturn 0;\n}\n\n\nint fixedPointCV(float Val,int whole,int fract)\n{\n\treturn 0;\n}\nvoid setDefaults(int State)\n{\n\n\tint i,ForceFelt,j,HiBoundry,LowBoundry,BoundryACC;\n\tchar c;\n    FILE *CentersFile,*RemoteRobotAddress,*DiffFile;\n\tint HexValue;\n\tint IntFloat;\n\tfloat *fConvert=(float *)(\u0026IntFloat);\n\t\n\t\n\t\n\tDiffFile = fopen(\"DiffCorrectionFactor.txt\", \"rs\");\n\tif(DiffFile!=NULL)\n\t{\n\t\tfscanf (DiffFile, \"%f\", \u0026DiffCorrectionFactor);\n\t\t//fgets(DiffFile, sizeof(RemoteRobotAdd)+1, RemoteRobotAddress);\n\t\t//fscanf(RemoteRobotAddress,\"%[^\\n]\",RemoteRobotAdd);\n\t\t//printf(\"%f \\n\",DiffCorrectionFactor);\n\t\tfclose(DiffFile);\n\t}\n\tDiffFile = fopen(\"AngleBoundry.txt\", \"rs\");\n\tif(DiffFile!=NULL)\n\t{\n\t\tfscanf (DiffFile, \"%i %i\", \u0026AngleHIBoundry,\u0026AngleLOWBoundry);\n\t\t//fgets(DiffFile, sizeof(RemoteRobotAdd)+1, RemoteRobotAddress);\n\t\t//fscanf(RemoteRobotAddress,\"%[^\\n]\",RemoteRobotAdd);\n\t\t//printf(\"Angle High boundry%i    Angle Low Boundry %i \\n\",AngleHIBoundry,AngleLOWBoundry);\n\t\tfclose(DiffFile);\n\t}\n\n\t\n\tRemoteRobotAddress = fopen(\"RemoteRobotAddress.txt\", \"rs\");\n\tif(RemoteRobotAddress!=NULL)\n\t{\n\t\tfgets(RemoteRobotAdd, sizeof(RemoteRobotAdd)+1, RemoteRobotAddress);\n\t\t//fscanf(RemoteRobotAddress,\"%[^\\n]\",RemoteRobotAdd);\n\t\t//printf(\"%s \\n\",RemoteRobotAdd);\n\t\tfclose(RemoteRobotAddress);\n\t}\n\tmapped[FORCE_BIAS_BASE]=0;\n\tmapped[FORCE_BIAS_END]=0;\n\tmapped[FORCE_BIAS_PIVOT]=0;\n\tmapped[FORCE_BIAS_ANGLE]=0;\n\tmapped[FORCE_BIAS_ROT]=0;\n\n\tmapped[ACCELERATION_MAXSPEED]=ACCELERATION_MAXSPEED_DEF;\n\tmaxSpeed=(ACCELERATION_MAXSPEED_DEF) \u0026 0b00000000000011111111111111111111;\n\tcoupledAcceleration=((ACCELERATION_MAXSPEED_DEF) \u0026 0b00000011111100000000000000000000) \u003e\u003e 20;\n\n\tif(State==1)\n\t{\n\n\n\t\tmapped[BASE_FORCE_DECAY]=000000;\n\t\tmapped[END_FORCE_DECAY]=000000;\n\t\tmapped[PIVOT_FORCE_DECAY]=000000;\n\t\tmapped[ANGLE_FORCE_DECAY]=000000;\n\t\tmapped[ROTATE_FORCE_DECAY]=000000;\n\n\t\tmapped[ACCELERATION_MAXSPEED]=ACCELERATION_MAXSPEED_DEF;\n\t\tmaxSpeed=(ACCELERATION_MAXSPEED_DEF) \u0026 0b00000000000011111111111111111111;\n\t\tcoupledAcceleration=((ACCELERATION_MAXSPEED_DEF) \u0026 0b00000011111100000000000000000000) \u003e\u003e 20;\n\t\n\n\t\tmapped[REC_PLAY_TIMEBASE]=5;\n\t\t\n    CentersFile = fopen(\"AdcCenters.txt\", \"rs\");\n\n    //read file into array\n\tif(CentersFile!=NULL)\n\t{\n\t\tfscanf(CentersFile, \"%x\", \u0026HexValue);\n\t\t//printf(\"%x \\n\",HexValue);\n\t\tmapped[BASE_SIN_CENTER]=HexValue;\n\t\tfscanf(CentersFile, \"%x\", \u0026HexValue);\n\t\tmapped[BASE_COS_CENTER]=HexValue;\n\t\tfscanf(CentersFile, \"%x\", \u0026HexValue);\n\t\tmapped[END_SIN_CENTER]=HexValue;\n\t\tfscanf(CentersFile, \"%x\", \u0026HexValue);\n\t\tmapped[END_COS_CENTER]=HexValue;\n\t\tfscanf(CentersFile, \"%x\", \u0026HexValue);\n\t\tmapped[PIVOT_SIN_CENTER]=HexValue;\n\t\tfscanf(CentersFile, \"%x\", \u0026HexValue);\n\t\tmapped[PIVOT_COS_CENTER]=HexValue;\n\t\tfscanf(CentersFile, \"%x\", \u0026HexValue);\n\t\tmapped[ANGLE_SIN_CENTER]=HexValue;\n\t\tfscanf(CentersFile, \"%x\", \u0026HexValue);\n\t\tmapped[ANGLE_COS_CENTER]=HexValue;\n\t\tfscanf(CentersFile, \"%x\", \u0026HexValue);\n\t\tmapped[ROT_SIN_CENTER]=HexValue;\n\t\tfscanf(CentersFile, \"%x\", \u0026HexValue);\n\t\tmapped[ROT_COS_CENTER]=HexValue;\n\t\tfclose(CentersFile);\n\t}\n\t\n\tCentersFile = fopen(\"AxisCal.txt\", \"rs\");\n\n    //read file into array\n\tif(CentersFile!=NULL)\n\t{\n\t\tfscanf(CentersFile, \"%f\", \u0026JointsCal[0]);\n\t\t//printf(\"%f \\n\",JointsCal[0]);\n\t\tfscanf(CentersFile, \"%f\", \u0026JointsCal[1]);\n\t\tfscanf(CentersFile, \"%f\", \u0026JointsCal[2]);\n\t\tfscanf(CentersFile, \"%f\", \u0026JointsCal[3]);\n\t\tfscanf(CentersFile, \"%f\", \u0026JointsCal[4]);\n\t\tfscanf(CentersFile, \"%i\", \u0026HexValue);\n\t\tmapped[ANGLE_END_RATIO]=HexValue;//((LG_RADIUS/SM_RADIUS * MOTOR_STEPS * MICRO_STEP)/(MOTOR_STEPS*GEAR_RATIO*MICRO_STEP))*2^24\n\t\tfclose(CentersFile);\n\t}\n \n    \n\n/*\t\tmapped[BASE_SIN_CENTER]=0x7f80000;\n\t\tmapped[BASE_COS_CENTER]=0x7f80000;\n\t\tmapped[END_SIN_CENTER]=0x7f80000;\n\t\tmapped[END_COS_CENTER]=0x07f80000;\n\t\tmapped[PIVOT_SIN_CENTER]=0x4a00000;\n\t\tmapped[PIVOT_COS_CENTER]=0x6500000;\n\t\tmapped[ANGLE_SIN_CENTER]=0x7890000;\n\t\tmapped[ANGLE_COS_CENTER]=0x5080000;\n\t\tmapped[ROT_SIN_CENTER]=0x5200000;\n\t\tmapped[ROT_COS_CENTER]=0x6f00000;*/\n\n//\t\tmapped[BASE_SIN_CENTER]=0x7f80000;\n//\t\tmapped[BASE_COS_CENTER]=0x7f80000;\n//\t\tmapped[END_SIN_CENTER]=0x7f80000;\n//\t\tmapped[END_COS_CENTER]=0x07f80000;\n//\t\tmapped[PIVOT_SIN_CENTER]=0x6500000;\n//\t\tmapped[PIVOT_COS_CENTER]=0x4a00000;\n//\t\tmapped[ANGLE_SIN_CENTER]=0x7f80000;\n//\t\tmapped[ANGLE_COS_CENTER]=0x4900000;\n//\t\tmapped[ROT_SIN_CENTER]=0x6f00000;\n//\t\tmapped[ROT_COS_CENTER]=0x5200000;\n\n\n\t\tmapped[DIFF_FORCE_BETA ]=0x0102;\n\n\t// set up PID defaults\n\t\n\t\tmapped[PID_P]=DEFAULT_PID_SETTING_XYZ;\n\t\tmapped[PID_I]=0;\n\t\tmapped[PID_D]=0;\n\t\tmapped[PID_DELTATNOT]=0;\n\t\tmapped[PID_DELTAT]=0;\n\t\tmapped[PID_ADDRESS]=0;\n\t\tmapped[PID_ADDRESS]=1;\n\t\tmapped[PID_ADDRESS]=2;\n\t\tmapped[PID_ADDRESS]=3;\n\t\tmapped[PID_P]=DEFAULT_PID_SETTING_PY;\n\t\tmapped[PID_ADDRESS]=4;\n\t\t\n\t\t// set boundries  \n\t\tBaseBoundryHigh=90000;\n\t\tBaseBoundryLow=-90000;\n\t\tEndBoundryHigh=130000;\n\t\tEndBoundryLow=-130000;\n\t\tPivotBoundryHigh=70000;\n\t\tPivotBoundryLow=-70000;\n\t\tAngleBoundryHigh=DEFAULT_ANGLE_BOUNDRY_HI;\n\t\tAngleBoundryLow=DEFAULT_ANGLE_BOUNDRY_LOW;\n\t\tRotateBoundryHigh=3000;\n\t\tRotateBoundryLow=-3000;\n\t\t\n\t\t\n\t\tmapped[BOUNDRY_BASE]=0x2bf2d40e; //\n\t\tmapped[BOUNDRY_END]=0x3f7ac086;  //\n\t\tmapped[BOUNDRY_PIVOT]=0x222eddd2; //1\n\t\tHiBoundry=AngleHIBoundry/8;\n\t\tLowBoundry=AngleLOWBoundry/8;\n\t\tBoundryACC=(HiBoundry \u003c\u003c 16) | (0x0000ffff \u0026 LowBoundry);\n\t\tmapped[BOUNDRY_ANGLE]=BoundryACC;// 0x01f4fe0c; //\n\t\t//printf(\"Angle boundry pack %x \",BoundryACC);\n\t\t\n\t\tmapped[BOUNDRY_ROT]=((RotateBoundryHigh/8) \u003c\u003c 16) | (0X0000FFFF \u0026 (RotateBoundryLow/8));\n\n\t\t\n\t\t\n\t\t\n\t\t\n\n\t\tmapped[SPEED_FACTORA]=0;\n\t\tmapped[SPEED_FACTORB]=0;\n\t\tmapped[MAX_ERROR]=(2000 ^ 6000);\n\t\t\n\n\t\tmapped[DIFF_FORCE_TIMEBASE]=140000; // this is currently remapped to MaxSpeed XYZ Force\n\t\t\n\t\tmapped[DIFF_FORCE_MAX_SPEED]=90000;\n//\t\tIntFloat)\n\t\t*fConvert=DiffCorrectionFactor;\n\t\t\n\t\t//printf(\" float value %x \\n\",IntFloat);\n\t\t//if(RunMode==1)\n\t\t\tmapped[DIFF_FORCE_ANGLE_COMPENSATE]=0;//IntFloat;\n\t\t//else\n\t\t//\tmapped[DIFF_FORCE_ANGLE_COMPENSATE]=0;//IntFloat;\n\t\t\n\t\t//mapped[ANGLE_END_RATIO]=645278;//((LG_RADIUS/SM_RADIUS * MOTOR_STEPS * MICRO_STEP)/(MOTOR_STEPS*GEAR_RATIO*MICRO_STEP))*2^24\n\t\t\n\n\n\n\n\t\n\t\tmapped[FRICTION_BASE]=0x130; // fixed point 16 bit == .1\n\t\tmapped[FRICTION_END]=0x130; // fixed point 16 bit == .1\n\t\tmapped[FRICTION_PIVOT]=0x130; // fixed point 16 bit == .1\n\t\tmapped[FRICTION_ANGLE]=0x50; // fixed point 16 bit == .1\n\t\tmapped[FRICTION_ROT]=0x50; // fixed point 16 bit == .1\n\t\n\t\t\n\t\t\n\t}\n\tif(State==2)\n\t{\n\t\tCmdVal=3158688;\n\t\tmapped[FINE_ADJUST_BASE]=0;\n\t\tmapped[FINE_ADJUST_END]=0;\n\t\tmapped[FINE_ADJUST_PIVOT]=0;\n\t\tmapped[FINE_ADJUST_ANGLE]=0;\n\t\tmapped[FINE_ADJUST_ROT]=0;\n\t}\n\tif(State==3)\n\t{\n\t\t\n\t\t//printf(\"\\nMonitor Force Started\");\n\t\twhile(1)\n\t\t{\n\t\t\t//printf(\"\\nTable Delta \");\n\t\t\tfor(i=0;i\u003c5;i++)\n\t\t\t{\n\t\t\t\t//printf(\"%6d \",getNormalInput(BASE_POSITION_DELTA+i));\n\t\t\t}\n\t\t\t//printf(\" PID_DELTA \");\n\t\t\tfor(i=0;i\u003c5;i++)\n\t\t\t{\n\t\t\t\t//printf(\"%5d \",getNormalizedInput(BASE_POSITION_PID_DELTA+i));\n\t\t\t}\n\t\t\t////printf(\"\\nForce_DELTA \");\n\t\t\t//for(i=0;i\u003c5;i++)\n\t\t\t//{\n\t\t\t//\t//printf(\"%5d \",getNormalizedInput(BASE_POSITION_FORCE_DELTA+i));\n\t\t\t//}\n\t\t}\n\t\t\n\t}\n\t\n\tif(State==4)\n\t{\n\t\t//initTermios(0);\n\t\tmapped[PID_ADDRESS]=3;\n\t\tmapped[PID_P]=0;\n\t\tmapped[PID_I]=0;\n\t\tmapped[PID_D]=0;\n\t\tmapped[PID_DELTATNOT]=0;\n\t\tmapped[PID_DELTAT]=0;\n\t\tmapped[PID_ADDRESS]=4;\n\t\tc=' ';\n\t\tmapped[FINE_ADJUST_BASE]=0;\n\t\tmapped[FINE_ADJUST_END]=0;\n\t\tmapped[FINE_ADJUST_PIVOT]=0;\n\t\tmapped[FINE_ADJUST_ANGLE]=0;\n\t\tmapped[FINE_ADJUST_ROT]=0;\n\t\t\n\t\t//printf(\"\\nForce Protect Started\");\n\t\twhile(1) \n//\t\twhile(c!='k') // kill\n\t\t{\n\t\t\t//c=getchar();\n\t\t\tfor(i=0;i\u003c5;i++)\n\t\t\t{\n\t\t\t\tForceFelt=getNormalizedInput(BASE_POSITION_PID_DELTA+i)-getNormalizedInput(BASE_POSITION_DELTA+i);\n\t\t\t\t//printf(\"%5d \",ForceFelt);\n\t\t\t\tif(abs(ForceFelt-PositionAdjust[i]) \u003e= ForceLimit[i])\n\t\t\t\t{\n\t\t\t\t\tif((ForceFelt-PositionAdjust[i]) \u003c 0)\n\t\t\t\t\t{\n\t\t\t\t\t\t//PositionAdjust[i]=2*(abs((ForceFelt-PositionAdjust[i]))-ForceLimit[i]);\t\t\t\t\t\t\n\t\t\t\t\t}\n\t\t\t\t\telse\n\t\t\t\t\t{\n\t\t\t\t\t\t//PositionAdjust[i]=2*-(abs((ForceFelt-PositionAdjust[i]))-ForceLimit[i]);\n\t\t\t\t\t}\n\t\t\t\t}\n\t\t\t}\n\t\t\tfor(i=0;i\u003c3;i++)\n\t\t\t{\n\t\t\t\t//mapped[FINE_ADJUST_BASE+i]=PositionAdjust[i];\n\t\t\t\t//printf(\"%5d \",PositionAdjust[i]);\n\t\t\t}\n\t\t\t//printf(\"\\n\");\n\t\t\t\n\t\t\tusleep(10000);\n\t\t}\t\n\t//resetTermios();\t\t\n\t}\n\tif(State==5)\n\t{\n\t\t\n\t\t//printf(\"\\nMonitor Accelerateion Force Started\");\n\t\twhile(1)\n\t\t{\n\t\t\t//printf(\"\\nSpeed Delta \");\n\t\t\tfor(i=0;i\u003c5;i++)\n\t\t\t{\n\t\t\t\t//printf(\"%5d \",mapped[SLOPE_BASE_POSITION+i]);\n\t\t\t}\n\t\t}\n\t\t\n\t}\n\tif(State==6)\n\t{\n\t\t\n\t\t//printf(\"\\nMonitor Force offset Started\");\n\t\twhile(1)\n\t\t{\n\t\t\t////printf(\"\\nSpeed Delta \");\n\t\t\tfor(i=0;i\u003c5;i++)\n\t\t\t{\n\t\t\t\t//printf(\"%6d  \",GetAxisCurrent(i));\n\t\t\t}\n\t\t\t//printf(\"\\n\");\n\t\t}\n\t\t\n\t}\n}\n\nvoid wait_fifo_flush(void)\n{\n\twhile((mapped[CMD_FIFO_STATE] \u0026 0x2) != 2)\n\t{\n\t\t\n\t}\n}\nint HashInputCMD(char *s)\n{\n\n\tif(s[0]=='r')\n\t\treturn READ_CMD;\n\tif(s[0]=='w')\n\t{\n\t\twait_fifo_flush();\n\t\treturn WRITE_CMD;\n\t}\n\tif(s[0]=='x')\n\t\treturn EXIT_CMD;\n\tif(s[0]=='s')\n\t\treturn SLOWMOVE_CMD;\n\tif(s[0]=='a')\n\t\treturn MOVEALL_CMD;\n\tif(s[0]=='d')\n\t\treturn DMAREAD_CMD;\n\tif(s[0]=='t')\n\t\treturn DMAWRITE_CMD;\n\tif(s[0]=='c')\n\t\treturn CAPTURE_AD_CMD;\n\tif(s[0]=='f')\n\t\treturn FIND_HOME_CMD;\n\tif(s[0]=='n')\n\t\treturn FIND_INDEX_CMD;\n\tif(s[0]=='p')\n\t\treturn FIND_HOME_REP_CMD;\n\tif(s[0]=='l')\n\t\treturn LOAD_TABLES;\n\tif(s[0]=='i')\n\t\treturn CAPTURE_POINTS_CMD;\n\tif(s[0]=='z')\n\t\treturn SLEEP_CMD;\n\tif(s[0]=='m')\n\t\treturn RECORD_MOVEMENT;\n\tif(s[0]=='o')\n\t\treturn REPLAY_MOVEMENT;\n\tif(s[0]=='R')\n\t\treturn MOVEALL_RELATIVE;\n\tif(s[0]=='S')\n\t\treturn SET_PARAM;\n\tif(s[0]=='g')\n\t\treturn SEND_HEARTBEAT;\n\t/*if(s[0]=='F')\n\t\treturn SET_FORCE_MOVE_POINT;*/\n\tif(s[0]=='G')\n\t\treturn HEART_BEAT;\n\tif(s[0]=='h')\n\t\treturn HEART_BEAT;\n\tif(s[0]=='P')\n\t\treturn PID_FINEMOVE;\n\tif(s[0]=='F')\n\t{\n\t\twait_fifo_flush();\n\t\treturn HEART_BEAT;\n\t}\n\treturn 0;\n}\n\n\n\nint isNear(int a,int b,int range)\n{\n\tif(abs(a-b)\u003c=range)\n\t\treturn TRUE;\n\telse\n\t\treturn FALSE;\n}\nint getNormalizedInput(int param) \n{\n\tint val;\n\tfloat corrF=1;\n\tval = mapped[param];\n\tif(param \u003c= ROT_POSITION_FORCE_DELTA)\n\t{\n\t\tcorrF = JointsCal[(param-INPUT_OFFSET) % 5];\n\t}\n\tif((val \u0026 0x40000)!=0)\n\t{\n\t\tval = (val | 0xfff80000);\n\t}\n\treturn (int)((float)val / corrF);\n}\nint getNormalInput(int param) \n{\n\tint val;\n\tfloat corrF=1;\n\tval = mapped[param];\n\tif((val \u0026 0x40000)!=0)\n\t{\n\t\tval = (val | 0xfff80000);\n\t}\n\treturn (int)((float)val / corrF);\n}\n\n\n\nint WaitMoveGoal(int a1,int a2,int a3,int a4,int a5,int timeout)\n{\n\tint b1,b2,b3,b4,b5;\n    long            ms; // Milliseconds\n    time_t          oldTime,newTime;  // Seconds\n    struct timespec spec;\n\n\tb1=getNormalizedInput(BASE_POSITION_AT);\n\tb3=getNormalizedInput(END_POSITION_AT);\n\tb2=getNormalizedInput(PIVOT_POSITION_AT);\n\tb4=getNormalizedInput(ANGLE_POSITION_AT);\n\tb5=getNormalizedInput(ROT_POSITION_AT);\n\n    clock_gettime(CLOCK_REALTIME, \u0026spec);\n\toldTime  = spec.tv_sec;\n\t////printf(\"\\n%d %d %d %d %d\\n\",b1,b2,b3,b4,b5);\t\n\t////printf(\"\\n%d %d %d %d %d\\n\",a1,a2,a3,a4,a5);\t\n//    //printf(\"\\nStart wait Goal\");\n\twhile(!isNear(a1,b1,500) || !isNear(a2,b2,500) || !isNear(a3,b3,500) || !isNear(a4,b4,500) || !isNear(a5,b5,500))\n\t{\n\t\tb1=getNormalizedInput(BASE_POSITION_AT);\n\t\tb3=getNormalizedInput(END_POSITION_AT);\n\t\tb2=getNormalizedInput(PIVOT_POSITION_AT);\n\t\tb4=getNormalizedInput(ANGLE_POSITION_AT);\n\t\tb5=getNormalizedInput(ROT_POSITION_AT);\n/*\t\tc1=getNormalizedInput(BASE_POSITION_DELTA);\n\t\tc3=getNormalizedInput(END_POSITION_DELTA);\n\t\tc2=getNormalizedInput(PIVOT_POSITION_DELTA);\n\t\tc4=getNormalizedInput(ANGLE_POSITION_DELTA);\n\t\tc5=getNormalizedInput(ROT_POSITION_DELTA);*/\n\t\tclock_gettime(CLOCK_REALTIME, \u0026spec);\n\t\tnewTime  = spec.tv_sec;\n\t\tif((newTime-oldTime)\u003e20)\n\t\t\treturn 0;\n\t\t\n\t\t////printf(\"\\n%d %d %d %d %d Waiting for %d %d %d %d %d \",b1,b2,b3,b4,b5,a1,a2,a3,a4,a5);\t\n\t}\n    ////printf(\"\\nEnd wait Goal\");\n\treturn 0;\n\t\t\n}\n\nint moverobotPID(int a1,int a2,int a3,int a4,int a5)\n{\n\tCheckBoundry(\u0026a1,\u0026a2,\u0026a3,\u0026a4,\u0026a5);\t\n\ta1=(int)((double)a1 * JointsCal[0]);\n\ta2=(int)((double)a2 * JointsCal[1]);\n\ta3=(int)((double)a3 * JointsCal[2]);\n\ta4=(int)((double)a4 * JointsCal[3]);\n\ta5=(int)((double)a5 * JointsCal[4]);\n\tmapped[FINE_ADJUST_BASE]=a1;\n\tmapped[FINE_ADJUST_END]=a3;\n\tmapped[FINE_ADJUST_PIVOT]=a2;\n\tmapped[FINE_ADJUST_ANGLE]=a4;\n\tmapped[FINE_ADJUST_ROT]=a5;\n}\n\nint MoveRobot(int a1,int a2,int a3,int a4,int a5, int mode)\n{\n\n\tCheckBoundry(\u0026a1,\u0026a2,\u0026a3,\u0026a4,\u0026a5);\n/*\tint b1,b2,b3,b4,b5;\n\tb1=getNormalizedInput(PLAYBACK_BASE_POSITION);\n\tb2=getNormalizedInput(PLAYBACK_END_POSITION);\n\tb3=getNormalizedInput(PLAYBACK_PIVOT_POSITION);\n\tb4=getNormalizedInput(PLAYBACK_ANGLE_POSITION);\n\tb5=getNormalizedInput(PLAYBACK_ROT_POSITION);\n\ta1=a1-b1; // subtract out the playback position \n\ta2=a2-b2;\n\ta3=a3-b3;\n\ta4=a4-b4;\n\ta5=a5-b5;\n\t//printf(\"\\nPlayback position %d %d %d %d %d\",b1,b2,b3,b4,b5);\n*/\n\t////printf(\"\\nStart wait Goal\");\n//\t332800\n//\t166400\n//\t0.25679012345679012345679012345679\n//\t0.00987654320987654320987654320988\n\n\t//if(mode==BLOCKING_MOVE)\n\t\t//WaitMoveGoal(LastGoal[0],LastGoal[1],LastGoal[2],LastGoal[3],LastGoal[4],DEFAULT_MOVE_TIMEOUT);\n\n\t\n\n\tLastGoal[0]=a1;\n\tLastGoal[1]=a2;\n\tLastGoal[2]=a3;\n\tLastGoal[3]=a4;\n\tLastGoal[4]=a5;\n\n\ta1=(int)((double)a1 * JointsCal[0]);\n\ta2=(int)((double)a2 * JointsCal[1]);\n\ta3=(int)((double)a3 * JointsCal[2]);\n\ta4=(int)((double)a4 * JointsCal[3]);\n\ta5=(int)((double)a5 * JointsCal[4]);\n\t////printf(\"angle 0 result %d \",a1);\n\twhile(mapped[CMD_FIFO_STATE] \u0026 0x01 != 0);\n\t\n\tmapped[COMMAND_REG]=CMD_MOVEEN | CmdVal;\n\tmapped[BASE_POSITION]=a1;\n\tmapped[END_POSITON]=a3;\n\tmapped[PIVOT_POSITON]=a2;\n\tmapped[ANGLE_POSITON]=a4;\n\tmapped[ROT_POSITON]=a5;\n\tmapped[COMMAND_REG]=CMD_MOVEEN | CMD_MOVEGO | CmdVal;\n\tmapped[COMMAND_REG]=CmdVal;\n\t/*if(mode==BLOCKING_MOVE)\n\t\tWaitMoveGoal(LastGoal[0],LastGoal[1],LastGoal[2],LastGoal[3],LastGoal[4],DEFAULT_MOVE_TIMEOUT);*/\n\treturn 0;\n}\n\nint CheckBoundry(int* j1, int* j2, int* j3, int* j4, int* j5)\n{\n\tint h1,h2,h3,h4,h5,l1,l2,l3,l4,l5;\n\th1=(int)((float)BaseBoundryHigh / fabs(JointsCal[0]));\n\th2=(int)((float)PivotBoundryHigh / fabs(JointsCal[1]));\n\th3=(int)((float)EndBoundryHigh / fabs(JointsCal[2]));\n\th4=(int)((float)AngleBoundryHigh / fabs(JointsCal[3]));\n\th5=(int)((float)RotateBoundryHigh / fabs(JointsCal[4]));\n\tl1=(int)((float)BaseBoundryLow / fabs(JointsCal[0]));\n\tl2=(int)((float)PivotBoundryLow / fabs(JointsCal[1]));\n\tl3=(int)((float)EndBoundryLow / fabs(JointsCal[2]));\n\tl4=(int)((float)AngleBoundryLow / fabs(JointsCal[3]));\n\tl5=(int)((float)RotateBoundryLow / fabs(JointsCal[4]));\n\tif(*(j1) \u003e= h1)\n\t{\n\t\t*(j1) = h1;\n\t}\n\tif(*(j1) \u003c= l1)\n\t{\n\t\t*(j1) = l1;\n\t}\n\tif(*(j2) \u003e= h2)\n\t{\n\t\t*(j2) = h2;\n\t}\n\tif(*(j2) \u003c= l2)\n\t{\n\t\t*(j2) = l2;\n\t}\n\tif(*(j3) \u003e= h3)\n\t{\n\t\t*(j3) = h3;\n\t}\n\tif(*(j3) \u003c= l3)\n\t{\n\t\t*(j3) = l3;\n\t}\n\tif(*(j4) \u003e= h4)\n\t{\n\t\t*(j4) = h4;\n\t}\n\tif(*(j4) \u003c= l4)\n\t{\n\t\t*(j4) = l4;\n\t}\n\tif(*(j5) \u003e= h5)\n\t{\n\t\t*(j5) = h5;\n\t}\n\tif(*(j5) \u003c= l5)\n\t{\n\t\t*(j5) = l5;\n\t}\n\t////printf(\" boundry calc %d %d %f %d\",*(j1),BaseBoundryHigh,fabs(JointsCal[0]),(int)((float)BaseBoundryHigh / fabs(JointsCal[0])));\n/*\nEndBoundryHigh\nEndBoundryLow\nPivotBoundryHigh\nPivotBoundryLow\nAngleBoundryHigh\nAngleBoundryLow\nRotateBoundryHigh\nRotateBoundryLow\n*/\n if(*(j1)==0)\n\t *(j1)=1;\n return 0;\n}\n\nint SetParam(char *a1,float fa2,int a3,int a4,int a5)\n{\n\tint i,BDH,BDL,Axis;\n\tint a2=(int)fa2;\n\tint fxa2=(a2\u003c\u003c8)+(fa2-a2)*256;\n\t\n\t////printf(\"%s %s %d %d \\n\",Params[i],a1,a2,i);\n\n\tfor(i=0;i\u003cMAX_PARAMS;i++)\n\t{\n\t\t////printf(\"%s %s %d %d \\n\",Params[i],a1,a2,i);\n\t\tif(strcmp(a1,Params[i])==0)\n\t\t{\n\t\t\t\tswitch(i)\n\t\t\t\t{\n\t\t\t\t\t\tcase 0:\n\t\t\t\t\t\t////printf(\"Set Speed\\n\");\n\t\t\t\t\t\t\t//set Max Speed\n\t\t\t\t\t\t\tmaxSpeed=a2 \u0026 0b00000000000011111111111111111111;\n\t\t\t\t\t\t\tmapped[ACCELERATION_MAXSPEED]=maxSpeed + (coupledAcceleration \u003c\u003c 20);\n\t\t\t\t\t\t\treturn 0;\n\t\t\t\t\t\tbreak;\n\t\t\t\t\t\tcase 1:\n\t\t\t\t\t\t\t//set Acceleration\n\t\t\t\t\t\t\tcoupledAcceleration=a2 \u0026 0b111111;\n\t\t\t\t\t\t\tmapped[ACCELERATION_MAXSPEED]=maxSpeed + (coupledAcceleration \u003c\u003c 20);\n\t\t\t\t\t\t\treturn 0;\n\t\t\t\t\t\tbreak;\n\t\t\t\t\t\tcase 2:\n\t\t\t\t\t\t\tmapped[FORCE_BIAS_BASE]=a2;\n\t\t\t\t\t\t\treturn 0;\n\t\t\t\t\t\tcase 3:\n\t\t\t\t\t\t\tmapped[FORCE_BIAS_END]=a2;\n\t\t\t\t\t\t\treturn 0;\n\t\t\t\t\t\tbreak;\n\t\t\t\t\t\tcase 4:\n\t\t\t\t\t\t\tmapped[FORCE_BIAS_PIVOT]=a2;\n\t\t\t\t\t\t\treturn 0;\n\t\t\t\t\t\tbreak;\n\t\t\t\t\t\tcase 5:\n\t\t\t\t\t\t\tmapped[FORCE_BIAS_ANGLE]=a2;\n\t\t\t\t\t\t\treturn 0;\n\t\t\t\t\t\tbreak;\n\t\t\t\t\t\tcase 6:\n\t\t\t\t\t\t\tmapped[FORCE_BIAS_ROT]=a2;\n\t\t\t\t\t\t\treturn 0;\n\t\t\t\t\t\tbreak;\n\t\t\t\t\t\tcase 7:\n\t\t\t\t\t\t\tmapped[FRICTION_BASE]=fxa2;\n\t\t\t\t\t\t\treturn 0;\n\t\t\t\t\t\tbreak;\n\t\t\t\t\t\tcase 8:\n\t\t\t\t\t\t\tmapped[FRICTION_END]=fxa2;\n\t\t\t\t\t\t\treturn 0;\n\t\t\t\t\t\tbreak;\n\t\t\t\t\t\tcase 9:\n\t\t\t\t\t\t\tmapped[FRICTION_PIVOT]=fxa2;\n\t\t\t\t\t\t\treturn 0;\n\t\t\t\t\t\tbreak;\n\t\t\t\t\t\tcase 10:\n\t\t\t\t\t\t\tmapped[FRICTION_ANGLE]=fxa2;\n\t\t\t\t\t\t\treturn 0;\n\t\t\t\t\t\tbreak;\n\t\t\t\t\t\tcase 11:\n\t\t\t\t\t\t\tmapped[FRICTION_ROT]=fxa2;\n\t\t\t\t\t\t\treturn 0;\n\t\t\t\t\t\tbreak;\n\t\t\t\t\t\tcase 12:\n\t\t\t\t\t\t\tBaseBoundryHigh=(int)((float)a2 * fabs(JointsCal[0]));\n\t\t\t\t\t\t\t////printf(\" boundry set %d %d %f %f \", a2, BaseBoundryHigh, JointsCal[0], fabs(JointsCal[0]));\n\t\t\t\t\t\t\tBDH=BaseBoundryHigh;\n\t\t\t\t\t\t\tBDL=BaseBoundryLow;\n\t\t\t\t\t\t\tAxis=BOUNDRY_BASE;\n\t\t\t\t\t\t\tmapped[Axis]=((BDH/8) \u003c\u003c 16) | (0X0000FFFF \u0026 (BDL/8));\n\t\t\t\t\t\t\treturn 0;\n\t\t\t\t\t\tbreak;\n\t\t\t\t\t\tcase 13:\n\t\t\t\t\t\t\tBaseBoundryLow=(int)((float)a2 * fabs(JointsCal[0]));\n\t\t\t\t\t\t\tBDH=BaseBoundryHigh;\n\t\t\t\t\t\t\tBDL=BaseBoundryLow;\n\t\t\t\t\t\t\tAxis=BOUNDRY_BASE;\n\t\t\t\t\t\t\tmapped[Axis]=((BDH/8) \u003c\u003c 16) | (0X0000FFFF \u0026 (BDL/8));\n\t\t\t\t\t\t\treturn 0;\n\t\t\t\t\t\tbreak;\n\t\t\t\t\t\tcase 14:\n\t\t\t\t\t\t\tEndBoundryHigh=(int)((float)a2 * fabs(JointsCal[2]));\n\t\t\t\t\t\t\tBDH=EndBoundryHigh;\n\t\t\t\t\t\t\tBDL=EndBoundryLow;\n\t\t\t\t\t\t\tAxis=BOUNDRY_END;\n\t\t\t\t\t\t\tmapped[Axis]=((BDH/8) \u003c\u003c 16) | (0X0000FFFF \u0026 (BDL/8));\n\t\t\t\t\t\t\treturn 0;\n\t\t\t\t\t\tbreak;\n\t\t\t\t\t\tcase 15:\n\t\t\t\t\t\t\tEndBoundryLow=(int)((float)a2 * fabs(JointsCal[2]));\n\t\t\t\t\t\t\tBDH=EndBoundryHigh;\n\t\t\t\t\t\t\tBDL=EndBoundryLow;\n\t\t\t\t\t\t\tAxis=BOUNDRY_END;\n\t\t\t\t\t\t\tmapped[Axis]=((BDH/8) \u003c\u003c 16) | (0X0000FFFF \u0026 (BDL/8));\n\t\t\t\t\t\t\treturn 0;\n\t\t\t\t\t\tbreak;\n\t\t\t\t\t\tcase 16:\n\t\t\t\t\t\t\tPivotBoundryHigh=(int)((float)a2 * fabs(JointsCal[1]));\n\t\t\t\t\t\t\tBDH=PivotBoundryHigh;\n\t\t\t\t\t\t\tBDL=PivotBoundryLow;\n\t\t\t\t\t\t\tAxis=BOUNDRY_PIVOT;\n\t\t\t\t\t\t\tmapped[Axis]=((BDH/8) \u003c\u003c 16) | (0X0000FFFF \u0026 (BDL/8));\n\t\t\t\t\t\t\treturn 0;\n\t\t\t\t\t\tbreak;\n\t\t\t\t\t\tcase 17:\n\t\t\t\t\t\t\tPivotBoundryLow=(int)((float)a2 * fabs(JointsCal[1]));\n\t\t\t\t\t\t\tBDH=PivotBoundryHigh;\n\t\t\t\t\t\t\tBDL=PivotBoundryLow;\n\t\t\t\t\t\t\tAxis=BOUNDRY_PIVOT;\n\t\t\t\t\t\t\tmapped[Axis]=((BDH/8) \u003c\u003c 16) | (0X0000FFFF \u0026 (BDL/8));\n\t\t\t\t\t\t\treturn 0;\n\t\t\t\t\t\tbreak;\n\t\t\t\t\t\tcase 18:\n\t\t\t\t\t\t\tAngleBoundryHigh=(int)((float)a2 * fabs(JointsCal[3]));\n\t\t\t\t\t\t\tBDH=AngleBoundryHigh;\n\t\t\t\t\t\t\tBDL=AngleBoundryLow;\n\t\t\t\t\t\t\tAxis=BOUNDRY_ANGLE;\n\t\t\t\t\t\t\tmapped[Axis]=((BDH/8) \u003c\u003c 16) | (0X0000FFFF \u0026 (BDL/8));\n\t\t\t\t\t\t\treturn 0;\n\t\t\t\t\t\tbreak;\n\t\t\t\t\t\tcase 19:\n\t\t\t\t\t\t\tAngleBoundryLow=(int)((float)a2 * fabs(JointsCal[3]));\n\t\t\t\t\t\t\tBDH=AngleBoundryHigh;\n\t\t\t\t\t\t\tBDL=AngleBoundryLow;\n\t\t\t\t\t\t\tAxis=BOUNDRY_ANGLE;\n\t\t\t\t\t\t\tmapped[Axis]=((BDH/8) \u003c\u003c 16) | (0X0000FFFF \u0026 (BDL/8));\n\t\t\t\t\t\t\treturn 0;\n\t\t\t\t\t\tbreak;\n\t\t\t\t\t\tcase 20:\n\t\t\t\t\t\t\tRotateBoundryHigh=(int)((float)a2 * fabs(JointsCal[4]));\n\t\t\t\t\t\t\tBDH=RotateBoundryHigh;\n\t\t\t\t\t\t\tBDL=RotateBoundryLow;\n\t\t\t\t\t\t\tAxis=BOUNDRY_ROT;\n\t\t\t\t\t\t\tmapped[Axis]=((BDH/8) \u003c\u003c 16) | (0X0000FFFF \u0026 (BDL/8));\n\t\t\t\t\t\t\treturn 0;\n\t\t\t\t\t\tbreak;\n\t\t\t\t\t\tcase 21:\n\t\t\t\t\t\t\tRotateBoundryLow=(int)((float)a2 * fabs(JointsCal[4]));\n\t\t\t\t\t\t\tBDH=RotateBoundryHigh;\n\t\t\t\t\t\t\tBDL=RotateBoundryLow;\n\t\t\t\t\t\t\tAxis=BOUNDRY_ROT;\n\t\t\t\t\t\t\tmapped[Axis]=((BDH/8) \u003c\u003c 16) | (0X0000FFFF \u0026 (BDL/8));\n\t\t\t\t\t\t\treturn 0;\n\t\t\t\t\t\tbreak;\n\t\t\t\t\t\tcase 22:\n\t\t\t\t\t\t\tSetGripperMotor(a2);\n\t\t\t\t\t\t\t//printf(\"Gripper Motor Set\\n\");\n\t\t\t\t\t\t\treturn 0;\n\t\t\t\t\t\tbreak;\n\t\t\t\t\t\tcase 23:\n\t\t\t\t\t\t\tSetGripperRoll(a2);\n\t\t\t\t\t\t\t//printf(\"Gripper Roll Set\\n\");\n\t\t\t\t\t\t\treturn 0;\n\t\t\t\t\t\tbreak;\n\t\t\t\t\t\tcase 24:\n\t\t\t\t\t\t\tmapped[START_SPEED]=500 ^ a2;\n\t\t\t\t\t\tbreak;\n\t\t\t\t\t\tcase 25:\n\t\t\t\t\t\tbreak;\n\t\t\t\t\t\tdefault:\n\t\t\t\t\t\tbreak;\n\t\t\t\t}\n\t\t\n\t\t}\n\t}\n\treturn 0;\n\n}\nint MoveRobotRelative(int a1,int a2,int a3,int a4,int a5, int mode)\n{\n\tint b1,b2,b3,b4,b5;\n\tb1=getNormalizedInput(BASE_POSITION_AT);\n\tb3=getNormalizedInput(END_POSITION_AT);\n\tb2=getNormalizedInput(PIVOT_POSITION_AT);\n\tb4=getNormalizedInput(ANGLE_POSITION_AT);\n\tb5=getNormalizedInput(ROT_POSITION_AT);\n\t\n\t////printf(\"\\nRelative move %d %d %d %d %d\",b1,b2,b3,b4,b5);\n\treturn MoveRobot(a1+b1,a2+b2,a3+b3,a4+b4,a5+b5,mode);\n}\n\n\nint ReadDMA(int p1,int p2,char *p3)\n{\n\tFILE *fp;\n\tint i,j,blocks,writeSize;\n\tint dataarray[256];\n\tfp=fopen(p3, \"wb\");\n\tif(fp!=0)\n\t{\n\t\tmapped[DMA_CONTROL]=DMA_RESET_ALL;\n\t\tmapped[DMA_CONTROL]=0;\n\t\tblocks=p2/256;\n\t\tfor(j=0;j\u003cblocks;j++) // only do full blocks inside loop\n\t\t{\t\n\t\t\tmapped[DMA_READ_ADDRESS]=p1+(j*1024);\n\t\t\tmapped[DMA_READ_PARAMS]=(2\u003c\u003c8) | 127;\n\t\t\tmapped[DMA_CONTROL]=DMA_READ_BLOCK;\n\t\t\tmapped[DMA_CONTROL]=0;\n\t\t\tfor(i=0;i\u003c256;i++)\n\t\t\t{\n\t\t\t\tmapped[DMA_CONTROL]=DMA_READ_DEQUEUE;\n\t\t\t\tdataarray[i]=mapped[DMA_READ_DATA];\n\t\t\t\t////printf(\"\\n %d\",dataarray[i]);\n\t\t\t\tmapped[DMA_CONTROL]=0;\n\t\t\t}\n\t\t\twriteSize=fwrite((const void *)dataarray,sizeof(int),256,fp);\n\t\t\t//printf(\"\\n write %d iteration %d\",writeSize,j);\n\t\t\tmapped[DMA_CONTROL]=0;\n\t\t}\n\t\tfclose(fp);\n\t}\n\telse\n\t{\n\t\t//printf(\"\\n %s\",p3);\n\t}\n\treturn 0;\n}\nint WriteDMA(int Address,char *FileName)\n{\n\tFILE *fp;\n\tint i,j,blocks,readSize,Length;\n\tint dataarray[256];\n\tfp=fopen(FileName, \"rb\");\n\tif(fp!=0)\n\t{\n\t\t\n\t\tfseek(fp, 0, SEEK_END);    /* file pointer at the end of file */\n\t\tLength = ftell(fp);   /* take a position of file pointer size variable */\n\t\tfseek(fp, 0, SEEK_SET);    /* file pointer at the beginning of file */\n\t\tmapped[DMA_CONTROL]=DMA_RESET_ALL;\n\t\tmapped[DMA_CONTROL]=0;\n\t\tblocks=Length/256;\n\t\tfor(j=0;j\u003cblocks;j++) // only do full blocks inside loop\n\t\t{\t\n\t\t\tif(readSize=fread((const void *)dataarray,sizeof(int),256,fp)==256)\n\t\t\t{\n\t\t\t\tmapped[DMA_WRITE_ADDRESS]=Address+(j*1024);\n\t\t\t\tmapped[DMA_WRITE_PARAMS]=(2\u003c\u003c8) | 127;\n\t\t\t\tmapped[DMA_CONTROL]=0;\n\t\t\t\tfor(i=0;i\u003c256;i++)\n\t\t\t\t{\n\t\t\t\t\tmapped[DMA_WRITE_DATA]=dataarray[i];\n\t\t\t\t\tmapped[DMA_CONTROL]=DMA_WRITE_ENQUEUE;\n\t\t\t\t\tmapped[DMA_CONTROL]=0;\n\t\t\t\t}\n\t\t\t\tmapped[DMA_CONTROL]=DMA_WRITE_INITIATE;\n\t\t\t\tmapped[DMA_CONTROL]=0;\n\t\t\t\t////printf(\"\\n write %d iteration %d\",readSize,j);\n\t\t\t}\n\t\t}\n\t\tfclose(fp);\n\t\t//printf(\"\\n Table Loaded: Length %d\",Length);\n\t}\n\telse\n\t{\n\t    //printf(\"errno = %d.\\n\", errno);\n\t\t//perror(errno);\n\t\t//printf(\"\\n failed to open %s\",FileName);\n\t}\n\treturn Length;\n}\n\nint CaptureADtoFile(int Axis,int Start,int Length,int Delay,char *FileName)\n{\n\tFILE *fp;\n\tint i,j,k,blocks,writeSize,AvgSIN,AvgCOS,ADVal;\n\tint dataarray[512];\n\tswitch(Axis)\n\t{\n\t    case 0  :\n\t\tMoveRobot(Start,0,0,0,0,BLOCKING_MOVE);\n\t\tbreak; \n\t    case 1  :\n\t\tMoveRobot(0,Start,0,0,0,BLOCKING_MOVE);\n\t\tbreak; \n\t    case 2  :\n\t\tMoveRobot(0,0,Start,0,0,BLOCKING_MOVE);\n\t\tbreak; \n\t    case 3  :\n\t\tMoveRobot(0,0,0,Start,0,BLOCKING_MOVE);\n\t\tbreak; \n\t    case 4  :\n\t\tMoveRobot(0,0,0,0,Start,BLOCKING_MOVE);\n\t\tbreak; \n\t}\n\tfp=fopen(FileName, \"w\");\n\tif(fp!=0)\n\t{\n\t\tblocks=Length/256;\n\t\tfor(k=0;k\u003cblocks;k++) // only do full blocks inside loop\n\t\t{\t\n\t\t\tfor(i=0;i\u003c256;i++)\n\t\t\t{\n\t\t\t\tmapped[Axis]=Start+i+(k*256);\n\t\t\t\tAvgSIN=0;\n\t\t\t\tAvgCOS=0;\n\t\t\t\tfor(j=0;j\u003cDelay;j++)\n\t\t\t\t{\t\n\t\t\t\t\tADVal=mapped[ADLookUp[Axis]];\n\t\t\t\t\tAvgSIN=AvgSIN+ADVal;\n\t\t\t\t\tAvgCOS=AvgCOS+mapped[ADLookUp[Axis]+1];\n\t\t\t\t}\t\t\t\n\t\t\t\tdataarray[i*2]=AvgSIN/Delay;\n\t\t\t\tdataarray[(i*2)+1]=AvgCOS/Delay;\n//\t\t\t\t//printf(\"\\n SIN %d COS %d\",dataarray[i*2],dataarray[(i*2)+1]);\n\t\t\t}\n\t\t\twriteSize=fwrite((const void *)dataarray,sizeof(int),512,fp);\n\t\t\t//printf(\"\\n write %d iteration %d\",writeSize,k);\n\t\t}\n\t\tfclose(fp);\n\t}\n\telse\n\t{\n\t\t//printf(\"\\n %s\",FileName);\n\t}\n\treturn 0;\n}\nint FindHome(int Axis,int Start,int Length,int Delay,char *FileName)\n{\n\tFILE *fp;\n\tint i,j,k,blocks,AvgSIN,AvgCOS,ADVal,fileLen,ReadSize,MinSIN,MinCOS,MinSINIdx=0,MinCOSIdx=0;\n\tint RefrenceArray[1000000];\n\tint CaptureArray[Length*2];\n\tswitch(Axis)\n\t{\n\t    case 0  :\n\t\tMoveRobot(Start,0,0,0,0,BLOCKING_MOVE);\n\t\tbreak; \n\t    case 1  :\n\t\tMoveRobot(0,Start,0,0,0,BLOCKING_MOVE);\n\t\tbreak; \n\t    case 2  :\n\t\tMoveRobot(0,0,Start,0,0,BLOCKING_MOVE);\n\t\tbreak; \n\t    case 3  :\n\t\tMoveRobot(0,0,0,Start,0,BLOCKING_MOVE);\n\t\tbreak; \n\t    case 4  :\n\t\tMoveRobot(0,0,0,0,Start,BLOCKING_MOVE);\n\t\tbreak; \n\t}\n\tfp=fopen(FileName, \"rb\");\n\tif(fp!=0)\n\t{\n\t\tfseek(fp, 0, SEEK_END);\n\t\tfileLen=ftell(fp);\n\t\tfseek(fp, 0, SEEK_SET);\n\t\tReadSize=fread((char *)RefrenceArray, fileLen, 1, fp);\n\t\tfclose(fp);\n\t}\n\telse\n\t{\n\t\t//printf(\"\\n %s\",FileName);\n\t\treturn 1;\n\t}\n\tfor(k=0;k\u003cLength;k++) \n\t{\t\n\t\tmapped[Axis]=Start+k;\n\t\tAvgSIN=0;\n\t\tAvgCOS=0;\n\t\tfor(j=0;j\u003cDelay;j++)\n\t\t{\t\n\t\t\tADVal=mapped[ADLookUp[Axis]];\n\t\t\tAvgSIN=AvgSIN+ADVal;\n\t\t\tAvgCOS=AvgCOS+mapped[ADLookUp[Axis]+1];\n\t\t}\t\t\t\n\t\tCaptureArray[k*2]=AvgSIN/Delay;\n\t\tCaptureArray[(k*2)+1]=AvgCOS/Delay;\n//\t\t//printf(\"\\n Query %d Refrence %d\",CaptureArray[k*2],RefrenceArray[k*2]);\n\t}\n\tMoveRobot(0,0,0,0,0,BLOCKING_MOVE);\n\tblocks=fileLen/8;\n\tMinSIN=0x7fffffff;\n\tMinCOS=0x7fffffff;\n\tfor(i=0;i\u003cblocks;i++)\n\t{\n\t\tAvgSIN=0;\n\t\tAvgCOS=0;\n\t\tfor(k=0;k\u003cLength;k++)\n\t\t{\n\t\t\tAvgSIN=AvgSIN+abs(CaptureArray[k*2]-RefrenceArray[(k+i)*2]);\n\t\t\tAvgSIN=AvgSIN+abs(CaptureArray[(k*2)+1]-RefrenceArray[(((k+i)*2)+1)]);\n//\t\t\tAvgCOS=AvgCOS+abs(CaptureArray[(k*2)+1]-RefrenceArray[(((k+i)*2)+1)]);\n\t\t}\n\t\tif(MinSIN\u003eAvgSIN)\n\t\t{\n\t\t\tMinSIN=AvgSIN;\n\t\t\tMinSINIdx=i;\n\t\t}\n/*\t\tif(MinCOS\u003eAvgCOS)\n\t\t{\n\t\t\tMinCOS=AvgCOS;\n\t\t\tMinCOSIdx=i;\n\t\t}*/\n\t}\n\t//printf(\"\\n SAD %d SINidx %d\\n\",MinSIN,MinSINIdx);\n\tswitch(Axis)\n\t{\n\t    case 0  :\n\t\tMoveRobot(30000-MinSINIdx,0,0,0,0,BLOCKING_MOVE);\n\t\tbreak; \n\t    case 1  :\n\t\tMoveRobot(0,30000-MinSINIdx,0,0,0,BLOCKING_MOVE);\n\t\tbreak; \n\t    case 2  :\n\t\tMoveRobot(0,0,30000-MinSINIdx,0,0,BLOCKING_MOVE);\n\t\tbreak; \n\t    case 3  :\n\t\tMoveRobot(0,0,0,30000-MinSINIdx,0,BLOCKING_MOVE);\n\t\tbreak; \n\t    case 4  :\n\t\tMoveRobot(0,0,0,0,30000-MinSINIdx,BLOCKING_MOVE);\n\t\tbreak; \n\t}\n\tmapped[COMMAND_REG]=256;  //reset home\n\tmapped[COMMAND_REG]=0;\n\treturn 30000-MinSINIdx;\n}\n\n\nvoid showPosAt(void)\n{\n\t\tint b1,b2,b3,b4,b5;\n\t\tb1=getNormalizedInput(BASE_POSITION_AT);\n\t\tb3=getNormalizedInput(END_POSITION_AT);\n\t\tb2=getNormalizedInput(PIVOT_POSITION_AT);\n\t\tb4=getNormalizedInput(ANGLE_POSITION_AT);\n\t\tb5=getNormalizedInput(ROT_POSITION_AT);\n\t\t//printf(\"\\nPos %d %d %d %d %d  \",b1,b2,b3,b4,b5);\t\n}\nvoid ReplayMovement(char *FileName)\n{\n\tint Length,rbc;\n\tshowPosAt();\n\tLength=WriteDMA(0x3f000000,FileName);\n\tmapped[RECORD_LENGTH]=Length/4;\n\tmapped[REC_PLAY_TIMEBASE]=1;\n\tmapped[REC_PLAY_CMD]=CMD_RESET_RECORD;\n\tmapped[REC_PLAY_CMD]=CMD_RESET_PLAY;\n\tmapped[REC_PLAY_CMD]=0;\n\tmapped[REC_PLAY_CMD]=CMD_PLAYBACK;\n\trbc=mapped[READ_BLOCK_COUNT];\n\tshowPosAt();\n\t\n\t//sleep(1);\n\twhile((mapped[READ_BLOCK_COUNT] \u0026 0x003fffff) != 0 )\n\t{\n\t\tif(rbc != mapped[READ_BLOCK_COUNT])\n\t\t{\n\t\t\t\t\n\t\t\t//printf(\"%d \\n\",mapped[READ_BLOCK_COUNT] \u0026 0x003fffff);\n\t\t\trbc=mapped[READ_BLOCK_COUNT];\n\t\t\tshowPosAt();\n\t\t}\n\t\t////printf(\"%d \\n\",mapped[READ_BLOCK_COUNT]);\n\t}\n\twhile((mapped[READ_BLOCK_COUNT] \u0026 0x00400000) != 0 )\n\t\t//printf(\"%d \\n\",mapped[READ_BLOCK_COUNT]);\n\tshowPosAt();\n\tmapped[REC_PLAY_CMD]=CMD_RESET_RECORD;\n\tmapped[REC_PLAY_CMD]=CMD_RESET_PLAY;\n\tmapped[REC_PLAY_CMD]=0;\n\n\t/*fa0=fa0+mapped[PLAYBACK_BASE_POSITION];\n\tfa1=fa1+mapped[PLAYBACK_END_POSITION];\n\tfa2=fa2+mapped[PLAYBACK_PIVOT_POSITION];\n\tfa3=fa3+mapped[PLAYBACK_ANGLE_POSITION];\n\tfa4=fa4+mapped[PLAYBACK_ROT_POSITION];\n\tmapped[FINE_ADJUST_BASE]=fa0;\n\tmapped[FINE_ADJUST_END]=fa1;\n\tmapped[FINE_ADJUST_PIVOT]=fa2;\n\tmapped[FINE_ADJUST_ANGLE]=fa3;\n\tmapped[FINE_ADJUST_ROT]=fa4;*/\n\tmapped[REC_PLAY_CMD]=CMD_RESET_RECORD;\n}\nint getInput(void)\n{\n\tchar iString[255];\n\tif(gets(iString)!=NULL)\n\t{\n\t\treturn ParseInput(iString);\n\t}\n\t\n}\nint ParseInput(char *iString)\n{\n\t//char iString[255];\n\tconst char delimiters[] = \" ,\";\n\tchar *token,*p1,*p2,*p3,*p4,*p5;\n\tint i,j,Add,Start,Length,Delay,Axis,tokenVal;\n\t////printf(\"\\nStart wait Goal\");\n\tif(iString !=NULL)\n\t{\n\t\ttoken = strtok (iString, delimiters);\n\t\tif (token !=NULL)\n\t\t{\n\t\t\ttokenVal=HashInputCMD(token);\n\t\t\t////printf(\"Token %s TokenVal %i\",token,tokenVal);\n\t\t}\n\t\telse\n\t\t\treturn 1;\n\t\tif (tokenVal != 0 ){\n\t\t\tswitch(tokenVal)\n\t\t\t{\t\n\t\t\t\tcase PID_FINEMOVE :\n\t\t\t\t\tp1=strtok (NULL, delimiters);\n\t\t\t\t\tp2=strtok (NULL, delimiters);\n\t\t\t\t\tp3=strtok (NULL, delimiters);\n\t\t\t\t\tp4=strtok (NULL, delimiters);\n\t\t\t\t\tp5=strtok (NULL, delimiters);\n\t\t\t\t\tmoverobotPID(atoi(p1),atoi(p2),atoi(p3),atoi(p4),atoi(p5));\n\t\t\t\tbreak; \n\t\t\t\tcase HEART_BEAT :\n\t\t\t\tbreak; \n\t\t\t\n\t\t\t\tcase SET_FORCE_MOVE_POINT :\n\t\t\t\t\tp1=strtok (NULL, delimiters);\n\t\t\t\t\tp2=strtok (NULL, delimiters);\n\t\t\t\t\tp3=strtok (NULL, delimiters);\n\t\t\t\t\tp4=strtok (NULL, delimiters);\n\t\t\t\t\tp5=strtok (NULL, delimiters);\n\t\t\t\t\tForceDestination[0]=atoi(p1);\n\t\t\t\t\tForceDestination[1]=atoi(p2);\n\t\t\t\t\tForceDestination[2]=atoi(p3);\n\t\t\t\t\tForceDestination[3]=atoi(p4);\n\t\t\t\t\tForceDestination[4]=atoi(p5);\n\t\t\t\tbreak; \n\t\n\t\t\t\tcase SEND_HEARTBEAT :\n\t\t\t\t\t////printf(\"heartbeat dispatched\\n\");\n\t\t\t\tbreak; \n\t\t\t\tcase SET_PARAM :\n\t\t\t\t\tp1=strtok (NULL, delimiters);\n\t\t\t\t\tp2=strtok (NULL, delimiters);\n\t//\t\t\t\tp3=strtok (NULL, delimiters);\n\t//\t\t\t\tp4=strtok (NULL, delimiters);\n\t//\t\t\t\tp5=strtok (NULL, delimiters);\n\t\t\t\t\tSetParam(p1,atof(p2),0,0,0);\n\t\t\t\tbreak; \n\t\t\t\tcase MOVEALL_RELATIVE :\n\t\t\t\t\tp1=strtok (NULL, delimiters);\n\t\t\t\t\tp2=strtok (NULL, delimiters);\n\t\t\t\t\tp3=strtok (NULL, delimiters);\n\t\t\t\t\tp4=strtok (NULL, delimiters);\n\t\t\t\t\tp5=strtok (NULL, delimiters);\n\t\t\t\t\tMoveRobotRelative(atoi(p1),atoi(p2),atoi(p3),atoi(p4),atoi(p5),BLOCKING_MOVE);\n\t\t\t\tbreak; \n\t\t\t\tcase REPLAY_MOVEMENT  :\n\t\t\t\t\tp1=strtok (NULL, delimiters);\n\t\t\t\t\tReplayMovement(p1);\n\t\t\t\tbreak; \n\t\t\t\tcase SLEEP_CMD  :\n\t\t\t\t\tp1=strtok (NULL, delimiters);\n\t\t\t\t\tusleep(atoi(p1));\n\t\t\t\tbreak; \n\t\t\t\tcase MOVE_CMD  :\n\t\t\t\t\tp1=strtok (NULL, delimiters);\n\t\t\t\t\tp2=strtok (NULL, delimiters);\n\t\t\t\tbreak; \n\t\t\t\tcase LOAD_TABLES  :\n\t\t\t\t\tWriteDMA(0x3f800000,\"/srv/samba/share/a0tbl.dta\");\n\t\t\t\t\tWriteDMA(0x3f900000,\"/srv/samba/share/a1tbl.dta\");\n\t\t\t\t\tWriteDMA(0x3fa00000,\"/srv/samba/share/a2tbl.dta\");\n\t\t\t\tbreak; \n\t\t\t\tcase CAPTURE_POINTS_CMD :\n\t\t\t\t\tp1=strtok (NULL, delimiters);\n\t\t\t\t\tInitCapturePoints(p1);\n\t\t\t\tbreak;\n\t\t\t\tcase RECORD_MOVEMENT :\n\t\t\t\t\tp1=strtok (NULL, delimiters);\n\t\t\t\t\tInitCaptureMovement(p1);\n\t\t\t\tbreak;\n\t\t\t\tcase DMAREAD_CMD :\n\t\t\t\t\tp1=strtok (NULL, delimiters);\n\t\t\t\t\tp2=strtok (NULL, delimiters);\n\t\t\t\t\tp3=strtok (NULL, delimiters);\n\t\t\t\t\tAdd=(int)strtol(p1,0,16);\n\t\t\t\t\tLength=(int)strtol(p2,0,16);\n\t\t\t\t\tReadDMA(Add,Length,p3);\n\t\t\t\tbreak;\n\t\t\t\tcase DMAWRITE_CMD :\n\t\t\t\t\tp1=strtok (NULL, delimiters);\n\t\t\t\t\tp2=strtok (NULL, delimiters);\n\t\t\t\t\tAdd=(int)strtol(p1,0,16);\n\t\t\t\t\tWriteDMA(Add,p2);\n\t\t\t\tbreak;\n\t\t\t\tcase CAPTURE_AD_CMD :\n\t\t\t\t\tp1=strtok (NULL, delimiters);\n\t\t\t\t\tp2=strtok (NULL, delimiters);\n\t\t\t\t\tp3=strtok (NULL, delimiters);\n\t\t\t\t\tp4=strtok (NULL, delimiters);\n\t\t\t\t\tp5=strtok (NULL, delimiters);\n\t\t\t\t\tAxis=atoi(p1);\n\t\t\t\t\tStart=atoi(p2);\n\t\t\t\t\tLength=atoi(p3);\n\t\t\t\t\tDelay=atoi(p4);\n\t\t\t\t\tCaptureADtoFile(Axis,Start,Length,Delay,p5);\n\t\t\t\tbreak;\n\t\t\t\tcase FIND_HOME_REP_CMD :\n\t\t\t\t\tp1=strtok (NULL, delimiters);\n\t\t\t\t\tAxis=atoi(p1);\n\t\t\t\t\tStart=0;\n\t\t\t\t\tLength=5000;\n\t\t\t\t\tDelay=10000;\n\t\t\t\t\tswitch(Axis)\n\t\t\t\t\t{\n\t\t\t\t\t\tcase 0  :\n\t\t\t\t\t\t\twhile(abs(FindHome(Axis,Start,Length,Delay,\"/srv/samba/share/d0.dta\"))\u003e300);\n\t\t\t\t\t\tbreak; \n\t\t\t\t\t\tcase 1  :\n\t\t\t\t\t\t\twhile(abs(FindHome(Axis,Start,Length,Delay,\"/srv/samba/share/d1.dta\"))\u003e300);\n\t\t\t\t\t\tbreak; \n\t\t\t\t\t\tcase 2  :\n\t\t\t\t\t\t\twhile(abs(FindHome(Axis,Start,Length,Delay,\"/srv/samba/share/d2.dta\"))\u003e300);\n\t\t\t\t\t\tbreak; \n\t\t\t\t\t\tcase 3  :\n\t\t\t\t\t\t\twhile(abs(FindHome(Axis,Start,Length,Delay,\"/srv/samba/share/d3.dta\"))\u003e300);\n\t\t\t\t\t\tbreak; \n\t\t\t\t\t\tcase 4  :\n\t\t\t\t\t\t\twhile(abs(FindHome(Axis,Start,Length,Delay,\"/srv/samba/share/d4.dta\"))\u003e300);\n\t\t\t\t\t\tbreak; \n\t\t\t\t\t}\n\t\t\t\tbreak; \n\t\t\t\t\tcase FIND_HOME_CMD :\n\t\t\t\t\tp1=strtok (NULL, delimiters);\n\t\t\t\t\tp2=strtok (NULL, delimiters);\n\t\t\t\t\tp3=strtok (NULL, delimiters);\n\t\t\t\t\tp4=strtok (NULL, delimiters);\n\t\t\t\t\tp5=strtok (NULL, delimiters);\n\t\t\t\t\tAxis=atoi(p1);\n\t\t\t\t\tStart=atoi(p2);\n\t\t\t\t\tLength=atoi(p3);\n\t\t\t\t\tDelay=atoi(p4);\n\t\t\t\t\tFindHome(Axis,Start,Length,Delay,p5);\n\t\t\t\tbreak; \n\t\t\t\tcase FIND_INDEX_CMD :\n\t\t\t\t\tp1=strtok (NULL, delimiters);\n\t\t\t\t\tp2=strtok (NULL, delimiters);\n\t\t\t\t\tp3=strtok (NULL, delimiters);\n\t\t\t\t\tp4=strtok (NULL, delimiters);\n\t\t\t\t\tAxis=atoi(p1);\n\t\t\t\t\tStart=atoi(p2);\n\t\t\t\t\tLength=atoi(p3);\n\t\t\t\t\tDelay=atoi(p4);\n\t\t\t\t\tif(FindIndex(Axis,Start,Length,Delay)==0)\n\t\t\t\t\t{\n\t\t\t\t\t\t//printf(\"/nIndex reached\");\n\t\t\t\t\t\tbreak;\n\t\t\t\t\t}\n\t\t\t\t\telse\n\t\t\t\t\t{\n\t\t\t\t\t\t//printf(\"/nIndex not found\");\n\t\t\t\t\t}\n\t\t\t\tbreak; \n\t\t\t\tcase MOVEALL_CMD :\n\t\t\t\t\tp1=strtok (NULL, delimiters);\n\t\t\t\t\tp2=strtok (NULL, delimiters);\n\t\t\t\t\tp3=strtok (NULL, delimiters);\n\t\t\t\t\tp4=strtok (NULL, delimiters);\n\t\t\t\t\tp5=strtok (NULL, delimiters);\n\t\t\t\t\tif(p1!=NULL \u0026\u0026 p2!=NULL \u0026\u0026 p3!=NULL \u0026\u0026 p4!=NULL \u0026\u0026 p5!=NULL)\t\t\t\t\t\t\n\t\t\t\t\t\tMoveRobot(atoi(p1),atoi(p2),atoi(p3),atoi(p4),atoi(p5),BLOCKING_MOVE);\n\t\t\t\tbreak; \n\t\t\t\tcase SLOWMOVE_CMD  :\n\t\t\t\t\tp1=strtok (NULL, delimiters);\n\t\t\t\t\tp2=strtok (NULL, delimiters);\n\t\t\t\t\tp3=strtok (NULL, delimiters);\n\t\t\t\t\tp4=strtok (NULL, delimiters);\n\t\t\t\t\tp5=strtok (NULL, delimiters);\n\t\t\t\t\tAdd=atoi(p1);\n\t\t\t\t\tLength=atoi(p3);\n\t\t\t\t\tStart=atoi(p2);\n\t\t\t\t\tDelay=atoi(p4);\n\t\t\t\t\tmapped[Add]=Start;\n\t\t\t\t\t//printf(\"\\n %d %d \\n\",Add,Length);\n\t\t\t\t\tfor(i=0;i\u003cLength;i++)\n\t\t\t\t\t{\n\t\t\t\t\t\tfor(j=0;j\u003cDelay;j++)\t\t\t\n\t\t\t\t\t\t\tmapped[Add]=Start+i;\n\t\t\t\t\t}\n\t\t\t\tbreak;\n\t\t\t\tcase READ_CMD  :\n\t\t\t\t\tp1=strtok (NULL, delimiters);\n\t\t\t\t\tp2=strtok (NULL, delimiters);\n\t\t\t\t\tif((p1==NULL) | (p2==NULL))\n\t\t\t\t\t{\n\t\t\t\t\t\t//printf(\"\\n %d %d need addres and length\",atoi(p1),atoi(p2));\n\t\t\t\t\t\tbreak;\n\t\t\t\t\t}\n\t\t\t\t\tAdd=atoi(p1);\n\t\t\t\t\tLength=atoi(p2); \n\t\t\t\t\t//printf(\"\\n %d %d \\n\",Add,Length);\n\t\t\t\t\tfor(i=0;i\u003cLength;i++)\n\t\t\t\t\t{\n\t\t\t\t\t\t\n\t\t\t\t\t\t//printf(\"\\n  %x %d\",CalTables[Add+i],CalTables[Add+i]);\n\t\t\t\t\t\t//printf(\"\\n %x \",mapped[Add+i]);\n\t\t\t\t\t}\n\t\t\t\tbreak; \n\t\t\t\tcase WRITE_CMD  :\n\t\t\t\t\tp1=strtok (NULL, delimiters);\n\t\t\t\t\tp2=strtok (NULL, delimiters);\n\t\t\t\t\tif((p1==NULL) | (p2==NULL))\n\t\t\t\t\t{\n\t\t\t\t\t\t//printf(\"\\n %d %d need addres and data\",atoi(p1),atoi(p2));\n\t\t\t\t\t}\n\t\t\t\t\t//printf(\"\\n %d %d \\n\",atoi(p1),atoi(p2));\n\t\t\t\t\ti=atoi(p1);\n\t\t\t\t\tj=atoi(p2);\n\t\t\t\t\tif(i==COMMAND_REG)\n\t\t\t\t\t\tCmdVal=j;\n\t\t\t\t\tmapped[i]=j;\n\t\t\t\t\tif(i==ACCELERATION_MAXSPEED)\n\t\t\t\t\t{\n\t\t\t\t\t\tmaxSpeed=j \u0026 0b00000000000011111111111111111111;\n\t\t\t\t\t\tcoupledAcceleration=(j \u0026 0b00000011111100000000000000000000) \u003e\u003e 20;\n\t\t\t\t\t\t//startSpeed=0,\n\t\t\t\t\t\t\n\t\t\t\t\t}\n\t\t\t\tbreak; \n\t\t\t\tcase EXIT_CMD  :\n\t\t\t\t\tmapped[FINE_ADJUST_BASE]=0;\n\t\t\t\t\tmapped[FINE_ADJUST_END]=0;\n\t\t\t\t\tmapped[FINE_ADJUST_PIVOT]=0;\n\t\t\t\t\tmapped[FINE_ADJUST_ANGLE]=0;\n\t\t\t\t\tmapped[FINE_ADJUST_ROT]=0;\n\t\t\t\t\treturn 1;\n\t\t\t\tbreak; \n\t\t\t\t\n\t\t\n\t\t\t\t\n\t\t\t\tdefault : \n\t\t\t\t  //printf(\"\\nunrecognized command\");\n\t\t\t\t  break;\n\t\t\t}\n\t\t\treturn 0;\n\t\t}\n\t\telse\n\t\t\treturn 1;\n\t}\n\treturn 1;\n}\n\nint main(int argc, char *argv[]) {\n\n  int fd,mfd,err;\n  \n  int size;\n  int DefaultMode;\n  int CalTblSize = 32*1024*1024; \n\n\n  if (argc != 4) {\n    fprintf(stderr, \"Usage: %s Needs init mode, Master/Slave and RunMode\\n\", argv[0]);\n    exit(1);\n  }\n\n  DefaultMode = atoi(argv[1]);\n  size = 4095;\n  RunMode=atoi(argv[3]);\n  ServerMode = atoi(argv[2]);\n  if (size \u003c= 0) {\n    fprintf(stderr, \"Bad size: %d\\n\", size);\n    exit(1);\n  }\n\n  fd = open(\"/dev/uio1\", O_RDWR);\n  if (fd \u003c 0) {\n    perror(\"Failed to open devfile\");\n    return 1;\n  }\n\n  map_addr = mmap( NULL, size, PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0);\n\n  if (map_addr == MAP_FAILED) {\n    perror(\"Failed to mmap\");\n    return 1;\n  }\n\n  mapped = map_addr;\n  \n  \n  mfd = open(\"/dev/mem\", O_RDWR);\n  map_addrCt = mmap(NULL, CalTblSize, PROT_READ | PROT_WRITE, MAP_SHARED, mfd, 0x3f000000);\n\n\n   if (map_addrCt == MAP_FAILED) {\n\t\tprintf(\"\\nCant Map Callibration Tables \\n\");\n    perror(\"Failed to mmap High memory\");\n    return 1;\n  }\n  CalTables = map_addrCt;\n\n\n//  Addr= = atoi(argv[3]);\n//  Dta= = atoi(argv[4]);\n//  mapped[Addr] = Dta;\n\tsetDefaults(DefaultMode);\n//\tif(DefaultMode ==2 )\n\tif(ServerMode==1)\n\t{\n\t\n\t\terr = pthread_create(\u0026(tid[0]), NULL, \u0026StartServerSocket,  (void*)\u0026ThreadsExit);\n\t\tif (err != 0)\n\t\t{\n\t\t\t//printf(\"\\ncan't create thread :[%s]\", strerror(err));\n\t\t\treturn 1;\n\t\t}\n\t\telse\n\t\t{\n\t\t\t//printf(\"\\n Begin Socket Server Thread\\n\");\n\t\t}\n\t}\n\telse if(ServerMode==2)\n\t{\n\t\n\t\terr = pthread_create(\u0026(tid[1]), NULL, \u0026StartClientSocket, NULL);\n\t\tif (err != 0)\n\t\t{\n\t\t\t//printf(\"\\ncan't create thread :[%s]\", strerror(err));\n\t\t\treturn 1;\n\t\t}\n\t\telse\n\t\t{\n\t\t\t//printf(\"\\n Begin Socket Client Thread\\n\");\n\t\t}\n\t}\n\telse if(ServerMode==3)\n\t{\n\t\tif(mapped[SENT_BASE_POSITION]!=0)\n\t\t{\n\t   \t\tmunmap(map_addr, size);\n\t\t\tmunmap(map_addrCt, CalTblSize);\n \t\t\tclose(fd);\n\t\t\tclose(mfd);\n\t\t\treturn 0;   \n\t\t}\n\t\tmapped[BASE_POSITION]=1;\n\n\t\tMoveRobot(0,0,0,50000,50000,BLOCKING_MOVE);\n\t    MoveRobot(0,0,0,0,0,BLOCKING_MOVE);\n\t    MoveRobot(0,0,0,50000,-50000,BLOCKING_MOVE);\n\t    MoveRobot(0,0,0,0,0,BLOCKING_MOVE);\n\t    MoveRobot(0,0,0,50000,50000,BLOCKING_MOVE);\n\t    MoveRobot(0,0,0,0,0,BLOCKING_MOVE);\n\t    MoveRobot(0,0,0,50000,-50000,BLOCKING_MOVE);\n\t    MoveRobot(0,0,0,0,0,BLOCKING_MOVE);\n\t\t\n\t\terr = pthread_create(\u0026(tid[0]), NULL, \u0026StartServerSocketDDE,  (void*)\u0026ThreadsExit);\n\t\tif (err != 0)\n\t\t{\n\t\t\t//printf(\"\\ncan't create thread :[%s]\", strerror(err));\n\t\t\treturn 1;\n\t\t}\n\t\telse\n\t\t{\n\t\t\t//printf(\"\\n Begin Socket Server Thread For DDE\\n\");\n\t\t}\n\t}\n\tif(RunMode==1 || RunMode==2)\n\t{\n\t\terr = pthread_create(\u0026(tid[2]), NULL, \u0026RealtimeMonitor, (void*)\u0026ThreadsExit );\n\t\tif (err != 0)\n\t\t{\n\t\t\t//printf(\"\\ncan't create thread :[%s]\", strerror(err));\n\t\t\treturn 1;\n\t\t}\n\t\telse\n\t\t{\n\t\t\t//printf(\"\\n Begin Realtime Monitor Thread\\n\");\n\t\t}\t\t\n\t}\n\n\t\n\t\n    if(ServerMode==3)\n\t{\n\t\twhile(1){} //loop forever\n\t}\n\twhile(getInput()==0);\n\tThreadsExit=0;\n\tsleep(1);\n\n\n\t//printf(\"\\nExiting \\n\");\n\n  munmap(map_addr, size);\n  munmap(map_addrCt, CalTblSize);\n  \n\n  close(fd);\n   close(mfd);\n  return 0;\n}\nGNU General Public License v3.0 \n","project_url":"https://awesome.ecosyste.ms/api/v1/projects/github.com%2Fengineermichael%2F-robotic-arm---haddington-dynamics-robotics-engineering-","html_url":"https://awesome.ecosyste.ms/projects/github.com%2Fengineermichael%2F-robotic-arm---haddington-dynamics-robotics-engineering-","lists_url":"https://awesome.ecosyste.ms/api/v1/projects/github.com%2Fengineermichael%2F-robotic-arm---haddington-dynamics-robotics-engineering-/lists"}