engineering.nyu.eduengineering.nyu.edu/mechatronics/projects/ME3484/2003… · Web viewSookram...

Remote Robot Arm ManipulationSookram Sobhan, Imran Ahmed, Omar Ramos

Team 8

Abstract

The focus of Remote Robot Arm Manipulation was the integration of a microcontroller

and a LAN. This was accomplished by having two modes of control for the arm. The first, a

joystick connected to a PC on the LAN running the right software. The second mode of control

is through a web based application. In both cases, when action is performed by the user, packets

are sent and routed to the microcontroller which will handle the command sequence

appropriately.

Introduction

The advent of the internet created an infinite amount of possibilities to enhance all

aspects of life. However, a glass ceiling has been reached, and development over the internet

has been stagnant. Integration of microcontrollers with the internet is leading the new boom, and

therefore we will explore this technology.

Using the Basic Stamp, BS2p40, as our microcontroller, an interface will be created in

over a LAN in which a person can manipulate the movement of a robotic arm. When the BS2

receives a command, it will trigger various motors which results in the actuation of the robotic

arm. By itself, the BS2 cannot receive data from the internet. An additional interface to the

Embedded Ethernet, an Ethernet controller will be made.

Data transmitted through a network is sent as packets. A packet is essentially composed

of source and destination addresses as well as data for the destination. The Ethernet controller is

assigned a static Internet Protocol address (IP), which can be thought of as the IP of the BS2.

When the Ethernet controller receives a packet, the BS2 decomposes the packet and the

appropriate action is performed.

To send commands to the BS2, two modes of input will be employed. The first, a

joystick connected to a PC on the same LAN as BS2. The second mode of input is through a

web application. Both modes allow the user to move the arm with three degrees of freedom,

polar movement and gripping action. An LED attached to the arm can be controlled to brighten

the area of interest.

Hardware Specifications

Components for Arm

Lego Mindstorm Kit

BS2p40

40 Pin DIP Package

32 I/O

32 Bytes RAM (26 usable)

2 KB EEPROM

Embedded Ethernet

16 Pin Board

10BaseT Connection (10 MB)

DC Motors

1/9 V Min/Max

350 mA Max

Transistors

TIP3055 – NPN Power Transistor

TIP120 – Darlington Pair Transistor

IRF511 – N Channel Power MOSFET

Block Diagram

A block diagram of the overall system is shown below. Essentially, there are two modes

of input, a joystick and a web application. The PC running either input must be connected to the

LAN. Also connected to the LAN is the Embedded Ethernet. When either input is triggered,

packets are sent to the Embedded Ethernet. When the Embedded Ethernet receives a packet, the

Basic Stamp decodes it and performs the appropriate actions on the motors controlling the arm.

Physical Representation

Circuits

The circuits shown below were used to control the direction of the dc motors. Two H-

Bridge circuits were used and one inverter circuit was used. One H-Bridge was designed using

NPN Power Transistor and the other using N Channel Power MOSFETs. The inverter circuit

was made using Darlington transistors. The H-Bridges were used to control the polar movement

of the arm. The inverter circuit was designed to control the gripper motor. Each circuit was

designed to provide sufficient torque. Therefore, voltage was well as sufficient current was

needed. Different transistor was used to show the flexibility in the design process. The only

difference between the H-Bridges is the use of resistors for the BJT design. This is because the

BJTs require a small current as well as voltage to switch.

For the BJT H-Bridge, non-inverting operational amplifiers with designed gain of two

were used to amplify the input signal from the Basic Stamp. This was done because the required

input current and voltage required for the power transistors to switch is higher than normal small

signal transistors.

The three circuits used are shown below.

+V312V

+V212V

+V

In25V

+V

In15V

R47k

Rf47k

R47k

Rf47k

+

In2Op Amp2

+

In1Op Amp1

+V16V

Motor

R41k

R31k

R21kR1

1k

Q4TIP3055

Q3TIP3055

Q2TIP3055Q1

TIP3055

Circuit 1. BJT H-Bridge

Q4IRF511

Q2IRF511

Q3IRF511

Q1IRF511

+V

In25V

+VIn15V

+V16V

Motor

Circuit 2. MOSFET H-Bridge

Circuit 3. Darlington Inverter

Code Analysis

The majority of this project involved coding. For the BS2/Embedded Ethernet interface,

PBasic was utilized. The joystick application was developed using C++ and MFC programming.

Finally, for the web application, a Visual Basic script was made use of.

Since the coding is quite lengthy, only relevant sections will be discussed. All code is

attached and an electronic copy is available.

The BS2/Embedded Ethernet Interface

To the interaction between the BS2 and the Embedded Ethernet, it is wise to make sure

proper connection is made. We also need to reset the chip and initialize the functionality we

wish to exploit. All this is done with the piece of code below. The read, write, and enable pins

are active low, and is therefore initially set to high. The data bus that will be used to transfer

data are pins P8-P15 on the BS2 and are initially set to inputs. These pins will vary between

input and output throughout the program. The pins used for address specification on the

Embedded Ethernet are pins P0-P3 and are set to outputs, as they will remain for the lifetime of

the program.

In the function verChip, BS2 checks if the Embedded Ethernet is connected properly. It

also writes data to the chip such as product identification and protocols. The resetChip function

does what the function name suggests. The initChip function initializes some functionality such

as using 10baseT and the chip’s MAC address.

high rdhigh wrhigh aendirh = 0 dira = %1111 gosub verChipgosub resetChipgosub initChip

The following lines of code start the decomposition of a packet. The packet is received

as a series of bytes. Therefore when calls recvWord are made, two bytes are retrieved. The two

bytes are stored in a word variable, dataW. There is no real indication in the code as to what is

being received, but essentially there is a sequence that is followed as to what we should receive

after each calling of recvWord. For the block shown below, the first word is read and if we did

receive data, we will continue. The length of the packet is then received in the next calling of the

function. The two sections of calling recvWord three times returns the MAC address of the

source and destination. Finally, the packet type is determined. The type could be TCP, UDP,

EGP, or ICMP. For our case, only UDP packets will be sent. A person can ping the BS2, in

which ICMP would be used.

Loop:offsetW = ppRxEvtgosub readPPif dataH.bit0 = 0 then loop

addr = portRxTxData+1gosub ioReadaddr = portRxTxDatagosub ioReadaddr = portRxTxData+1

gosub ioReadlengthH = valueaddr = portRxTxDatagosub ioRead

gosub recvWord gosub recvWord gosub recvWord

gosub recvWordgosub recvWordgosub recvWord

gosub recvWordpacketType = dataW

The last major block of code worth discussing is shown below. It is here where the real

“action” takes place, the data of the packet is debugged. The true length of the packet is no use

since one command is being sent on the user side at a time. Therefore, only the first four words

sent will be read. This was done on purpose, because a UDP packet consists of 42 bytes, nearly

all of which are no use to us. To ensure only four words are read, the for loop runs four times.

Each time, one word is retrieved and compared with predetermined values. The values were

selected as the hexadecimal representation of the action being sent. For example, sending the

command “right” is equivalent to sending $7200 in hex. Whenever an action is caught by one of

the branches, the appropriate tasks are performed. The tasks involved here are the pulsing of

motors. An example of one of the branches is shown below as well. When the command “right”

is found, the appropriate pins are pulsed and the arms turns eastward.

debug "Packet Data:",cr

for i = 0 to 4

gosub recvWorddebug HEX4 dataW, " "

if dataW = $7200 then rightif dataW = $6C00 then leftif dataW = $7500 then upif dataW = $6400 then downif dataW = $6100 then gripif dataW = $6F00 then openif dataW = $6300 then closeif dataW = $7700 then lon 'lights onif dataW = $7300 then loff 'lights off

checkend:next

debug cr,crgoto loop

right:debug "RIGHT"auxiohigh 14low 15pause 200low 14mainio

goto checkend

The Joystick Interface

There are four files that bring together the joystick interface. The four files are part of a

Microsoft Foundation Class (MFC) application.

To block of code below shows how the USB port is sampled how the sending of a packet

is accomplished. In the first line, the Y position is attained. The next line changes the visual

interface with the new value. The thresholds for the Y position are then declared and compared

against the current Y position value. If the joystick is pressed in the negative Y direction, the

value ‘3’ will be sent as a parameter in the packetize function. Alternatively, if the joystick is in

the positive Y direction, the value ‘2’ will be packetized.

pCPCYAxis->SetPos(0xFFFF-ji.dwYpos);sprintf(lpszText,"Y-Axis\n%d",ji.dwYpos);

float y_down_threshhold = 50000;float y_up_threshhold = 15000;

if (ji.dwYpos < y_up_threshhold){

packetize(2);}

if (ji.dwYpos > y_down_threshhold){

packetize(3);}

Packetize is a function created to send a specific packet according to the direction pressed

by the user. Below shows a portion of the packetize code. The code shown below is augmented

to the processing of the Y direction only. To send a packet, a socket has to be established. A

socket has two things related to it, an IP address and a port number. In laymen terms, a socket is

like a telephone, with different extension numbers.

Before the data can be sent, it must be converted to Big Endian. This is a standard that

must be followed. This is accomplished by calling htons(). The address and port number we

want to send a packet to is then embedded in the sockaddr_in struct. Finally, to send the packet,

the send() is used.

int packetize(int action) {

short command;struct sockaddr_in sin; WSADATA wsaData; SOCKET s;

if(action == 2) command = htons(29952);

if(action == 3) command = htons(25600);

sin.sin_addr.S_un.S_addr = inet_addr("192.168.0.2"); sin.sin_port = htons(1000);

send(s,(const char *)&command,2,0);closesocket(s); return(0);

}

The Web Interface

The web based control is done with an HTML page with embedded VBScript. There are

8 buttons on the web page: up, down, left, right, open, close, lights on, lights off. When a button

is pressed, a corresponding VBScript section is executed. The purpose of the VBScript code is

to create a socket connection with the embedded ethernet controller, and send a packet across

this socket to the ethernet controller. We want this all to be done using the UDP protocol. This

packet will then be read by the BS2 and an action, or no action, will be taken based on the value.

We used a library to remove the very large overhead of creating a socket and sending a packet by

using a library called Xceed Winsock Library (www.xceedsoft.com).

http://www.xceedsoft.com/

<SCRIPT FOR="left" EVENT="onClick" LANGUAGE="VBScript"> ' This function sends a packet on port 1000 to the ethernet chip connected to ' the BS2

' The Protocols class is a collection of all available protocols' on the machine. We ask for a wafInet(2) protocol, of type UDP(17).

Dim xProtocolsDim xProtocol

Set xProtocols = CreateObject(XceedSoftware.XceedWinsock.Protocols.1")Set xProtocol = xProtocols.GetProtocol( 2, 0, 17 )

' The SocketFactory is the creator for sockets. It contains 5 ' socket-creation methods. We create a ConnectionlessSocket.

Dim xSocketDim xSocketFactory

Set xSocketFactory = CreateObject("XceedSoftware.XceedWinsock.SocketFactory.1")Set xSocket = xSocketFactory.CreateConnectionlessSocket( xProtocol, 0 )

' The server's address is hardcoded here. Change the address below to' accomodate your setup.

Dim xAddress

Set xAddress = CreateObject( "XceedSoftware.XceedWinsock.InetAddress.1" )xAddress.SetAddressString( "128.238.129.106:1000" )

Call xSocket.SendBytesTo( xAddress, "left", 0 )

</SCRIPT>

To use the web interface the user must go to the main web site that contains the live video

feed of the robot arm and download 2 items: the Xceed Winsock Library and the HTML control

page which must be run locally.

Cost Analysis

BS2p40 $99

BS2p40 Board $89

Embedded Ethernet $75

Joystick Controller $30

Transistors $28

Power Supplies $20

Conclusion

There are a few problems the in the design of the robotic arm. First, no feedback was

employed in the design. Movement pass the extreme in any range of motion would cause the

arm to break. Secondly, the arm has only one speed. This is because a fixed pulse was

established by the BS2. This problem could be solved with Pulse Width Modulation (PWM).

This would allow the arm to move faster when the control is pressed in the extreme directions.

Finally, the arm can only move in any one direction at a time. This can be solved by using some

circuitry to control specific directions of motion. All of these problems can easily be overcome,

as they will be in future improvements.

Code

BS2 Code

'{$STAMP BS2p}'' Crystal CS8900 PacketPage equates'portRxTxData con $00 'Receive/Transmit data (port 0)portRxTxData1 con $02 'Receive/Transmit data (port 0)portTxCmd con $04 'Transmit CommnadportTxLength con $06 'Transmit LengthportISQ con $08 'Interrupt status queue

portPtr con $0a 'PacketPage pointerportData con $0c 'PacketPage data (port 0)portData1 con $0e 'PacketPage data (port 1)'' CS8900 PacketPage Offsets 'ppEISA con $0000 'EISA Registration number of CS8900ppProdID con $0002 'Product ID NumberppIOBase con $0020 'I/O Base AddressppIntNum con $0022 'Interrupt number (0,1,2, or 3)ppMemBase con $002C 'Memory Base address register (20 bit)ppRxCfg con $0102 'Receiver ConfigurationppRxCtl con $0104 'Receiver ControlppTxCfg con $0106 'Transmit ConfigurationppBufCfg con $010A 'Buffer ConfigurationppLineCtl con $0112 'Line ControlppSelfCtl con $0114 'Self ControlppBusCtl con $0116 'Bus ControlppTestCtl con $0118 'Test ControlppISQ con $0120 'Interrupt status queueppRxEvt con $0124 'Receiver EventppTxEvt con $0128 'Transmitter EventppBufEvt con $012C 'Buffer EventppRxMiss con $0130 'Receiver Miss CounterppTxCol con $0132 'Transmit Collision CounterppLineSt con $0134 'Line StatusppSelfSt con $0136 'Self StatusppBusSt con $0138 'Bus StatusppTxCmd con $0144 'Transmit Command RequestppTxLength con $0146 'Transmit LengthppIndAddr con $0158 'Individual Address (IA)ppRxStat con $0400 'Receive StatusppRxLength con $0402 'Receive LengthppRxFrame con $0404 'Receive Frame LocationppTxFrame con $0A00 'Transmit Frame Location'' Register Numbers 'REG_NUM_MASK con $003FREG_NUM_RX_EVENT con $0004REG_NUM_TX_EVENT con $0008REG_NUM_BUF_EVENT con $000CREG_NUM_RX_MISS con $0010REG_NUM_TX_COL con $0012'' Self Control Register'SELF_CTL_RESET con $0040SELF_CTL_HC1E con $2000SELF_CTL_HCB1 con $8000'' Self Status Register 'SELF_ST_INIT_DONE con $0080SELF_ST_SI_BUSY con $0100SELF_ST_EEP_PRES con $0200SELF_ST_EEP_OK con $0400

SELF_ST_EL_PRES con $0800'' Bus Control Register 'BUS_CTL_USE_SA con $0200BUS_CTL_MEM_MODE con $0400BUS_CTL_IOCHRDY con $1000BUS_CTL_INT_ENBL con $8000'' Bus Status Register 'BUS_ST_TX_BID_ERR con $0080BUS_ST_RDY4TXNOW con $0100'' Line Control Register 'LINE_CTL_RX_ON con $0040LINE_CTL_TX_ON con $0080LINE_CTL_AUI_ONLY con $0100LINE_CTL_10BASET con $0000'' Test Control Register 'TEST_CTL_DIS_LT con $0080TEST_CTL_ENDEC_LP con $0200TEST_CTL_AUI_LOOP con $0400TEST_CTL_DIS_BKOFF con $0800TEST_CTL_FDX con $4000'' Receiver Configuration Register 'RX_CFG_SKIP con $0040RX_CFG_RX_OK_IE con $0100RX_CFG_CRC_ERR_IE con $1000RX_CFG_RUNT_IE con $2000RX_CFG_X_DATA_IE con $4000'' Receiver Event Register 'RX_EVENT_RX_OK con $0100RX_EVENT_IND_ADDR con $0400RX_EVENT_BCAST con $0800RX_EVENT_CRC_ERR con $1000RX_EVENT_RUNT con $2000RX_EVENT_X_DATA con $4000''Receiver Control Register 'RX_CTL_PROMISCUOUS con $0080RX_CTL_RX_OK_A con $0100RX_CTL_MCAST_A con $0200RX_CTL_IND_A con $0400RX_CTL_BCAST_A con $0800RX_CTL_CRC_ERR_A con $1000RX_CTL_RUNT_A con $2000RX_CTL_X_DATA_A con $4000'

'Transmit Configuration Register 'TX_CFG_LOSS_CRS_IE con $0040TX_CFG_SQE_ERR_IE con $0080TX_CFG_TX_OK_IE con $0100TX_CFG_OUT_WIN_IE con $0200TX_CFG_JABBER_IE con $0400TX_CFG_16_COLL_IE con $8000TX_CFG_ALL_IE con $8FC0''Transmit Event Register 'TX_EVENT_TX_OK con $0100TX_EVENT_OUT_WIN con $0200TX_EVENT_JABBER con $0400TX_EVENT_16_COLL con $1000'' Transmit Command Register'TX_CMD_START_5 con $0000TX_CMD_START_381 con $0080TX_CMD_START_1021 con $0040TX_CMD_START_ALL con $00C0TX_CMD_FORCE con $0100TX_CMD_ONE_COLL con $0200TX_CMD_NO_CRC con $1000TX_CMD_NO_PAD con $2000''Buffer Configuration Register 'BUF_CFG_SW_INT con $0040BUF_CFG_RDY4TX_IE con $0100BUF_CFG_TX_UNDR_IE con $0200

'' The IP address and MAC address can be changed to whatever is appropriate'IP1 con 128 'first octet of IP addressIP2 con 238 'second octet of IP addressIP3 con 129 'third octet of IP addressIP4 con 106 'fourth octet of IP address

MAC1 con $00 '\MAC2 con $00 ' \MAC3 con $00 ' \MAC4 con $12 ' 48 bit IEEE OUI (Organizationally Unique Identifier)MAC5 con $34 ' /MAC6 con $56 '/

rd con 4 wr con 5 aen con 6

addrBusOut var outadataBusIn var inhdataBusOut var outh

dataW var worddataH var dataW.highbytedataL var dataW.lowbyte

addr var nib

value var byte

temp var byte

offsetW var wordoffsetH var offsetW.highbyteoffsetL var offsetW.lowbyte

lengthW var wordlengthH var lengthW.highbytelengthL var lengthW.lowbyte

srcMAC1W var wordsrcMAC1H var srcMAC1W.highbytesrcMAC1L var srcMAC1W.lowbyte



packetType var word

srcIP1 var wordsrcIP2 var word

i var word

start:high rdhigh wrhigh aendirh = 0 dira = %1111 gosub verChipgosub resetChipgosub initChip

loop:offsetW = ppRxEvtgosub readPPif dataH.bit0 = 0 then loop

addr = portRxTxData+1gosub ioReadaddr = portRxTxDatagosub ioRead

addr = portRxTxData+1gosub ioReadaddr = portRxTxDatagosub ioRead

gosub recvWord gosub recvWord gosub recvWord

gosub recvWordgosub recvWordgosub recvWord

gosub recvWordpacketType = dataW

if packetType <> $0806 then otherType

goto loop

otherType:if packetType <> $0800 then discardAndContinue

gosub recvWordgosub recvWord gosub recvWordgosub recvWordgosub recvWord

if dataL <> 1 then notICMPdebug "Protocol=ICMP",crgoto nextHdrField

notICMP:if dataL <> 6 then notTCPdebug "Protocol=TCP",crgoto nextHdrField

notTCP:if dataL <> 8 then notEGPdebug "Protocol=EGP",crgoto nextHdrField

notEGP:if dataL <> $11 then nextHdrFielddebug "Protocol=UDP",crgoto nextHdrField

nextHdrField:

gosub recvWord

debug "Src IP Address="gosub dumpIP

debug "Dest IP Address="gosub dumpIP

debug "Packet Data:",cr

for i = 0 to 4

gosub recvWorddebug HEX4 dataW, " "

if dataW = $7200 then rightif dataW = $6C00 then leftif dataW = $7500 then upif dataW = $6400 then downif dataW = $6100 then gripif dataW = $6F00 then openif dataW = $6300 then closeif dataW = $7700 then lon 'lights onif dataW = $7300 then loff 'lights off

checkend:next

debug cr,crgoto loop

dumpIP: gosub recvWorddebug dec dataH,".",dec dataL,"."gosub recvWorddebug dec dataH,".",dec dataL,crreturn

discardAndContinue:gosub dropFramegoto loop

recvWord:addr = portRxTxDatagosub ioReaddataH = valueaddr = portRxTxData+1gosub ioReaddataL = valuereturn

dropFrame:offsetW = ppRxCtlgosub readPPdataW = dataW | RX_CFG_SKIPgosub writePPreturn

initChip:offsetW = ppLineCtldataW = LINE_CTL_10BASETgosub writePPoffsetW = ppTestCtldataW = TEST_CTL_FDXgosub writePPoffsetW = ppRxCtldataW = RX_CTL_RX_OK_A|RX_CTL_PROMISCUOUSgosub writePP

offsetW = ppIndAddrdataW = MAC2<<8|MAC1gosub writePPoffsetW = ppIndAddr+2dataW = MAC4<<8|MAC3gosub writePPoffsetW = ppIndAddr+4dataW = MAC6<<8|MAC5gosub writePP

offsetW = ppLineCtl gosub readPP dataL.bit6 = 1

dataL.bit7 = 1 gosub writePPreturn

resetChip:offsetW = ppSelfCtl 'send this command to embeddeddataW = SELF_CTL_RESET 'send this command to embeddedgosub writePP

resetWait:pause 1 ' wait 1 millisecondoffsetW = ppSelfCtl 'get the Self Control statusgosub readPPif dataL.bit6 = 1 then resetWait' if bit 6 cleared, chip is resetoffsetW = ppSelfSt 'get self statusgosub readPPif dataL.bit7 = 0 then resetWait debug "CS8900 RESET",crreturn

verChip:addr = portPtrgosub ioReaddataL = valueaddr = portPtr+1gosub ioReaddataH = valueif dataH.highnib = 3 then validChipdebug "The CS8900 is not present, verify all connections to

the board",crend

validChip:offsetW = ppEISAgosub readPPoffsetW = ppProdIDgosub readPPreturn

writePP:gosub setPPPointeraddr = portDatavalue = dataLgosub ioWrite

addr = portData+1value = dataHgosub ioWritereturn

readPP:gosub setPPPointeraddr = portDatagosub ioReaddataL = valueaddr = portData+1gosub ioReaddataH = valuereturn

setPPPointer:value = offsetLaddr = portPtrgosub ioWritevalue = offsetHaddr = portPtr+1gosub ioWritereturn

ioRead: dirh = 0 addrBusOut = addrlow aenlow rdvalue = dataBusInhigh rdhigh aenreturn

ioWrite:dirh = %11111111 dataBusOut = valueaddrBusOut = addrlow aenlow wrhigh wrhigh aenreturn

right:debug "RIGHT"auxiohigh 14low 15pause 200low 14mainiogoto checkend

left:debug "LEFT"auxiohigh 15low 14pause 200low 15mainio

goto checkendup:

debug "UP"auxiohigh 12low 13pause 400low 12mainiogoto checkend

down:debug "DOWN"auxiohigh 13low 12pause 100low 13mainiogoto checkend

grip:debug "GRIPPER"auxiopause 20mainiogoto checkend

open:debug "OPEN"auxiohigh 11low 10pause 100low 11mainiogoto checkend

close:debug "CLOSE"auxiolow 11high 10pause 100low 10mainiogoto checkend

lon:debug "LIGHTS ON"auxiohigh 0mainiogoto checkend

loff:debug "LIGHTS OFF"auxiolow 0mainiogoto checkend

Joystick Code

packetize.cpp

#include <winsock.h> #include <stdlib.h> #include <stdio.h> #include <string.h>

int packetize(int action) {

short command;struct sockaddr_in sin; WSADATA wsaData; SOCKET s;

if(action == 0) //rightcommand = htons(29184); // Make sure command is in network byte

order (big Endian)

if(action == 1) //leftcommand = htons(27648);

if(action == 2) //upcommand = htons(29952);

if(action == 3) //downcommand = htons(25600);

if(action == 4) //Triggercommand = htons(24832);

if(action == 5) //opencommand = htons(28416);

if(action == 6) //closecommand = htons(25344);

if(action == 7) //light oncommand = htons(30464);

if(action == 8) //light offcommand = htons(29440);

if (WSAStartup(0x101,&wsaData) == SOCKET_ERROR) {

printf("WSAStartup() failed, rc=%d\n",WSAGetLastError()); WSACleanup(); return -1;

} sin.sin_addr.S_un.S_addr = inet_addr("192.168.0.2");

sin.sin_family = AF_INET; sin.sin_port = htons(1000);

s = socket(AF_INET,SOCK_DGRAM,0); if(s<0){

printf("Couldn't get a socket, rc=%d\n",WSAGetLastError()); WSACleanup(); return -1;

}

if(connect(s,(struct sockaddr*)&sin,sizeof(sin)) == SOCKET_ERROR) {

printf("connect() failed, rc=%d\n",WSAGetLastError()); WSACleanup(); return -1;

}

send(s,(const char *)&command,2,0);closesocket(s); WSACleanup(); return(0);

}

JoyShow.h

#if!defined(AFX_JOYSHOW_H__3714E689_5F55_11D4_92D1_00105A14507C__INCLUDED_)#define AFX_JOYSHOW_H__3714E689_5F55_11D4_92D1_00105A14507C__INCLUDED_

#if _MSC_VER > 1000#pragma once#endif // _MSC_VER > 1000

#ifndef __AFXWIN_H__#error include 'stdafx.h' before including this file for PCH

#endif

#include "resource.h" // main symbols

/////////////////////////////////////////////////////////////////////////////// CJoyShowApp:// See JoyShow.cpp for the implementation of this class//

class CJoyShowApp : public CWinApp{public:

CJoyShowApp();

// Overrides// ClassWizard generated virtual function overrides//{{AFX_VIRTUAL(CJoyShowApp)public:virtual BOOL InitInstance();//}}AFX_VIRTUAL

// Implementation

//{{AFX_MSG(CJoyShowApp)// NOTE - the ClassWizard will add and remove member functions

here.// DO NOT EDIT what you see in these blocks of generated code !

//}}AFX_MSGDECLARE_MESSAGE_MAP()

};

/////////////////////////////////////////////////////////////////////////////

//{{AFX_INSERT_LOCATION}}// Microsoft Visual C++ will insert additional declarations immediately before the previous line.

#endif // !defined(AFX_JOYSHOW_H__3714E689_5F55_11D4_92D1_00105A14507C__INCLUDED_)

JoyShow.cpp

#include "stdafx.h"#include "JoyShow.h"#include "JoyShowDlg.h"#ifdef _DEBUG#define new DEBUG_NEW#undef THIS_FILEstatic char THIS_FILE[] = __FILE__;#endif

/////////////////////////////////////////////////////////////////////////////// CJoyShowApp

BEGIN_MESSAGE_MAP(CJoyShowApp, CWinApp)//{{AFX_MSG_MAP(CJoyShowApp)

// NOTE - the ClassWizard will add and remove mapping macros here.// DO NOT EDIT what you see in these blocks of generated code!

//}}AFX_MSGON_COMMAND(ID_HELP, CWinApp::OnHelp)

END_MESSAGE_MAP()

/////////////////////////////////////////////////////////////////////////////// CJoyShowApp construction

CJoyShowApp::CJoyShowApp(){

// TODO: add construction code here,// Place all significant initialization in InitInstance

}

/////////////////////////////////////////////////////////////////////////////// The one and only CJoyShowApp object

CJoyShowApp theApp;

/////////////////////////////////////////////////////////////////////////////// CJoyShowApp initialization

BOOL CJoyShowApp::InitInstance(){

AfxEnableControlContainer();

// Standard initialization// If you are not using these features and wish to reduce the size// of your final executable, you should remove from the following// the specific initialization routines you do not need.

#ifdef _AFXDLLEnable3dControls(); // Call this when using MFC in a

shared DLL#else

Enable3dControlsStatic(); // Call this when linking to MFC statically#endif

CJoyShowDlg dlg;m_pMainWnd = &dlg;int nResponse = dlg.DoModal();if (nResponse == IDOK){

// TODO: Place code here to handle when the dialog is// dismissed with OK

}else if (nResponse == IDCANCEL){

// TODO: Place code here to handle when the dialog is// dismissed with Cancel

}

// Since the dialog has been closed, return FALSE so that we exit the// application, rather than start the application's message pump.return FALSE;

}

JoyShowDlg.h

#if !defined(AFX_JOYSHOWDLG_H__3714E68B_5F55_11D4_92D1_00105A14507C__INCLUDED_)#define AFX_JOYSHOWDLG_H__3714E68B_5F55_11D4_92D1_00105A14507C__INCLUDED_

#if _MSC_VER > 1000#pragma once#endif // _MSC_VER > 1000

/////////////////////////////////////////////////////////////////////////////// CJoyShowDlg dialog

class CJoyShowDlg : public CDialog{// Constructionpublic:

CJoyShowDlg(CWnd* pParent = NULL); // standard constructor

// Dialog Data//{{AFX_DATA(CJoyShowDlg)enum { IDD = IDD_JOYSHOW_DIALOG };

// NOTE: the ClassWizard will add data members here//}}AFX_DATA

// ClassWizard generated virtual function overrides//{{AFX_VIRTUAL(CJoyShowDlg)protected:virtual void DoDataExchange(CDataExchange* pDX); // DDX/DDV support//}}AFX_VIRTUAL

// Implementationprotected:

HICON m_hIcon;

// Generated message map functions//{{AFX_MSG(CJoyShowDlg)virtual BOOL OnInitDialog();afx_msg void OnSysCommand(UINT nID, LPARAM lParam);afx_msg void OnPaint();afx_msg HCURSOR OnQueryDragIcon();afx_msg int OnCreate(LPCREATESTRUCT lpCreateStruct);afx_msg void OnTimer(UINT nIDEvent);afx_msg void OnButton1();//}}AFX_MSGDECLARE_MESSAGE_MAP()

};

//{{AFX_INSERT_LOCATION}}

JoyShowDlg.cpp

#include "stdafx.h"#include "mmsystem.h"#include "math.h"#include "packetize.h"

#include "JoyShow.h"#include "JoyShowDlg.h"

#ifdef _DEBUG#define new DEBUG_NEW#undef THIS_FILEstatic char THIS_FILE[] = __FILE__;#endif

/////////////////////////////////////////////////////////////////////////////

// CAboutDlg dialog used for App About

double LinearizeAxis(double dblRawInput, double dblScale, double dblDeadBand);

bool light=0;

class CAboutDlg : public CDialog{public:

CAboutDlg();

// Dialog Data//{{AFX_DATA(CAboutDlg)enum { IDD = IDD_ABOUTBOX };//}}AFX_DATA

// ClassWizard generated virtual function overrides//{{AFX_VIRTUAL(CAboutDlg)protected:virtual void DoDataExchange(CDataExchange* pDX); // DDX/DDV support//}}AFX_VIRTUAL

// Implementationprotected:

//{{AFX_MSG(CAboutDlg)//}}AFX_MSGDECLARE_MESSAGE_MAP()

};

CAboutDlg::CAboutDlg() : CDialog(CAboutDlg::IDD){

//{{AFX_DATA_INIT(CAboutDlg)//}}AFX_DATA_INIT

}

void CAboutDlg::DoDataExchange(CDataExchange* pDX){

CDialog::DoDataExchange(pDX);//{{AFX_DATA_MAP(CAboutDlg)//}}AFX_DATA_MAP

}

BEGIN_MESSAGE_MAP(CAboutDlg, CDialog)//{{AFX_MSG_MAP(CAboutDlg)

// No message handlers//}}AFX_MSG_MAP

END_MESSAGE_MAP()

/////////////////////////////////////////////////////////////////////////////// CJoyShowDlg dialog

CJoyShowDlg::CJoyShowDlg(CWnd* pParent /*=NULL*/): CDialog(CJoyShowDlg::IDD, pParent)

{//{{AFX_DATA_INIT(CJoyShowDlg)

// NOTE: the ClassWizard will add member initialization here

//}}AFX_DATA_INIT// Note that LoadIcon does not require a subsequent DestroyIcon in Win32m_hIcon = AfxGetApp()->LoadIcon(IDR_MAINFRAME);

}

void CJoyShowDlg::DoDataExchange(CDataExchange* pDX){

CDialog::DoDataExchange(pDX);//{{AFX_DATA_MAP(CJoyShowDlg)

// NOTE: the ClassWizard will add DDX and DDV calls here//}}AFX_DATA_MAP

}

BEGIN_MESSAGE_MAP(CJoyShowDlg, CDialog)//{{AFX_MSG_MAP(CJoyShowDlg)ON_WM_SYSCOMMAND()ON_WM_PAINT()ON_WM_QUERYDRAGICON()ON_WM_CREATE()ON_WM_TIMER()ON_BN_CLICKED(IDC_BUTTON1, OnButton1)//}}AFX_MSG_MAP

END_MESSAGE_MAP()

/////////////////////////////////////////////////////////////////////////////// CJoyShowDlg message handlers

static BOOL bJoyOK = 0;CProgressCtrl *pCPCXAxis ;CProgressCtrl *pCPCYAxis ;CProgressCtrl *pCPCZAxis ;CProgressCtrl *pCPCRAxis ;CProgressCtrl *pCPCRVel ;CProgressCtrl *pCPCLVel ;

BOOL CJoyShowDlg::OnInitDialog(){

CDialog::OnInitDialog();

// Add "About..." menu item to system menu.

// IDM_ABOUTBOX must be in the system command range.ASSERT((IDM_ABOUTBOX & 0xFFF0) == IDM_ABOUTBOX);ASSERT(IDM_ABOUTBOX < 0xF000);

CMenu* pSysMenu = GetSystemMenu(FALSE);if (pSysMenu != NULL){

CString strAboutMenu;strAboutMenu.LoadString(IDS_ABOUTBOX);if (!strAboutMenu.IsEmpty()){

pSysMenu->AppendMenu(MF_SEPARATOR);pSysMenu->AppendMenu(MF_STRING, IDM_ABOUTBOX, strAboutMenu);

}}

// Set the icon for this dialog. The framework does this automatically// when the application's main window is not a dialogSetIcon(m_hIcon, TRUE); // Set big iconSetIcon(m_hIcon, FALSE); // Set small icon

// TODO: Add extra initialization here

MMRESULT mmrResult;JOYCAPS jc;

mmrResult = joyGetDevCaps( JOYSTICKID1 , &jc, sizeof(JOYCAPS));switch (mmrResult){

case MMSYSERR_NODRIVER:MessageBox(TEXT("joyGetDevCaps returned MMSYSERR_NODRIVER"),

"Joystick Init Error",MB_ICONERROR);exit(1);

case MMSYSERR_INVALPARAM:MessageBox(TEXT("joyGetDevCaps returned

MMSYSERR_INVALPARAM"),"Joystick Init Error",MB_ICONERROR);

exit(1);

}bJoyOK = 1;SetTimer(1,10,NULL);

pCPCXAxis = (CProgressCtrl *)GetDlgItem( IDC_PROGRESS2);pCPCXAxis->SetRange32(0,0xFFFF);

pCPCYAxis = (CProgressCtrl *)GetDlgItem( IDC_PROGRESS1);pCPCYAxis->SetRange32(0,0xFFFF);

pCPCZAxis = (CProgressCtrl *)GetDlgItem( IDC_PROGRESS3);pCPCZAxis->SetRange32(0,0xFFFF);

pCPCRAxis = (CProgressCtrl *)GetDlgItem( IDC_PROGRESS4);pCPCRAxis->SetRange32(0,0xFFFF);

pCPCRVel = (CProgressCtrl *)GetDlgItem( IDC_PROGRESS_R_VEL);pCPCRVel->SetRange32(-100,100);

pCPCLVel = (CProgressCtrl *)GetDlgItem( IDC_PROGRESS_L_VEL);pCPCLVel->SetRange32(-100,100);

return TRUE; // return TRUE unless you set the focus to a control}

void CJoyShowDlg::OnSysCommand(UINT nID, LPARAM lParam){

if ((nID & 0xFFF0) == IDM_ABOUTBOX){

CAboutDlg dlgAbout;dlgAbout.DoModal();

}else{

CDialog::OnSysCommand(nID, lParam);}

}

void CJoyShowDlg::OnPaint() {

if (IsIconic()){

CPaintDC dc(this); // device context for painting

SendMessage(WM_ICONERASEBKGND, (WPARAM) dc.GetSafeHdc(), 0);

// Center icon in client rectangleint cxIcon = GetSystemMetrics(SM_CXICON);int cyIcon = GetSystemMetrics(SM_CYICON);CRect rect;GetClientRect(&rect);int x = (rect.Width() - cxIcon + 1) / 2;int y = (rect.Height() - cyIcon + 1) / 2;

// Draw the icondc.DrawIcon(x, y, m_hIcon);

}else{

CDialog::OnPaint();}

}

// The system calls this to obtain the cursor to display while the user drags// the minimized window.HCURSOR CJoyShowDlg::OnQueryDragIcon(){

return (HCURSOR) m_hIcon;}

int CJoyShowDlg::OnCreate(LPCREATESTRUCT lpCreateStruct) {

if (CDialog::OnCreate(lpCreateStruct) == -1)return -1;

// TODO: Add your specialized creation code herereturn 0;

}

void CJoyShowDlg::OnTimer(UINT nIDEvent) {

// TODO: Add your message handler code here and/or call defaultCHAR lpszText[81];JOYINFOEX ji;ji.dwSize = sizeof(JOYINFOEX);ji.dwFlags = JOY_RETURNALL;

MMRESULT mmrResult;mmrResult = joyGetPosEx( 0, &ji );

switch (mmrResult){

case MMSYSERR_NODRIVER:MessageBox(TEXT("joyGetDevCaps returned MMSYSERR_NODRIVER"),

"Joystick Init Error",MB_ICONERROR);exit(1);

case MMSYSERR_INVALPARAM:MessageBox(TEXT("joyGetDevCaps returned

MMSYSERR_INVALPARAM"),"Joystick Init Error",MB_ICONERROR);

exit(1);

case MMSYSERR_BADDEVICEID:MessageBox(TEXT("joyGetDevCaps returned

MMSYSERR_BADDEVICEID"),"Joystick Init Error",MB_ICONERROR);

exit(1);

case JOYERR_UNPLUGGED:if(bJoyOK==1)

{bJoyOK = 0;MessageBox(TEXT("joyGetDevCaps returned

JOYERR_UNPLUGGED"),"Joystick Init Error",MB_ICONERROR);

}break;

default:bJoyOK = 1;

}if(bJoyOK ==1){

if(ji.dwButtons&(1)){

SendDlgItemMessage(IDC_BUTTON1,BM_SETSTATE,TRUE);if (light==0){

packetize(7); //light onlight=1;

}else {

packetize(8); //light offlight=0;

}}

elseSendDlgItemMessage(IDC_BUTTON1,BM_SETSTATE,FALSE);

if(ji.dwButtons&(1<<1)){

SendDlgItemMessage(IDC_BUTTON2,BM_SETSTATE,TRUE);packetize(6); //close gripper

}else

SendDlgItemMessage(IDC_BUTTON2,BM_SETSTATE,FALSE);

if(ji.dwButtons&(1<<2))SendDlgItemMessage(IDC_RADIO1,BM_SETCHECK,BST_CHECKED);

elseSendDlgItemMessage(IDC_RADIO1,BM_SETCHECK,BST_UNCHECKED);

if(ji.dwButtons&(1<<3))SendDlgItemMessage(IDC_RADIO2,BM_SETCHECK,BST_CHECKED);

elseSendDlgItemMessage(IDC_RADIO2,BM_SETCHECK,BST_UNCHECKED);


SendDlgItemMessage(IDC_BUTTON3,BM_SETSTATE,TRUE);packetize(5); //open gripper

}else

SendDlgItemMessage(IDC_BUTTON3,BM_SETSTATE,FALSE);if(ji.dwButtons&(1<<5)){

SendDlgItemMessage(IDC_BUTTON4,BM_SETSTATE,TRUE);

}else




}else




}else


//HERE ARE THE VARIABLE NAMES that hold the position data// e.g. dwXpos, dwYpos......

pCPCXAxis->SetPos(ji.dwXpos);sprintf(lpszText,"X-Axis\n%d",ji.dwXpos);SendDlgItemMessage(IDC_STATIC_X,WM_SETTEXT,0,(LPARAM)lpszText);

//******************************************

float x_right_threshhold = 50000;float x_left_threshhold = 15000;if (ji.dwXpos > x_right_threshhold){

packetize(0);}

if (ji.dwXpos < x_left_threshhold){

packetize(1);}

//******************************************

pCPCYAxis->SetPos(0xFFFF-ji.dwYpos);sprintf(lpszText,"Y-Axis\n%d",ji.dwYpos);SendDlgItemMessage(IDC_STATIC_Y,WM_SETTEXT,0,(LPARAM)lpszText);

//******************************************

float y_down_threshhold = 50000;float y_up_threshhold = 15000;if (ji.dwYpos < y_up_threshhold){

packetize(2);}

if (ji.dwYpos > y_down_threshhold){

packetize(3);}

//******************************************

pCPCZAxis->SetPos(0xFFFF-ji.dwZpos);sprintf(lpszText,"Z-Axis\n%d",ji.dwZpos);SendDlgItemMessage(IDC_STATIC_Z,WM_SETTEXT,0,(LPARAM)lpszText);

pCPCRAxis->SetPos(ji.dwRpos);sprintf(lpszText,"R-Axis %d",ji.dwRpos);SendDlgItemMessage(IDC_STATIC_R,WM_SETTEXT,0,(LPARAM)lpszText);

for(int i = IDC_RADIO3; i <= IDC_RADIO10; i++)SendDlgItemMessage(i,BM_SETCHECK,BST_UNCHECKED);

if(ji.dwPOV != 0xFFFF)

SendDlgItemMessage(IDC_RADIO3+(ji.dwPOV/4500),BM_SETCHECK,BST_CHECKED);

int nRVel, nLVel;float ka,kp,w,Alpha,Roe,VAve;ka = 100;kp = 100;w = 1;

VAve = -LinearizeAxis((double)(ji.dwYpos), 65535.0, 5000.0);Roe = LinearizeAxis((double)(ji.dwXpos), 65535.0,

5000.0);Alpha = LinearizeAxis((double)(ji.dwRpos), 65535.0, 5000.0);

nRVel = (int)((ka*(VAve - w*Alpha)/2.0 + kp*VAve*(1.0-w*Roe/2.0)/2.0))/4;

nLVel = (int)((ka*(VAve + w*Alpha)/2.0 + kp*VAve*(1.0+w*Roe/2.0)/2.0))/4;

pCPCRVel->SetPos(nRVel);_snprintf(lpszText,80,"R-Wheel Velocity\n %d",nRVel);SendDlgItemMessage(IDC_R_VEL,WM_SETTEXT,0,(LPARAM)lpszText);

pCPCLVel->SetPos(nLVel);_snprintf(lpszText,80,"L-Wheel Velocity\n %d",nLVel);SendDlgItemMessage(IDC_L_VEL,WM_SETTEXT,0,(LPARAM)lpszText);

}

CDialog::OnTimer(nIDEvent);}

double LinearizeAxis(double dblRawInput, double dblScale, double dblDeadBand){

dblRawInput -= dblScale/2;if (dblRawInput > dblDeadBand)

return 2*(dblRawInput-dblDeadBand)/(dblScale/2.0-dblDeadBand);if (dblRawInput < -dblDeadBand)

return 2*(dblRawInput+dblDeadBand)/(dblScale/2.0-dblDeadBand);return 0;

}

void CJoyShowDlg::OnButton1() {

// TODO: Add your control notification handler code here

}

Web Application Code

<HTML><HEAD>

<TITLE>Working With VBScript: Exercise 1</TITLE>

</HEAD>

<BODY>

<H1><font size="6">ME 3484: Remote Manipulation of Robot Arm</font></H1>

<P> Click on the buttons below to control the robot arm.</P>

<P> </P>

<FORM NAME="frmExercise1">

<p align="left">

<INPUT TYPE="Button" NAME="up" VALUE="UP"></p> <p>

<INPUT TYPE="Button" NAME="left" VALUE="LEFT">

<INPUT TYPE="Button" NAME="right" VALUE="RIGHT">

<INPUT TYPE="Button" NAME="open" VALUE="OPEN">

<INPUT TYPE="Button" NAME="close" VALUE="CLOSE">

<INPUT TYPE="Button" NAME="wake" VALUE="LIGHTS ON">

<INPUT TYPE="Button" NAME="sleep" VALUE="LIGHTS OFF"></p> <p>

           <INPUT TYPE="Button" NAME="down" VALUE="DOWN"> <SCRIPT FOR="left" EVENT="onClick" LANGUAGE="VBScript">

' This function sends a packet on port 1000 to the ethernet chip connected to the BS2

' The Protocols class is a collection of all available protocols ' on the machine. We ask for a wafInet(2) protocol, of type UDP(17).


Set xProtocols = CreateObject( "XceedSoftware.XceedWinsock.Protocols.1")Set xProtocol = xProtocols.GetProtocol( 2, 0, 17 )



Set xSocketFactory = CreateObject( "XceedSoftware.XceedWinsock.SocketFactory.1" )

Set xSocket = xSocketFactory.CreateConnectionlessSocket( xProtocol, 0 )

' The server's address is hardcoded here. Change the address below to ' accomodate your setup.

Dim xAddress

Set xAddress = CreateObject( "XceedSoftware.XceedWinsock.InetAddress.1" )

xAddress.SetAddressString( "128.238.129.86:1000" )

Call xSocket.SendBytesTo( xAddress, "left", 0 )

</SCRIPT>

<SCRIPT FOR="right" EVENT="onClick" LANGUAGE="VBScript">










Dim xAddress



' A request to a time server consist of a packet that can contain ANYTHING!Call xSocket.SendBytesTo( xAddress, "right", 0 )

</SCRIPT> <SCRIPT FOR="up" EVENT="onClick" LANGUAGE="VBScript">



Dim xProtocols

Dim xProtocol







Dim xAddress



' A request to a time server consist of a packet that can contain ANYTHING!Call xSocket.SendBytesTo( xAddress, "up", 0 )

</SCRIPT> <SCRIPT FOR="down" EVENT="onClick" LANGUAGE="VBScript">










Dim xAddress



' A request to a time server consist of a packet that can contain ANYTHING!Call xSocket.SendBytesTo( xAddress, "down", 0 )

</SCRIPT> <SCRIPT FOR="open" EVENT="onClick" LANGUAGE="VBScript">










Dim xAddress



' A request to a time server consist of a packet that can contain ANYTHING!Call xSocket.SendBytesTo( xAddress, "open", 0 )

</SCRIPT> <SCRIPT FOR="close" EVENT="onClick" LANGUAGE="VBScript">




Set xProtocols = CreateObject( "XceedSoftware.XceedWinsock.Protocols.1")

Set xProtocol = xProtocols.GetProtocol( 2, 0, 17 )






Dim xAddress



' A request to a time server consist of a packet that can contain ANYTHING!Call xSocket.SendBytesTo( xAddress, "close", 0 )

</SCRIPT>

<SCRIPT FOR="wake" EVENT="onClick" LANGUAGE="VBScript">










Dim xAddress



' A request to a time server consist of a packet that can contain ANYTHING!Call xSocket.SendBytesTo( xAddress, "wake", 0 )

</SCRIPT> <SCRIPT FOR="sleep" EVENT="onClick" LANGUAGE="VBScript">










Dim xAddress



' A request to a time server consist of a packet that can contain ANYTHING!Call xSocket.SendBytesTo( xAddress, "sleep", 0 )

</SCRIPT> </FORM>

</BODY>

</HTML>

engineering.nyu.eduengineering.nyu.edu/mechatronics/projects/ME3484/2003… · Web viewSookram...

Documents

Transcript of engineering.nyu.eduengineering.nyu.edu/mechatronics/projects/ME3484/2003… · Web viewSookram...