Skip to content

Commit

Permalink
Board amc2c: some fixes in motor control code (#377)
Browse files Browse the repository at this point in the history
  • Loading branch information
@marcoaccame
marcoaccame authored May 4, 2023
1 parent 5a3b9da commit 1e1b260
Show file tree
Hide file tree
Showing 11 changed files with 157 additions and 152 deletions.
26 changes: 21 additions & 5 deletions emBODY/eBcode/arch-arm/board/amc2c/application/proj/amc2c-appl-can.uvoptx
View file
Original file line number Diff line number Diff line change
Expand Up @@ -453,7 +453,7 @@
<Bp>
<Number>0</Number>
<Type>0</Type>
<LineNumber>79</LineNumber>
<LineNumber>38</LineNumber>
<EnabledFlag>1</EnabledFlag>
<Address>0</Address>
<ByteObject>0</ByteObject>
Expand All @@ -469,6 +469,22 @@
<Bp>
<Number>1</Number>
<Type>0</Type>
<LineNumber>79</LineNumber>
<EnabledFlag>1</EnabledFlag>
<Address>0</Address>
<ByteObject>0</ByteObject>
<HtxType>0</HtxType>
<ManyObjects>0</ManyObjects>
<SizeOfObject>0</SizeOfObject>
<BreakByAccess>0</BreakByAccess>
<BreakIfRCount>0</BreakIfRCount>
<Filename>..\src\app-board-amc2c\embot_app_board_amc2c_mbd.demo.cpp</Filename>
<ExecCommand></ExecCommand>
<Expression></Expression>
</Bp>
<Bp>
<Number>2</Number>
<Type>0</Type>
<LineNumber>296</LineNumber>
<EnabledFlag>1</EnabledFlag>
<Address>0</Address>
Expand All @@ -483,7 +499,7 @@
<Expression></Expression>
</Bp>
<Bp>
<Number>2</Number>
<Number>3</Number>
<Type>0</Type>
<LineNumber>278</LineNumber>
<EnabledFlag>1</EnabledFlag>
Expand Down Expand Up @@ -1666,7 +1682,7 @@

<Group>
<GroupName>mbd</GroupName>
<tvExp>0</tvExp>
<tvExp>1</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel>
<RteFlg>0</RteFlg>
Expand Down Expand Up @@ -1769,7 +1785,7 @@
<File>
<GroupNumber>17</GroupNumber>
<FileNumber>70</FileNumber>
<FileType>1</FileType>
<FileType>8</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
Expand All @@ -1781,7 +1797,7 @@
<File>
<GroupNumber>17</GroupNumber>
<FileNumber>71</FileNumber>
<FileType>1</FileType>
<FileType>8</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
Expand Down
12 changes: 6 additions & 6 deletions emBODY/eBcode/arch-arm/board/amc2c/application/proj/amc2c-appl-can.uvprojx
View file
Original file line number Diff line number Diff line change
Expand Up @@ -931,12 +931,12 @@
</File>
<File>
<FileName>rtw_enable_disable_motors.c</FileName>
<FileType>1</FileType>
<FileType>8</FileType>
<FilePath>..\..\..\amcbldc\application\src\model-based-design\sharedutils\rtw_enable_disable_motors.c</FilePath>
</File>
<File>
<FileName>rtw_motor_config.c</FileName>
<FileType>1</FileType>
<FileType>8</FileType>
<FilePath>..\..\..\amcbldc\application\src\model-based-design\sharedutils\rtw_motor_config.c</FilePath>
</File>
</Files>
Expand Down Expand Up @@ -2172,12 +2172,12 @@
</File>
<File>
<FileName>rtw_enable_disable_motors.c</FileName>
<FileType>1</FileType>
<FileType>8</FileType>
<FilePath>..\..\..\amcbldc\application\src\model-based-design\sharedutils\rtw_enable_disable_motors.c</FilePath>
</File>
<File>
<FileName>rtw_motor_config.c</FileName>
<FileType>1</FileType>
<FileType>8</FileType>
<FilePath>..\..\..\amcbldc\application\src\model-based-design\sharedutils\rtw_motor_config.c</FilePath>
</File>
</Files>
Expand Down Expand Up @@ -3464,12 +3464,12 @@
</File>
<File>
<FileName>rtw_enable_disable_motors.c</FileName>
<FileType>1</FileType>
<FileType>8</FileType>
<FilePath>..\..\..\amcbldc\application\src\model-based-design\sharedutils\rtw_enable_disable_motors.c</FilePath>
</File>
<File>
<FileName>rtw_motor_config.c</FileName>
<FileType>1</FileType>
<FileType>8</FileType>
<FilePath>..\..\..\amcbldc\application\src\model-based-design\sharedutils\rtw_motor_config.c</FilePath>
</File>
</Files>
Expand Down
81 changes: 2 additions & 79 deletions ...e/arch-arm/board/amc2c/application/src/app-board-amc2c/embot_app_board_amc2c_mbd.demo.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,94 +18,17 @@
// - external dependencies
// --------------------------------------------------------------------------------------------------------------------

#include "embot_hw_timer.h"

#include "adcm.h"
#include "enc.h"
#include "hall.h"
#include "pwm.h"

#include "embot_hw_motor.h"

namespace embot::app::board::amc2c::mbd {

// void hei(void *p)
// {
// static volatile uint64_t prev {0};
// static volatile uint64_t delta {0};
// volatile uint64_t no = embot::core::now();
// delta = no - prev;
// prev = no;
// }
// embot::core::Callback cc {hei, nullptr};

void Startup(embot::prot::can::Address adr)
{
// in debug mode we have to click the "RUN" button when stucked on main, before it will be unlocked by CM7
// otherwise, it seems that this initialization is not performed.
embot::hw::motor::init(embot::hw::MOTOR::one, {});
embot::app::board::amc2c::theMBD::getInstance().initialise({adr});

// embot::hw::timer::Config cfg{embot::core::time1millisec, embot::hw::timer::Mode::periodic, cc};
// embot::hw::timer::init(embot::hw::TIMER::one, cfg);
// embot::hw::timer::start(embot::hw::TIMER::one);


embot::app::board::amc2c::theMBD::getInstance().initialise({adr});
}

void OnTick(const std::vector<embot::prot::can::Frame> &input, std::vector<embot::prot::can::Frame> &output)
{

size_t numRXframes = input.size();

if(numRXframes > 0)
{
embot::prot::can::Frame f = input[0];

switch(f.id)
{
case 0x001:
{
// test 1
PwmPhaseSet(1, 2, 3);
} break;

case 0x002:
{
// test 2
} break;

default:
{
} break;
}
}


embot::app::board::amc2c::theMBD::getInstance().tick(input, output);


// static uint32_t cnt {0};
// if(0 == (cnt++ % 1))
// {
//// embot::core::print("MBD is ticking on an amc2c");
//
// uint8_t b0 = cnt&0xff;
// uint8_t b1 = (cnt>>8)&0xff;
// uint8_t b2 = (cnt>>16)&0xff;
// uint8_t b3 = (cnt>>24)&0xff;
// embot::prot::can::Frame fr1 {0x202, 4, {b0, b1, b2, b3, 0, 0, 0, 0}};
// output.push_back(fr1);
// }
//
// size_t numRXframes = input.size();
//
// if(numRXframes > 0)
// {
// // just for test: i get the first only and i send it back
// embot::prot::can::Frame f = input[0];
// output.push_back(f);
// }
embot::app::board::amc2c::theMBD::getInstance().tick(input, output);
}

} // end of namespace
Expand Down
12 changes: 1 addition & 11 deletions ...Bcode/arch-arm/board/amc2c/application/src/app-board-amc2c/embot_app_board_amc2c_test.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,17 +48,7 @@ namespace embot::app::board::amc2c::mbd {
EncInit();
HallInit();
PwmInit();


// in debug mode questa init non viene chiamata a meno che non si clicca "RUN" manualmente prima che venga sbloccato dal CM7
// embot::hw::motor::init(embot::hw::MOTOR::one, {});
// embot::app::board::amc2c::theMBD::getInstance().initialise({adr});

// embot::hw::timer::Config cfg{embot::core::time1millisec, embot::hw::timer::Mode::periodic, cc};
// embot::hw::timer::init(embot::hw::TIMER::one, cfg);
// embot::hw::timer::start(embot::hw::TIMER::one);



}

void OnTick(const std::vector<embot::prot::can::Frame> &input, std::vector<embot::prot::can::Frame> &output)
Expand Down
50 changes: 41 additions & 9 deletions ...ode/arch-arm/board/amc2c/application/src/app-board-amc2c/embot_app_board_amc2c_theMBD.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,9 +19,11 @@

#if defined(STM32HAL_BOARD_AMC2C)
#include "embot_hw_bsp_amc2c.h"
#warning READ CAREFULLY: we may use the same object for both amcbldc and amc2c
#else
//#warning READ CAREFULLY: we may use the same object for both amcbldc and amc2c
#elif defined(STM32HAL_BOARD_AMCBLDC)
#include "embot_hw_bsp_amcbldc.h"
#else
#error: choose a STM32HAL_BOARD_*
#endif

#include "embot_hw_button.h"
Expand All @@ -45,6 +47,8 @@
// --------------------------------------------------------------------------------------------------------------------
//#define TEST_DURATION_FOC

#define TEST_FOC_logvalues


// If the target setup is the KOLLMORGEN motor on RED LEGO platform,
// you need to enable the macro USE_KOLLMORGEN_SETUP, otherwise
Expand Down Expand Up @@ -469,16 +473,29 @@ bool embot::app::board::amc2c::theMBD::Impl::tick(const std::vector<embot::prot:
}


// If motor configuration parameters changed due to a SET_MOTOR_CONFIG message, then update hal as well (Only pole_pairs at the moment)
// TODO: We should perform the following update inside the architectural model after a SET_MOTOR_CONFIG message has been received
//MainConf.pwm.num_polar_couples = AMC_BLDC_Y.ConfigurationParameters_p.motorconfig.pole_pairs;

measureTick->stop();


return true;
}

#if defined(TEST_FOC_logvalues)

struct dbgFOCvalues
{
embot::hw::motor::Currents currents {0, 0, 0};
uint8_t hall {0};
embot::hw::motor::Position electricalangle {0};
int32_t position {0};
float jointangle {0.0};
std::array<int32_t, 3> pwms {0, 0, 0};

dbgFOCvalues() = default;
};

dbgFOCvalues dbgFOC {};

#endif

void embot::app::board::amc2c::theMBD::Impl::onCurrents_FOC_innerloop(void *owner, const embot::hw::motor::Currents * const currents)
{
Expand All @@ -499,8 +516,10 @@ void embot::app::board::amc2c::theMBD::Impl::onCurrents_FOC_innerloop(void *owne

// remember to manage impl->EXTFAULTisPRESSED ............

// retrieve the current value of the Hall sensors
embot::hw::motor::gethallstatus(embot::hw::MOTOR::one, AMC_BLDC_U.SensorsData_p.motorsensors.hallABC);
// retrieve the current value of the Hall sensors
uint8_t hall {0};
embot::hw::motor::gethallstatus(embot::hw::MOTOR::one, hall);
AMC_BLDC_U.SensorsData_p.motorsensors.hallABC = hall;

// retrieve the current value of the encoder
embot::hw::motor::Position electricalAngle {0};
Expand All @@ -514,7 +533,8 @@ void embot::app::board::amc2c::theMBD::Impl::onCurrents_FOC_innerloop(void *owne
electricalAngleOld = electricalAngle;

// calculate the current joint position
position = position + delta / AMC_BLDC_Y.ConfigurationParameters_p.motorconfig.pole_pairs;
size_t polepairs = (0 != AMC_BLDC_Y.ConfigurationParameters_p.motorconfig.pole_pairs) ? AMC_BLDC_Y.ConfigurationParameters_p.motorconfig.pole_pairs : 1;
position = position + delta / polepairs;

AMC_BLDC_U.SensorsData_p.motorsensors.angle = static_cast<real32_T>(electricalAngle)*0.0054931640625f; // (60 interval angle)

Expand All @@ -538,7 +558,19 @@ void embot::app::board::amc2c::theMBD::Impl::onCurrents_FOC_innerloop(void *owne
int32_T Vabc1 = static_cast<int32_T>(AMC_BLDC_Y.ControlOutputs_p.Vabc[1] * 163.83F);
int32_T Vabc2 = static_cast<int32_T>(AMC_BLDC_Y.ControlOutputs_p.Vabc[2] * 163.83F);

#if defined(TEST_FOC_logvalues)

dbgFOC.currents = currs;
dbgFOC.hall = hall;
dbgFOC.electricalangle = electricalAngle;
dbgFOC.position = position;
dbgFOC.jointangle = static_cast<float>(position) * 0.0054931640625f;
dbgFOC.pwms = {Vabc0, Vabc1, Vabc2};

#endif

embot::hw::motor::setpwm(embot::hw::MOTOR::one, Vabc0, Vabc1, Vabc2);


//#define DEBUG_PARAMS
#ifdef DEBUG_PARAMS
Expand Down
21 changes: 13 additions & 8 deletions emBODY/eBcode/arch-arm/board/amc2c/bsp/motorhal/motorhal.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -434,7 +434,7 @@ struct hall_Data
{
uint8_t order[3] = {0, 1, 2};
int32_t counter {0};
uint16_t angle {0};
int32_t angle {0};
volatile uint8_t status {0};

void reset(Mode::SWAP s = Mode::SWAP::none)
Expand Down Expand Up @@ -486,9 +486,9 @@ static uint8_t hall_Get()
// if we have order = {0, 1, 2} we dont reorder anything, so we have v = x = H3H2H1 = CBA.
// but in case swapBC is true, then we have order {0, 2, 1} which swap second w/third, so v = H2H3H1 = BCA

uint8_t v = (((v>>0)&0x1) >> _hall_internals.data.order[0]) | // H1 (pos 0) is moved to the final position
(((v>>1)&0x1) >> _hall_internals.data.order[1]) | // H2 (pos 1) is moved to the final position
(((v>>2)&0x1) >> _hall_internals.data.order[2]); // H3 (pos 2) is moved to the final position
uint8_t v = (((x>>0)&0x1) >> _hall_internals.data.order[0]) | // H1 (pos 0) is moved to the final position
(((x>>1)&0x1) >> _hall_internals.data.order[1]) | // H2 (pos 1) is moved to the final position
(((x>>2)&0x1) >> _hall_internals.data.order[2]); // H3 (pos 2) is moved to the final position
return v;
}

Expand All @@ -497,8 +497,8 @@ static void hall_OnCapture(TIM_HandleTypeDef *htim)
{
_hall_internals.data.status = hall_Get();

// in here we DONT ... perform any calibration of the encoder ....
// because in the wrist we dont have encoder. we may implement this calibration later on
// in here, we surely fill angle.
_hall_internals.data.angle = _hall_internals.mode.offset + hall_Table::status2angle(_hall_internals.data.status);
}


Expand Down Expand Up @@ -587,10 +587,15 @@ bool isstarted()
return _hall_internals.started;
}

uint8_t getstatus(void)
uint8_t getstatus()
{
return _hall_internals.data.status;
}
}

int32_t getangle()
{
return _hall_internals.data.angle;
}

} // namespace embot::hw::motor::hall {

Expand Down
3 changes: 2 additions & 1 deletion emBODY/eBcode/arch-arm/board/amc2c/bsp/motorhal/motorhal.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -160,7 +160,8 @@ bool init(const Configuration &config);
bool deinit();
bool start(const Mode &mode);
bool isstarted();
uint8_t getstatus(void);
uint8_t getstatus();
int32_t getangle();

} // namespace embot::hw::motor::hall {

Expand Down
Loading

0 comments on commit 1e1b260

Please sign in to comment.