Skip to content

Commit

Permalink
Lego setup for two motors using an amc and a amcbldc (#686)
Browse files Browse the repository at this point in the history 
Co-authored-by: Ugo Pattacini <[email protected]>
  • Loading branch information
@SanLordKevin @pattacini
SanLordKevin and pattacini authored Nov 5, 2024
1 parent 36d027c commit bee41fc
Show file tree
Hide file tree
Showing 22 changed files with 554 additions and 3 deletions.
47 changes: 47 additions & 0 deletions experimentalSetups/lego_setup_2motors_amc_amcbldc/README.md
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,47 @@
2-Motors Lego Setup
===================
This lego setup is built to command two motors using `yarprobotinterface` and `yarpmotorgui`.
One motor is controlled by an `amc` board and the other by an `amcbldc` board.

# Electronic configuration

## `Amcbldc`
The first joint, the one linked to the `amcbldc` has the encoder `aea3` connected to the connector `J5_X1` and the motor linked to the board itself.
The `amcbldc` is connected to the `amc` on `CAN1:1`.
The fault button is connected to the baord as shown in the image below.

## `Amc`
The second joint is linked to the `amc` and has the encoder on the connector `J5_X2`.
The `amc` is also connected via ethernet to the PC.
The fault button is connected to the baord using the `J5` connector.


# Mechanichal configuration
This setup works with motors that have a `Gearbox_M2J` of 196.

# Files configuration
All the parameters cited above can be changed in the configuration files.
This setup is configured to be used with `yarprobotinterface` and `yarpmotorgui`.

# Build the setup
| |
| :---: |
| ![](assets/Lego_setup_view.jpg) |
| **Figure 1** Setup view |

## `Amc`
| |
| :---: |
| ![](assets/Lego_setup_aea3_amc.jpg) |
| **Figure 2** Cable connection for `aea3` of the `amc` |
| ![](assets/Lego_setup_amc_motor.jpg) |
| **Figure 3** Cable connection for the motor of the `amc` |
| ![](assets/Lego_setup_amc_fault.jpg) |
| **Figure 4** Cable connection for the fault of the `amc` |
## `Amcbldc`
| |
| :---: |
| ![](assets/Lego_setup_aea3_amcbldc.jpg) |
| **Figure 5** Cable connection for `aea3` of the `amcbldc` |
| ![](assets/Lego_setup_amcbldc_fault.jpg) |
| **Figure 6** Cable connection for the fault of the `amcbldc` |
Binary file added experimentalSetups/lego_setup_2motors_amc_amcbldc/assets/Lego_setup_aea3_amc.jpg
View file
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Binary file added ...imentalSetups/lego_setup_2motors_amc_amcbldc/assets/Lego_setup_aea3_amcbldc.jpg
View file
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Binary file added experimentalSetups/lego_setup_2motors_amc_amcbldc/assets/Lego_setup_amc_fault.jpg
View file
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Binary file added experimentalSetups/lego_setup_2motors_amc_amcbldc/assets/Lego_setup_amc_motor.jpg
View file
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Binary file added ...mentalSetups/lego_setup_2motors_amc_amcbldc/assets/Lego_setup_amcbldc_fault.jpg
View file
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Binary file added ...mentalSetups/lego_setup_2motors_amc_amcbldc/assets/Lego_setup_amcbldc_motor.jpg
View file
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Binary file added experimentalSetups/lego_setup_2motors_amc_amcbldc/assets/Lego_setup_view.jpg
View file
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
47 changes: 47 additions & 0 deletions experimentalSetups/lego_setup_2motors_amc_amcbldc/calibrators/setup-calib.xml
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,47 @@
<?xml version="1.0" encoding="UTF-8" ?>
<!DOCTYPE devices PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">

<device xmlns:xi="http://www.w3.org/2001/XInclude" name="setup-calibrator" type="parametricCalibratorEth">

<xi:include href="../general.xml" />

<group name="GENERAL">
<param name="joints"> 2 </param> <!-- the number of joints of the robot part -->
<param name="deviceName"> Setup_Calibrator </param>
</group>

<group name="HOME">
<param name="positionHome"> 0 0 </param>
<param name="velocityHome"> 10 10 </param>
</group>

<group name="CALIBRATION">
<param name="calibrationType"> 12 12 </param>

<param name="calibration1"> 6865 6865 </param>
<param name="calibration2"> 0 0 </param>
<param name="calibration3"> 0 0 </param>
<param name="calibration4"> 0 0 </param>
<param name="calibration5"> 0 0 </param>
<param name="calibrationZero"> 0 0 </param>
<param name="calibrationDelta"> 0 0 </param>

<param name="startupPosition"> 0.0 0.0 </param>
<param name="startupVelocity"> 30.0 30.0 </param>
<param name="startupMaxPwm"> 16000 16000 </param>
<param name="startupPosThreshold"> 2 2 </param>
</group>

<param name="CALIB_ORDER"> (0 1) </param>

<action phase="startup" level="10" type="calibrate">
<param name="target"> setup-mc_remapper </param>
</action>

<action phase="interrupt1" level="1" type="park">
<param name="target"> setup-mc_remapper </param>
</action>

<action phase="interrupt3" level="1" type="abort" />

</device>
2 changes: 2 additions & 0 deletions experimentalSetups/lego_setup_2motors_amc_amcbldc/firmwareupdater.ini
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
[DRIVERS]
ETH "eth"
12 changes: 12 additions & 0 deletions experimentalSetups/lego_setup_2motors_amc_amcbldc/general.xml
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,12 @@
<?xml version="1.0" encoding="UTF-8" ?>
<!DOCTYPE params PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">

<params xmlns:xi="http://www.w3.org/2001/XInclude" robot="setup" build="1">

<group name="GENERAL">
<param name="skipCalibration"> false </param>
<param name="useRawEncoderData"> false </param>
<param name="useLimitedPWM"> false </param>
<param name="verbose"> false </param>
</group>
</params>
14 changes: 14 additions & 0 deletions experimentalSetups/lego_setup_2motors_amc_amcbldc/hardware/electronics/pc104.xml
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,14 @@
<?xml version="1.0" encoding="UTF-8" ?>
<!DOCTYPE params PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">

<params xmlns:xi="http://www.w3.org/2001/XInclude" robot="setup" build="1">

<group name="PC104">
<param name="PC104IpAddress"> 10.0.1.104 </param>
<param name="PC104IpPort"> 12345 </param>
<param name="PC104TXrate"> 1 </param>
<param name="PC104RXrate"> 5 </param>
</group>

</params>

40 changes: 40 additions & 0 deletions ...ups/lego_setup_2motors_amc_amcbldc/hardware/electronics/setup-2motors_amc_amcbldc-eln.xml
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,40 @@
<?xml version="1.0" encoding="UTF-8" ?>
<!DOCTYPE params PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">

<params xmlns:xi="http://www.w3.org/2001/XInclude" robot="setup" build="1">

<xi:include href="./pc104.xml" />

<group name="ETH_BOARD">

<group name="ETH_BOARD_PROPERTIES">
<param name="IpAddress"> 10.0.1.1 </param>
<param name="IpPort"> 12345 </param>
<param name="Type"> amc </param>
<param name="maxSizeRXpacket"> 768 </param>
<param name="maxSizeROP"> 384 </param>
</group>

<group name="ETH_BOARD_SETTINGS">
<param name="Name"> "setup-2motors_amc_amcbldc" </param>
<group name="RUNNINGMODE">
<param name="period"> 1000 </param>
<param name="maxTimeOfRXactivity"> 400 </param>
<param name="maxTimeOfDOactivity"> 300 </param>
<param name="maxTimeOfTXactivity"> 300 </param>
<param name="TXrateOfRegularROPs"> 5 </param>
</group>
</group>

<group name="ETH_BOARD_ACTIONS">
<group name="MONITOR_ITS_PRESENCE">
<param name="enabled"> true </param>
<param name="timeout"> 0.020 </param>
<param name="periodOfMissingReport"> 60.0 </param>
</group>
</group>

</group>

</params>

91 changes: 91 additions & 0 deletions ...ups/lego_setup_2motors_amc_amcbldc/hardware/mechanicals/setup-2motors_amc_amcbldc-mec.xml
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,91 @@
<?xml version="1.0" encoding="UTF-8" ?>
<!DOCTYPE params PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">

<params xmlns:xi="http://www.w3.org/2001/XInclude" robot="setup" build="1">
<group name="GENERAL">
<param name="MotioncontrolVersion"> 6 </param>
<param name="Joints"> 2 </param>
<param name="AxisMap"> 0 1 </param>
<param name="AxisName"> "setup_amcbldc" "setup_amc" </param>
<param name="AxisType"> "revolute" "revolute" </param>
<param name="Encoder"> 182.044 182.044 </param>
<param name="fullscalePWM"> 32000 32000 </param>
<param name="ampsToSensor"> 1000.0 1000.0 </param>
<param name="Gearbox_M2J"> -196 -196 </param> <!-- motor to joint -->
<param name="Gearbox_E2J"> 1 1 </param> <!-- encoder to joint -->
<param name="useMotorSpeedFbk"> 0 0 </param>
<param name="MotorType"> "FAULHABER" "FAULHABER" </param>
<param name="Verbose"> 0 0 </param>
</group>

<group name="LIMITS">
<param name="hardwareJntPosMax"> 180 180 </param>
<param name="hardwareJntPosMin"> -180 -180 </param>
<param name="rotorPosMin"> 0 0 </param>
<param name="rotorPosMax"> 0 0 </param>
</group>

<group name="AMCBLDC">
<param name="HasHallSensor"> 1 1 </param>
<param name="TemperatureSensorType"> NONE NONE </param>
<param name="HasRotorEncoder"> 0 0 </param>
<param name="HasRotorEncoderIndex"> 0 0 </param>
<param name="HasSpeedEncoder"> 0 0 </param>
<param name="RotorIndexOffset"> 0 0 </param>
<param name="MotorPoles"> 14 14 </param>
</group>

<group name="ADVFOC">
<param name="Verbose"> 0 0 </param>
<param name="AutoCalibration"> 0 0 </param>
<param name="HasHallSensor"> 1 1 </param>
<param name="TemperatureSensorType"> NONE NONE </param>
<param name="HasRotorEncoder"> 0 0 </param>
<param name="HasRotorEncoderIndex"> 0 0 </param>
<param name="HasSpeedEncoder"> 0 0 </param>
<param name="RotorIndexOffset"> 0 0 </param>
<param name="MotorPoles"> 14 14 </param>
</group>

<group name="COUPLINGS">

<param name ="matrixJ2M">
1.0 0.0 0.0 0.0
0.0 1.0 0.0 0.0
0.0 0.0 1.0 0.0
0.0 0.0 0.0 1.0
</param>

<param name ="matrixM2J">
1.0 0.0 0.0 0.0
0.0 1.0 0.0 0.0
0.0 0.0 1.0 0.0
0.0 0.0 0.0 1.0
</param>

<param name ="matrixE2J">
1.00 0.00 0.00 0.00 0.00 0.00
0.00 1.00 0.00 0.00 0.00 0.00
0.00 0.00 1.00 0.00 0.00 0.00
0.00 0.00 0.00 1.00 0.00 0.00
</param>

</group>

<group name="JOINTSET_CFG">
<param name= "numberofsets"> 2 </param>
<group name="JOINTSET_0">
<param name="listofjoints"> 0 </param>
<param name="constraint"> none </param>
<param name="param1"> 0 </param>
<param name="param2"> 0 </param>
</group>
<group name="JOINTSET_1">
<param name="listofjoints"> 1 </param>
<param name="constraint"> none </param>
<param name="param1"> 0 </param>
<param name="param2"> 0 </param>
</group>
</group>

</params>
103 changes: 103 additions & 0 deletions ...ups/lego_setup_2motors_amc_amcbldc/hardware/motorControl/setup-2motors_amc_amcbldc-mc.xml
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,103 @@
<?xml version="1.0" encoding="UTF-8" ?>
<!DOCTYPE devices PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">


<device xmlns:xi="http://www.w3.org/2001/XInclude" name="setup-2motors_amc_amcbldc-mc" type="embObjMotionControl">

<xi:include href="../../general.xml" />
<xi:include href="../../hardware/electronics/setup-2motors_amc_amcbldc-eln.xml" />
<xi:include href="../../hardware/mechanicals/setup-2motors_amc_amcbldc-mec.xml" />
<xi:include href="./setup-2motors_amc_amcbldc-mc_service.xml" />

<!-- joint number 0 1 -->
<!-- joint name amcbldc amc -->
<group name="LIMITS">
<param name="jntPosMax"> 180 180 </param>
<param name="jntPosMin"> -180 -180 </param>
<param name="jntVelMax"> 90 90 </param>
<param name="motorNominalCurrents"> 1000 1000 </param>
<param name="motorPeakCurrents"> 1500 1500 </param>
<param name="motorOverloadCurrents"> 2000 2000 </param>
<param name="hardwareTemperatureLimits"> 100 100 </param>
<param name="warningTemperatureLimits"> 85 85 </param>
<param name="motorPwmLimit"> 16000 16000 </param>
</group>

<group name="TIMEOUTS">
<param name="velocity"> 100 100 </param>
</group>

<group name="IMPEDANCE">
<param name="stiffness"> 0 0 </param>
<param name="damping"> 0 0 </param>
</group>

<group name="CONTROLS">
<param name="positionControl"> POS_PID_DEFAULT POS_PID_DEFAULT </param>
<param name="velocityControl"> POS_PID_DEFAULT POS_PID_DEFAULT </param>
<param name="mixedControl"> POS_PID_DEFAULT POS_PID_DEFAULT </param>
<param name="torqueControl"> none none </param>
<param name="currentPid"> 2FOC_CUR_CONTROL 2FOC_CUR_CONTROL </param>
<param name="speedPid"> 2FOC_VEL_CONTROL 2FOC_VEL_CONTROL </param>
</group>


<!-- default position PID: begin -->

<group name="POS_PID_DEFAULT">
<param name="controlLaw"> minjerk </param>
<param name="outputType"> current </param>
<param name="fbkControlUnits"> metric_units </param>
<param name="outputControlUnits"> machine_units </param>
<param name="kp"> -30 -30 </param>
<param name="kd"> -10 -10 </param>
<param name="ki"> -100 -100 </param>
<param name="maxOutput"> 500 500 </param>
<param name="maxInt"> 200 200 </param>
<param name="stictionUp"> 0 0 </param>
<param name="stictionDown"> 0 0 </param>
<param name="kff"> 0 0 </param>
</group>

<!-- default position PID: end -->


<!-- other default PIDs: begin -->

<group name="2FOC_CUR_CONTROL">
<param name="controlLaw"> low_lev_current </param>
<param name="fbkControlUnits"> machine_units </param>
<param name="outputControlUnits"> machine_units </param>
<param name="kp"> 2 2 </param>
<param name="kd"> 0 0 </param>
<param name="ki"> 500 500 </param>
<param name="shift"> 0 0 </param>
<param name="maxOutput"> 32000 32000 </param>
<param name="maxInt"> 32000 32000 </param>
<param name="kff"> 0 0 </param>
</group>

<group name="2FOC_VEL_CONTROL">
<param name="controlLaw"> low_lev_speed </param>
<param name="fbkControlUnits"> machine_units </param>
<param name="outputControlUnits"> machine_units </param>
<param name="kff"> 0 0 </param>
<param name="kp"> 12 12 </param>
<param name="kd"> 0 0 </param>
<param name="ki"> 16 16 </param>
<param name="shift"> 10 10 </param>
<param name="maxOutput"> 32000 32000 </param>
<param name="maxInt"> 32000 32000 </param>
</group>

<!-- other default PIDs: end -->

<!-- custom PIDs: begin -->

<!-- custom PIDs: end -->

<group name="OTHER_CONTROL_PARAMETERS">
<param name="deadZone"> 0.0495 0.0495 </param>
</group>

</device>
Loading

0 comments on commit bee41fc

Please sign in to comment.