Skip to content

Commit

Permalink
Motor.c: put all the current targets to 0 when IDLE or HF (robotology…
Browse files Browse the repository at this point in the history 
…#362)
  • Loading branch information
@sgiraz @valegagge
sgiraz authored and valegagge committed May 5, 2023
1 parent 1f3015c commit 6cf5b0f
Show file tree
Hide file tree
Showing 6 changed files with 55 additions and 22 deletions.
8 changes: 4 additions & 4 deletions emBODY/eBcode/arch-arm/board/amc/examples/appl-01/cfg/theApplication_config.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,14 +40,14 @@ namespace embot { namespace app { namespace eth {
Process::eApplication,
#if defined(WRIST_MK2)
#if defined(WRIST_MK2_RIGHT)
{101, 0},
{101, 1},
#else
{102, 0},
{102, 1},
#endif
#else
{103, 0},
{103, 1},
#endif
{2023, Month::Mar, Day::two, 15, 20}
{2023, Month::Apr, Day::four, 15, 00}
},
.OStick = 1000*embot::core::time1microsec,
.OSstacksizeinit = 10*1024,
Expand Down
6 changes: 3 additions & 3 deletions emBODY/eBcode/arch-arm/board/amc/examples/appl-01/proj/appl-01-wrist.uvprojx
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,7 +49,7 @@
<InvalidFlash>1</InvalidFlash>
</TargetStatus>
<OutputDirectory>.\obj\</OutputDirectory>
<OutputName>h7eth</OutputName>
<OutputName>amc</OutputName>
<CreateExecutable>1</CreateExecutable>
<CreateLib>0</CreateLib>
<CreateHexFile>1</CreateHexFile>
Expand Down Expand Up @@ -80,9 +80,9 @@
<nStopB2X>0</nStopB2X>
</BeforeMake>
<AfterMake>
<RunUserProg1>0</RunUserProg1>
<RunUserProg1>1</RunUserProg1>
<RunUserProg2>0</RunUserProg2>
<UserProg1Name></UserProg1Name>
<UserProg1Name>cmd.exe /C copy .\obj\amc.hex ..\bin\amc.hex</UserProg1Name>
<UserProg2Name></UserProg2Name>
<UserProg1Dos16Mode>0</UserProg1Dos16Mode>
<UserProg2Dos16Mode>0</UserProg2Dos16Mode>
Expand Down
10 changes: 5 additions & 5 deletions emBODY/eBcode/arch-arm/board/ems004/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -81,21 +81,21 @@ extern "C" {
#define EOMTHEEMSAPPLCFG_VERSION_MAJOR (VERSION_MAJOR_OFFSET+3)
// <o> minor <0-255>
// <o> minor <0-255>
#define EOMTHEEMSAPPLCFG_VERSION_MINOR 64
#define EOMTHEEMSAPPLCFG_VERSION_MINOR 65

// </h>version

// <h> build date
// <o> year <2010-2030>
#define EOMTHEEMSAPPLCFG_BUILDDATE_YEAR 2023
// <o> month <1-12>
#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH 3
#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH 4
// <o> day <1-31>
#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 23
#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 4
// <o> hour <0-23>
#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR 11
#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR 15
// <o> minute <0-59>
#define EOMTHEEMSAPPLCFG_BUILDDATE_MIN 54
#define EOMTHEEMSAPPLCFG_BUILDDATE_MIN 00
// </h>build date
// </h>Info

Expand Down
10 changes: 5 additions & 5 deletions emBODY/eBcode/arch-arm/board/mc2plus/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -75,20 +75,20 @@ extern "C" {
#define EOMTHEEMSAPPLCFG_VERSION_MAJOR 3
// <o> minor <0-255>
// <o> minor <0-255>
#define EOMTHEEMSAPPLCFG_VERSION_MINOR 46
#define EOMTHEEMSAPPLCFG_VERSION_MINOR 47
// </h>version

// <h> build date
// <o> year <2010-2030>
#define EOMTHEEMSAPPLCFG_BUILDDATE_YEAR 2023
// <o> month <1-12>
#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH 3
#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH 4
// <o> day <1-31>
#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 23
#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 4
// <o> hour <0-23>
#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR 11
#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR 15
// <o> minute <0-59>
#define EOMTHEEMSAPPLCFG_BUILDDATE_MIN 56
#define EOMTHEEMSAPPLCFG_BUILDDATE_MIN 00
// </h>build date

// </h>Info
Expand Down
10 changes: 5 additions & 5 deletions emBODY/eBcode/arch-arm/board/mc4plus/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -86,21 +86,21 @@ extern "C" {
#define EOMTHEEMSAPPLCFG_VERSION_MAJOR (VERSION_MAJOR_OFFSET+3)
// <o> minor <0-255>

#define EOMTHEEMSAPPLCFG_VERSION_MINOR 67
#define EOMTHEEMSAPPLCFG_VERSION_MINOR 68

// </h>version

// <h> build date
// <o> year <2010-2030>
#define EOMTHEEMSAPPLCFG_BUILDDATE_YEAR 2023
// <o> month <1-12>
#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH 3
#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH 4
// <o> day <1-31>
#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 23
#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 4
// <o> hour <0-23>
#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR 11
#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR 15
// <o> minute <0-59>
#define EOMTHEEMSAPPLCFG_BUILDDATE_MIN 55
#define EOMTHEEMSAPPLCFG_BUILDDATE_MIN 00

// </h>build date

Expand Down
33 changes: 33 additions & 0 deletions emBODY/eBcode/arch-arm/embobj/plus/mc/JointSet.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -421,6 +421,7 @@ BOOL JointSet_do_check_faults(JointSet* o)

static void JointSet_do_vel_control(JointSet* o);
static void JointSet_do_current_control(JointSet* o);
static void JointSet_do_off(JointSet* o);

void JointSet_do_control(JointSet* o)
{
Expand All @@ -437,6 +438,10 @@ void JointSet_do_control(JointSet* o)
case eomc_ctrl_out_type_cur:
JointSet_do_current_control(o);
return;

case eomc_ctrl_out_type_off:
JointSet_do_off(o);
return;
}
}

Expand Down Expand Up @@ -1287,6 +1292,34 @@ static void JointSet_do_current_control(JointSet* o)
}
}

static void JointSet_do_off(JointSet* o)
{
// get the number of the joints (that is also the number of the motors)
int N = *(o->pN);

// for each joint set the reference to ZERO for each control mode
for (int js=0; js<N; ++js)
{
Joint *pJoint = o->joint+o->joints_of_set[js];

pJoint->pos_err = pJoint->pos_ref = ZERO;
pJoint->vel_err = pJoint->vel_ref = ZERO;
pJoint->trq_err = pJoint->trq_ref = ZERO;

pJoint->output = ZERO;
}

// for each reference mode set the motor reference to ZERO
for (int ms=0; ms<N; ++ms)
{
int m = o->motors_of_set[ms];

Motor_set_Iqq_ref(o->motor+m, 0);
Motor_set_pwm_ref(o->motor+m, 0);
Motor_set_vel_ref(o->motor+m, 0);
}
}

static void JointSet_do_vel_control(JointSet* o)
{
int N = *(o->pN);
Expand Down

0 comments on commit 6cf5b0f

Please sign in to comment.