| Both sides previous revisionPrevious revisionNext revision | Previous revision |
| software [2019/02/21 17:57] – [Control System] graf | software [2019/02/28 16:18] (current) – graf |
|---|
| Like any EEROS application our application to control the delta roboter is comprised of a control system, a safety system, and a sequencer running a given sequence. To get familiar with EEROS and its structure read [[http://wiki.eeros.org/eeros_architecture/start|EEROS Architecture]]. | Like any EEROS application our application to control the delta roboter is comprised of a control system, a safety system, and a sequencer running a given sequence. To get familiar with EEROS and its structure read [[http://wiki.eeros.org/eeros_architecture/start|EEROS Architecture]]. |
| |
| [[control_system|]] | * [[hal|]] |
| | * [[control_system|]] |
| | * [[safety_system|]] |
| | * [[sequencer|]] |
| | * [[calibration|]] |
| |
| |
| |
| ===== Safety System ===== | |
| To get more informations and details how the Safety System works, please have a look into the | |
| [[http://wiki.eeros.org/eeros_architecture/safety_system/start|Safety System]] described on the [[http://wiki.eeros.org/|eeros wiki]]. | |
| {{ :delta:safetysystem.jpg |Safety System of the Delta roboter}} | |
| |
| The forward paths of the Safety System are on the righthand side of the diagramm. The backward paths on the lefthand side.\\ | |
| The entry level of the Safety System is "slOff". In the file "delta.cpp" the Safety Event "doSwInit" gets triggered. In case of any failure in the Control System, the Safety Event "doEmergency" gets registered in the timedomain.\\ | |
| |
| ===== Sequencer & Sequences ===== | |
| For more informations how Sequences and the Sequencer work, have a look into [[http://wiki.eeros.org/eeros_architecture/sequencer/start|Sequencer]] on the [[http://wiki.eeros.org|eeros wiki]]. | |
| {{ :delta:software:sequencer.jpg?600 |Sequences of the Delta roboter}} | |
| When the homing of the Delta robot is completed the "Main Sequence" gets started. In this sequence, other sequences get added, dependent on the current Safety Level. The first activated Safety Level is "slAutoMoving". So the "Main Sequence" starts the "Sort Sequence" and waits until its finished, after that, the "Shuffle Sequence" gets started, and once again, the "Main Sequence" waits until this is finished. After that, the "Main Sequence" gets restarted, and if no Safety Event got triggered, the "Sort Sequence" and the "Shuffle Sequence" gets started again.\\ | |
| The "Sort Sequence" and the "Shuffle Sequence" are equipped with a Monitor. This Monitor checks the "Move Mouse Condition". If the mouse gets moved, or a button on the mouse gets pressed, the Condition returns "true", the current running sequence gets aborted and the "Move Mouse Exception Sequece" gets started. This sequence moves the TCP back to init position, and triggers the Safety Event "doMouseTeaching". The "Main Sequence" now starts the "Mouse Sequence". As long as the mouse gets moved, or buttons are pressed, the "Mouse Sequence" will be active. This sequence is equipped with a "Time Out Monitor". If the mouse doesn't send any event for 2 seconds, this Monitor triggers the "MouseSequenceTimeOutExpectionSequence". This "Exception Sequence" aborts the "Mouse Sequence", moves the TCP back to the init position and triggers the Safety Event "doAutoMoving".\\ \\ | |
| The "Calibration Sequence" can be started by triggering the Safety Event "doCalibrating". This can be done by pressing the red button until this lights up. Press and hold the red button for 2 seconds, this will trigger the Safety Event "doCalibrating". When finished with calibrating, press the red button once again. Press the green button to get back in normal operation mode. | |
| ===== HAL ===== | |
| To get more information about the Hardware Abstraction Layer and how it works, please have a look into the [[http://wiki.eeros.org/eeros_architecture/hal/start | HAL ]] described on the [[http://wiki.eeros.org/|eeros wiki]]. \\ | |
| EEROS for the Beagle Bone Blue is using the [[https://github.com/StrawsonDesign/librobotcontrol|robotis_cape_library]]. You'll find the wrapper and its functions in the bbblue-eeros folder. | |
| |
| ====Configure the motors==== | |
| If you want to run a motor with EEROS you need to define a Peripheral Output with the respective "signalId" used in the HAL-Configuration file. As you can see in the [[http://hw.eeros.org/eeduro/delta/software/hal#hal-configuration_file_for_the_delta_roboter|HAL-Configuration file for the Delta roboter]] in "subdevice2" the type for the motors is set to "AnalogOut". In this subdevice section the 4 motors are configured to match the functions of the robotics_cape_library. In the EEROS application for the Delta roboter you can set a motor voltage of -8.0V to 8.0V. The HAL gets this values and wraps it to match the values of the robotics_cape_library which then runs the motors with the desired voltage. | |
| {{ :delta:software:hal-motor.jpg?300 |HAL-Configuration for the motors}} | |
| |
| ====Configure the encoders==== | |
| The Delta roboter has two different motors with different encoders and gears. You'll find the datasheets of the two types [[http://hw.eeros.org/eeduro/delta/hardware|here]]. | |
| ===Calculate the Axis Motor=== | |
| The Axis Motor is equipped with gear with gear ratio of //76:1// and with the encoder ieh2-4096, which provides //4096 lines per revolution// ( //=16384 counts per revolution//). So for one full turn on the gear axis, the encoder sends //16384*76 = 1245184// counts. This is the input value for the HAL.\\ In the EEROS application we don't want to calculate everytime the respective radian value from this counts, so we do this in the HAL. To do that, we need to add some "blocks".\\ Start with the gear:\\ one full turn on the gear axis (=2*PI) equals to 76 turns of the motor axis. Next up, for one revolution of the motor axis, the encoder sends 4096 lines. So if we turn the gear axis for one revolution, the motor axis turns 76 times and the encoder sends //76*4096 = 311296// lines. The last block is the FQD. It converts the lines of the encoder into single counts. One line equals to 4 counts.\\ | |
| {{ :delta:software:hal-ieh2.jpg?300 |HAL config for ieh2 encoder}} | |
| The Axis Motors have mechanical upper and lower limits. | |
| ^Upper limit^Lower limit^ | |
| |{{:delta:motor-axis-calculation-upper-n.jpg?300|}}|{{:delta:motor-axis-calculation-lower-n.jpg?300|Lower limit of the Axis Motor}}| | |
| |Upper limit calculation below.|Measured on the Delta robot, γ is about 5°.| | |
| The upper limit calculates with the following two equations: | |
| \\ \\ | |
| //(1) tan(β)=(15-y)/17.5// \\ | |
| //(2) cos(β)=6/y// \\ | |
| \\ | |
| Transform this two equations until you get \\ | |
| \\ | |
| //(3) 17.5*sin(β)-15*cos(β)=-6// \\ | |
| \\ | |
| The subraction of a cosine function from a sine function results in another sine function with phase shift. So we are looking for a function in form of \\ | |
| \\ | |
| //(4) A*sin(β+φ) = 17.5*sin(β)-15*cos(β)// \\ | |
| \\ | |
| By using the addition theorem we get \\ | |
| \\ | |
| //A*sin(β+φ) = A*sin(β)*cos(φ)+A*cos(β)*sin(φ) \\ | |
| \\ | |
| Now we can compare the coefficients from sin(β) and cos(β) \\ | |
| \\ | |
| //(5) 17.5=A*cos(φ)// \\ | |
| //(6) -15=A*sin(φ)// \\ | |
| \\ | |
| By dividing (6) by (5) we get \\ | |
| \\ | |
| // -15/17.5=sin(φ)/cos(φ)=tan(φ) -> φ = arctan(-15/17.5)=-40.60°//\\ | |
| \\ | |
| Take the square of each function and add them together results in\\ | |
| \\ | |
| // 17.5<sup>2</sup>+(-15)<sup>2</sup> = A<sup>2</sup>*(sin<sup>2</sup>(φ)+cos<sup>2</sup>(φ)) = A<sup>2</sup>// \\ | |
| // A = sqrt(17.5<sup>2</sup>+(-15)<sup>2</sup>) = 23.048 //\\ | |
| \\ | |
| Inserting the values in (4)\\ | |
| \\ | |
| //23.048*sin(β-40.60°)=17.5*sin(β)-15*cos(β)=-6//\\ | |
| \\ | |
| we can now solve for β\\ | |
| \\ | |
| //β=arcsin(-6/23.048)+40.60° = 25.51°\\ | |
| \\ | |
| The maximum angle which the Gear axis can turn is // 90°+5°+25°=120°//.\\ | |
| \\ | |
| ===Calculate the TCP Motor=== | |
| {{ :delta:software:hal-hem3.jpg?300 |HAL config for HEM3 encoder}} | |
| As described in the previous encoder, the values are now: | |
| * gear ratio: 120:1 | |
| * lines per revolution: 256 | |
| So one turn on the gear axis results in // 256*4*120 = 122880 // counts.\\ | |
| \\ {{ :delta:tcp-limit.jpg?300 |TCP limit}} | |
| The TCP rotation is also limited. The maximum turn angle is calculated as follows:\\ | |
| \\ | |
| //sin(γ/2) = r/R//\\ | |
| //γ=2*arcsin(r/R)=3.438°//\\ | |
| \\ | |
| Thats the value for one side. The other side is identical, so the maximum angle to turn is\\ | |
| \\ | |
| //360°-2*3.438°=353°//. | |
| |
