1 1 2 ( MASTER-SLAVE ROBOT ARM PHASE 2 ) 1.1 , MASTER-SLAVE ROBOT ARM 1.2 1. Master arm Slave arm 2. MASTER SLAVE ROBOT ARM 1 3. 1.3 1. 2. slave arm 1 3. slave arm slave arm 1.4 1. Master slave robot arm 2. 3. 4. 21.5 .. .. .. .. .. .. .. .. 1. Master slave robot arm 2. position control 3. mechanics 4. 1 5. master slave arm 6. 7. master slave arm 8. 9. master slave arm 10. 3 2 2.1 Master Slave Robot Arm MASTER-SLAVE ROBOT ARM Master arm Master arm sensor potentiometer input slave arm robot Slave Robot arm master arm slave robot arm input master arm slave robot arm output transducer potentiometer slave robot arm master arm ( feedback control ) (control variable) ( reference variable )(correcting variable ) 2.1 block diagram ( feedback control) 42.2 2.2.1 (Proportional Control) ( ) output = Kfe Kf proportional gain Proportional control G(S) G(S) = Kf ( 2.1 ) gain proportional control proportional band. 2.2 proportional band 100% ( 2.2 ) 5 step step 2.3 proportional band 2.3 proportional control proportional control proportional control 2.4 ( 2.3 ) 2.4 proportional control ( Proportional Control) 62.2.2 (Integral Control) ( 2.4 ) Ki integral gain 1/sec 2.5 integral control step t 0 0 t step 2.5 integral control 2.4 ( 2.5 ) 2.6 integral control forward-path transfer function 7 2.6 integral control integral control 2.5 integral control type type 0 type 1 step input S = 0 (n) ( m ) 1 asymptote angles s-plane 2.2.2 (Derivative Control) (Derivative Controller) ( 2.6) derivative gain 2.7 Derivative Control 2.7 ramp 8 2.6 2.8 ( 2.7 ) 2.8 Derivative Control type 1 S type 1 2.2.4 (Proportional plus integral Control) ( Proportional plus Integral, PI ) 2.9 2.9 Proportional plus Integral 9 ( 2.8 ) 2.10 PI controller 2.10 step PI ( 2.9 ) integral time constant ( 2.10 ) ( 2.11 ) 10 ( 2.12 ) S=0 PI S 1 n m asymptote asymptotes S=0 2.2.3 (Proportional plus Derivative Control) (PD) 2.11 ( 2.13 ) 11 derivative time constant 2.11 PD Control 2.2.5 PID CONTROL 2.12 Block diagram PID control (PID control) 3 (tree-term control) 2.12 ( 2.14 ) ( 2.15 ) 12 ( 2.16 ) 2.12 ( 2.17 ) PID controller 2 1 type 2.3 DC MOTOR 2.3.1 2.13 13 2.3.2 3 2.14 (VA) (VR) VA (BACK EMF) (VA) (VR) (V) 2. 18 V = VA + VR ( 2.18 ) VA = 0, VR = V I = RVR A ( 2.19 ) VA VR VA V I VA V 14 2.4 2.4.1 H-bridge H-bridge switching Transistor Transistor (Semiconductor device) Cutoff Saturation Transistor - 2.15 2.15 - Transistor 2.15 Ib (Saturation mode) C E Ic Ib B (Cutoff mode) C E 2.4.2 H-bridge 4 Forward Q1 Q3 (On) Backward Q2 Q4 (On) H-bridge 2.16 15 2.16 H-bridge 2.5 Potentiometer Potentiometer 2.17 potentiometer Slave robot arm A-B C-B master slave robot arm potentiometer DC MOTOR 162.6 (digital control ) Plant continuous signal controller Plant Plant controller classical control block diagram 2.18 block diagram continuous control D(s) continuous controller analog electronic block diagram controller analog digital digital controller digital controller analog controller digital controller discrete time signal digital block diagram 2.19 block diagram digital control 17 2.20 block diagram digital control Block diagram digital controller digital control continuous time discrete time continuous time discrete time A2D conversion discrete time continuous time Zero order hold PWM Digital control (Transient response and stability) (Continuous time system) S-plane ( stable ) ( Continuous time system ) ( discrete time system) z-plane unit cycle ( Continuous time system ) S Z matched pole-zero mapping method Z = est ( 2.20 ) t = Sampling time (sec/sample) s = Location in the s-plane z = Location in the z-plane 18 Mapping s-plane z-plane damping ratio natural frequency map 2.21 Natural frequency and damping loci in z-plane z-plane s-plane zeta = Damping ratio Wn = Natural frequency (rad/sec) ; Natural freq. z-plane rad/sample s-plane rad/sec Ts = Settling time, Tr = Rise time, Mp = Maximum overshoot 19 3 1 Analog 3.1 MASTER ARM SLAVE ARM Master Arm Slave Arm Master Arm 3.1.1 1 1 1800 1 a) b) 3.1 Master Arm 1, a) 1 , b) 3 1 203.1.2 2 2 1800 3.2 a) a) b) c) 3.2 Master Arm 2 , a) 2 , b) 3 Master arm , c) 2 213.1.3 1800 a) b) 3.3 Master Arm a) b) 3 3.1.4 900 a ) b) 3.4 Master Arm , a) , b) 3 223.1.5 (gripper) 900 3.5 a) a) b) 3.5 Master Arm , a) , b) 3 Master Arm (Potentiometer ) a) b) 3.6 Master Arm 233.2 SLAVE ARM ( Slave Arm ) Master Arm ( ) 1 1800 2 1800 900 900 900 3.1 Master arm 3.2.1 1 1 7 3.7 Slave Arm 1 3.2.2 1 2 2 18001800 3.8 Slave Arm 2 243.2.3 900 3.9 Slave Arm 3.2.4 3.10 Slave Arm Slave Arm 3.11 Master arm 25 Slave Arm 6 . ( Potentiometer ) 10 k a) b) c) d) 3.12 (Slave Arm ) , a) 1 , b) 2 , c) , d) 26 Denavit-Hatenberg ( Slave Arm ) L1 L2 L3 END OF EFECTOR 3.12 link variable d a c 1 1 L1 - - d 2 - - L2 2 e 2 3 L3 - (+)90 3.2 Denavit-Hatenberg link(cm) Slave arm L130 L216.9 L325 3.3 ( Slave Arm ) 27 3.3 3.3.1 H-bridge 3.12 shacematic H-bridge switch transistor Microcontroller on-off switch PWM Microcontroller PORT Enable PWM H-bridge opto coupler 5 volt 12 volt truth table Enable FW REV condition 0 0 0 0 1 0 1 0 1 x 0 0 1 1 x Stop Forward Reverse Break Stop 3.4 28 3.3.2 3.13 Pin out ATMEGA16 (), ATMEGA8 () 1 2 3 4 5 6ABCD6 5 4 3 2 1DCBATitleNumber Revision SizeBDate: 29-Jan-2007 Sheet ofFile: C:\Documents and Settings\Administrator\My Documents\Robot\shacematic project ee 2006 Drawn By:AGND31X113X212RESET9PD2 (INT0)16PD3 (INT1)17PD4 (OC1B)18PD5 (OC1A)19PB0 (T0)1PB1 (T1)2PB2 (AIN0)3PB3 (AIN1)4PB4 (SS)5PB5 (MOSI)6PB6 (MISO)7PB7 (SCK)8(ADC0) PA040(ADC1) PA139(ADC2) PA238(ADC3) PA337(ADC4) PA436(ADC5) PA535(ADC6) PA634(ADC7) PA733PC022PC123PC224PC325PC426PC527(TOSC1) PC628(TOSC2) PC729PD7 (TOSC2)21PD6 (ICP)20AVCC30AREF32PD1 (TXD)15PD0 (RXD)14GND11VCC10U2ATMEGA16_DIP4012345678JP7A/DY1CRYSTALC322pFC422pFY2CRYSTALC522pFC622pFXTAL1XTAL2XTAL1XTAL2R181k+ C101uFVccRESET1RESET1RESET21 23 45 67 89 1011 1213 1415 1617 1819 2021 2223 2425 2627 2829 30JP11HEADER 15X2+C81uFVcc+ C91uFVcc123JP1MOTOR1123JP2MOTOR2123JP3MOTOR3123JP4MOTOR4123JP5MOTOR5REV1FWD1EN1FWD2REV2EN2FWD3REV3EN3FWD4REV4EN4FWD5REV5EN512JP6POWERVin1GND2VoutU1VOLTREG3Vcc+ C1CAP POL+ C21uFS1SWRq4.7kD1ON1 2JP12POWERVccVccFWD4FWD1EN1FWD3REV3EN2EN3EN4FWD2REV1REV2REV4MISOSCK12345678910JP10PC1 23 45 67 89 10JP8PROGRAMERRESET1VccSCKMISOREV3RESET1FWD5REV5EN5SCK1(RESET) PC61PD0 (RXD)2PD1 (TXD)3PD2 (INT0)4PD3 (INT1)5PD4 (XCK/T0)6VCC7GND8PB6 (XTAL1/TOSC1)9PB7 (XTAL2/TOSC2)10PD5 (T1)11PD6 (AIN0)12PD7 (AIN1)13PB0 (ICP)14PB1 (OC1A)15PB2 (SS/OC1B)16PB3 (MOSI/OC2)17PB4 (MISO)18PB5 (SCK)19AVCC20AREF21GND22(ADC0) PC023(ADC1) PC124(ADC2) PC225(ADC3) PC326(ADC4/SDA)PC427(ADC5/SCL)PC528U3ATMEGA8_DIP28SCKMISOREV312345JP13A/DVccS2RESET12345JP9PD12JP14POWER123JP15PORTD1 23 45 67 89 10JP16PROGRAMERRESET2SCK1RESET2SCK1VccR191k+ C111uFVccRESET2S3RESETFWD5REV5FWD5REV5VccVccC14CAPPOWER SUPPLY5 VOLTCIRCUITDIGITALCONTRLLOLER 3.14 shacematic diagram digital control Pin out ATMEGA16 ATMEGA 8 Microcontroller A2D TIMER PWM ATMEGA16 A2D 8 ports PWM 4 ports 4 ATMEGA 8 A2D 6 ports PWM 3 ports 1 29 ports A2D FWD REV EN master salve 1 2 2 4 PB.1 PD.2 PB.4 PD.1 PB.2 PD.3 PB.5 PD.6 PB.3 PD.4 PD.5 PD.7 PA.0 PA.2 PA.4 PA.6 PA.1 PA.3 PA.5 PA.7 3.5 ports ATMEGA 16 A2D FWD REV EN master salve 5 PB.4 PB.3 PB.2 PA.0 PA.1 3.6 ports ATMEGA 8 Interrupt Timer 0 overflow sampling time Microcontroller A2D 10bit Microcontroller potentiometer master PORTA MEGA16 potentiometer slave PI controller duty cycle PWM duty cycle ] 1 [ ]} 1 [ ] [ {2] [ ] [ + + + = k U k E k EfKk E K k Usip 3.1 U[k] = duty cycle E[K]=error (master slave) fs=sampling frequency (interrupt overflow Timer 0 sampling frequency T0 Phase correct PWM Prescaler frequency fs = 125 kHz ) fs 30Flowchart Interrupt subroutine MAIN PROGRAM Yes No No Yes 3.15 flowchart 31 3.3.3 Digital PI controller E(s) U(s) Gc(s) 3.16 block diagram continous time control ) () (= ) (s E s Us Gc PI controller ) (s GcsKiKp s Gc + = ) ( Discrete time equivalent Tustin Formula 1 +) 1 - ( 2=zzTs ) 1 - ( 2) 1 () (z z T KiKp z Gc + + = z domain E(z) U(s) Gc(z) 3.17 block diagram discrete time control Block diagram Gc(z)E(z) U(z) = ) ( ]) 1 - ( 2) 1 + ( + [ = ) ( s Ez z T KiKp z U ) (2) 1 + ( + ) ( = ) ( ) 1 - ( z Ez T Kiz KpE z U z ) ( + ) (2) 1 + ( + ) ( = ) ( z U z Ez T Kiz KpE z zU ] [ + ]) [ + ] 1 + [ (2+ ] [ = ] 1 + [ k u k e k eT Kik Kpe k u 32 KpKi Gc(s) continuous time system Microcontroller Microcontroller Gc(s) Digital controller Digital controller Gc(z)z domain Gc(s) s domain (Modeling system) Gp(s) U(s) Y(s) 3.18 block diagram discrete plant U(s) input plant Y(s) output plant Gp(s) plant Block diagram ) () (=s U s YGp slave (Motor and slave arm modeling) Transfer function controller slave DC motor model DC motor 3.19 Equivalent circuit motor 33- Motor model first-order systems Ea(s) Gp(s) Wn(s) 3.20 block diagram First-order system Ea(s) =DC Voltage Wn(s)=angular velocity Gp(s) =plant Gp(s) ) () (= ) (s E s s Ganp First-order system step response 3.21 First-order system step response 34 3.22 First-order system step response step input plant time constant output plant DC motor transfer function a sKs Gp+= ) ( DC motor K = motor steady state step input DC voltage a = 1 = time constant output motor (s) (s) Ea(s) Gp(s) 3.23 block diagram dt d = ) () ( 1=) () (=s Eas s s Ea s n Gp ) + (= ) (a s s Ks Gp 35 model motor plant transfer function model slave load torque - model motor Electromechanical system 3.24 model motor Electromechanical system J moment of inertia of the rotor ( kg.m^2/s^2) B damping ratio of the mechanical system (Nms) K=Ke=Kt electromotive force constant (Nm/Amp) R electric resistance (ohm) L electric inductance (H) V input (Voltage) output (position of shaft) The rotor and shaft are assumed to be rigid a ti K = Kt torque constant n a a K E = Ka back emf constant 36 Equivalent circuit Newton's law Kirchhoff's law Kidt dBdt dJ = +22 K V RidtdiL = + Laplace Transforms ) ( ) ( ) () ( ) ( ) (s Ks V s I R Ls s KI s B Js s = + = + transfer function 2+ ) + )( + (=) () (K R Ls B Js Ks V s n ) + ) + )( + ((=) () (2K R Ls B Js s Ks V s modeling slave plant controller modeling model 37 POTENTIOMETER 0 50 100 150 200 250 300012345678910DegreeR(k ohm)RESISTANCE vs DegreeR vs Degree 40 260 3.25 potentiometer POTENTIOMETER 10k ohm POTENTIOMETER linear 40-260 38 4 4.1 Master Arm Slave Arm Master Arm Slave Arm Master Arm Slave Arm () () Master Arm Slave Arm Master Arm Slave Arm 1 18009001800900 2 1800180015001500 1800180018001200 900900900900 900900900900 4.1 Master Arm Slave Arm 4.2 Master Arm Slave Arm ( Joint ) Master Arm Slave Arm 4.1 Master Arm Slave Arm 394.2.1 1 master arm slave arm 1 30 ( ) Master arm Slave arm Error( %) 0 0 0 30 29 3.33 60 61 3.33 90 81 10 4.2 Master Arm Slave Arm 1 4.2.2 2 master arm slave arm 2 30 ( ) Master arm Slave arm Error( %) 0 0 0 30 20 33.33 60 35 41.66 90 80 11.11 120 115 4.16 150 145 3.33 180 175 2.77 4.3 Master Arm Slave Arm 2 404.2.3 master arm slave arm 30 ( ) Master arm Slave arm Error( %) 0 0 0 30 29 3.33 60 55 8.33 90 87 3.33 120 116 3.33 4.4 Master Arm Slave Arm 41 5 5.1 2 1. master arm slave arm 2. h-bridge 3 1 , 2 3. 1. slave arm slave arm 2. ki kp 3. ( gripper ) 4. 5.2 1. 2. 3. H-bridge 425.3 1. 2. 3. 43 1. Norman S.Nise, Control System Engineering ,John Wiley And Sons , California 2. , Direct current machines , , 3. , , , 44 45 ( DC MOTOR ) ( dc motor ) 12 V 150 rpm 30 kg-cm 1 12 V 30 rpm 20 kg-cm 2, 12 V 30 rpm 1 kg-cm 46 - ( Potentiometer ) 10k - 12