/*
* motor_control.c
*
* Created: 16/02/2024 13:44:02
* Author: cbsadmin
*/
#include <math.h>
#include "samc21.h"
#include "functions.h"
#include "rtc.h"
#include "system.h"
#include "canopen.h"
#include "canopen_object_dictionary.h"
#include "canopen_sdo.h"
#include "canopen_pdo.h"
#include "canopen_dsp402.h"
#include "device_jetnado_ecu.h"
#include "motor_control.h"
/***************** CONSTANTS **************************/
const int16_t min_belt_speed_setting = 5 ; // if changed update min_motor_rpm
const int16_t mpm_to_rpm = 38 ; // if changed update min_motor_rpm
const int32_t motor_current_limit = 28000 ;
const int16_t torque_limit = 1000 ;
const int32_t starting_current = 2250 ;
const int32_t av_speed_factor = 10 ;
const int32_t av_torque_factor = 20 ;
const float tacho_length_m = 0.0160792 ; // was 0.00788 // m 12 pulses/rev
//float tacho_length_m = 0.0080396 ; // was 0.00788 // m 12 pulses/rev
////////////////////////////////// RUNTIME VARIABLES /////////////////////////
static int32_t motor_user_current_limit ; // user current limit
static int32_t speed_actual ;
static int32_t torque_actual ;
static int32_t av_speed_actual ;
static int32_t av_torque_actual ;
static int32_t voltage ;
static int32_t torque_target ;
static int32_t speed_target ;
static int32_t starting_torque ;
static int32_t direction ;
volatile time_recrord_t time_record ;
void motor_control(uint16_t timeout)
{
static volatile uint32_t last_run = 0 ;
if ((time_ms - last_run) > timeout)
{
last_run = time_ms ;
// start code body
//***************************************************** safe to move ***********************************************
voltage = (server_objects[DRIVE_VOLTAGE_ACTUAL_VALUE].value[1] + server_objects[DRIVE_VOLTAGE_ACTUAL_VALUE].value[2]) / 2 ;
if ( ENABLE_UNDERVOLTAGE_LOCKOUT && (voltage > 0 ) && (voltage < MINIMUM_SUPPLY_VOLTAGE ) )
ecu_data.low_voltage = 1 ;
else ecu_data.low_voltage = 0 ;
if (network_status[MASTER_NODE_ID].heartbeat_present ) ecu_data.comms_failure = 0 ; // comms check
else ecu_data.comms_failure = 1 ;
if (GUARD_SAFETY_ENABLE) ecu_data.guard_open = !pin_read(guard_switch_pin) ; // Update guard switch status
if(!pin_read(estop_pin)) // if e-stop pressed update ecu flag
{
ecu_data.estop_pressed = 1 ;
ecu_data.stopped = 1 ;
}
// Ascertain motor safe to move
if (ecu_data.comms_failure || ecu_data.low_voltage || ecu_data.estop_pressed || ecu_data.guard_open )
{
ecu_data.safe_to_move = 0 ;
torque_target = 0 ;
ecu_data.starting = 0 ;
ecu_data.stopping = 0 ;
ecu_data.running = 0 ;
ecu_data.stopped = 1 ;
}
else
{
ecu_data.safe_to_move = 1 ;
}
// end safety checks *************************************************************
ecu_data.airPressureCurrent = (uint8_t) (ret_map( adc_result[1][6] , 10 , 4000 , 0 , 160 ) ) ;
ecu_data.distance = (int32_t) (cable_taco_counts * tacho_length_m * 10) ;
//ecu_data.cable_speed = ecu_data.cable_speed ;
if (ecu_data.enabled)
{
speed_actual = ( ret_absolute_value_int(server_objects[VELOCITY_ACTUAL_VALUE].value[1])
+ ret_absolute_value_int(server_objects[VELOCITY_ACTUAL_VALUE].value[2]) ) / 2 ;
av_speed_actual = (av_speed_actual * (av_speed_factor - 1) + speed_actual) / av_speed_factor ;
torque_actual = (ret_absolute_value_int( server_objects[TORQUE_ACTUAL_VALUE].value[1] )
+ ret_absolute_value_int(server_objects[TORQUE_ACTUAL_VALUE].value[2]) ) / 2 ;
av_torque_actual = (av_torque_actual * (av_torque_factor - 1) + torque_actual ) / av_torque_factor ;
ecu_data.cable_speed = (int8_t) average_cable_speed ;
ecu_data.speed = (int8_t) (speed_actual / mpm_to_rpm ) ; // sent to HMI
ecu_data.torque = av_torque_actual * motor_user_current_limit / motor_current_limit ;
}
if (!ecu_data.enabled )
{
ecu_data.torque = 0 ;
ecu_data.speed = 0 ;
motor_user_current_limit = (motor_current_limit * hmi_cmd.torqueSetpoint) / 1000U ; // workout unity fraction of max current and multiply by max current
server_objects[NOMINAL_CURRENT].value[1] = (motor_current_limit * hmi_cmd.torqueSetpoint) / 10000U ; // update nominal current in units of cA
server_objects[NOMINAL_CURRENT].value[2] = (motor_current_limit * hmi_cmd.torqueSetpoint) / 10000U ;
starting_torque = starting_current * 1000 / motor_user_current_limit ;
speed_actual = 0 ;
}
// end code body
}
}
void speed_update(uint16_t timeout)
{
static uint32_t last_run = 0 ;
static int32_t speed_error ;
static int32_t adjustment ;
if (ecu_data.enabled && ((time_ms - last_run) > timeout))
{
last_run = time_ms ;
// start code body
// speed control
speed_target = (int32_t) (hmi_cmd.speedSetpoint * mpm_to_rpm) ;
// Direction setup
if (speed_target > 0)
direction = 1 ;
if (speed_target < 0) // if a reverse speed has been set
{
speed_target *= -1 ; // make the target speed positive again but
direction = -1 ; // set the reverse factor used to invert the final torque settings
}
if(speed_target > 0)
{
ecu_data.running = 1 ;
speed_error = ret_absolute_value_int(speed_actual - speed_target) ;
//adjustment = ret_map_int(speed_error, 1, 1000, 0, 100 ) ;
if (speed_error < 200) adjustment = 1 ;
if ((speed_error > 199) && (speed_error < 500)) adjustment = 5 ;
if (speed_error > 499) adjustment = 20 ;
if (speed_actual > speed_target) // slow down
torque_target -= adjustment ;
if (speed_actual < speed_target ) // speed up
torque_target += adjustment ;
if (torque_target > torque_limit)
torque_target = torque_limit ;
if (torque_target < 0 )
torque_target = 0 ;
}
/*
if (speed_target == 0)
torque_target = 0 ;
*/
server_objects[TARGET_TORQUE].value[1] = (int32_t) (torque_target * direction * -1) ;
server_objects[TARGET_TORQUE].value[2] = (int32_t) (torque_target * direction ) ;
// end code body
}
}
void ecu_data_initialization(void)
{
ecu_data.stopped = 1 ;
/*
ecu_data.cable_speed = 0 ;
ecu_data.stopping = 0 ;
ecu_data.starting = 0 ;
ecu_data.running = 0 ;
ecu_data.enabled = 0 ;
ecu_data.user_enable = 0 ;
ecu_data.safe_to_move = 0 ;
av_speed_actual = 0 ;
av_torque_actual = 0 ;*/
}
void motor_ds402_control(uint16_t run_interval)
{
static uint32_t last_run_time = 0 ;
if ((time_ms - last_run_time) > run_interval)
{
last_run_time = time_ms ;
// motor power enable
if ( ecu_data.user_enable && !ecu_data.enabled && ecu_data.safe_to_move)
{
dsp402_power_on( 1) ;
dsp402_power_on( 2) ;
if (network_status[1].powered_up && network_status[2].powered_up)
{
ecu_data.enabled = 1 ;
}
}
// motor power disable
if (ecu_data.stopped && ecu_data.enabled && !ecu_data.user_enable)
{
dsp402_power_off( 1) ;
dsp402_power_off( 2) ;
if (network_status[1].powered_down && network_status[2].powered_down)
{
ecu_data.enabled = 0 ;
ecu_data.running = 0 ;
}
}
}
}
void motor_start(uint16_t run_interval)
{
static uint32_t last_run_time = 0 ;
//starting_torque = starting_current_mA / 28 ;
if (ecu_data.enabled && !ecu_data.running && !network_status[0].powering_down && ((time_ms - last_run_time) > run_interval))
{
// start code body
if ( !ecu_data.starting && (speed_target > 0) )
{
ecu_data.starting = 1 ;
ecu_data.stopped = 0 ;
ecu_data.stopping = 0 ;
torque_target = starting_torque ;
}
// Motor starting COULD BENEFIT FROM USING START BUTTON TO OVER RIDE AND FOR TORQUE INCREASE
if ( ecu_data.starting && (speed_actual < speed_target)
&& (torque_target < torque_limit) )
{
torque_target += 10 ;
}
// Switch from starting to running
if ( ecu_data.starting && ( speed_actual > ( min_belt_speed_setting * mpm_to_rpm ) ) )
{
ecu_data.running = 1 ;
ecu_data.starting = 0 ;
}
server_objects[TARGET_TORQUE].value[1] = (int32_t) (torque_target * direction * -1) ;
server_objects[TARGET_TORQUE].value[2] = (int32_t) (torque_target * direction ) ;
// end code body
last_run_time = time_ms ;
}
}
void user_button_ctrl(uint16_t run_interval)
{
static uint32_t last_run_time = 0 ;
if ((time_ms - last_run_time) > run_interval)
{
// start code body
if (hmi_cmd.machineStart) // User enabling
{
ecu_data.user_enable = 1 ;
ecu_data.cable_stall = 0 ;
ecu_data.belt_slip = 0 ;
}
if (hmi_cmd.machineStop) // User disabling
{
ecu_data.user_enable = 0 ;
}
// end code body
last_run_time = time_ms ;
}
}
void motor_stopping(uint16_t run_interval)
{
static uint32_t last_run_time = 0 ;
if ((time_ms - last_run_time) > run_interval)
{
time_record.last_stop = time_ms ;
// start code body
// Stopping
if ( (ecu_data.running || ecu_data.starting) && !ecu_data.user_enable && !ecu_data.stopped )
{
time_record.last_stopping = time_ms ;
ecu_data.stopping = 1 ;
ecu_data.running = 0 ;
ecu_data.starting = 0 ;
}
if (ecu_data.stopping)
{
// Ramp torque down
if (torque_target >= 200)
{
torque_target -= 20 ;
}
else // stop!
{
torque_target = 0 ;
ecu_data.stopping = 0 ;
ecu_data.stopped = 1 ;
}
}
server_objects[TARGET_TORQUE].value[1] = (int32_t) (torque_target * direction * -1) ;
server_objects[TARGET_TORQUE].value[2] = (int32_t) (torque_target * direction ) ;
// end code body
}
}
void belt_slip_detection(uint16_t run_interval , uint16_t slip_detection_grace , float belt_slip_factor)
{
static uint32_t last_run_time = 0 ;
if (ENABLE_SLIP_DETECTION && ENABLE_TACHO_OUTPUT && ((time_ms - last_run_time) > run_interval))
{
last_run_time = time_ms ;
// code body
static int16_t belt_slip_time = 0 ;
uint16_t grace_step = slip_detection_grace / run_interval ;
if (!grace_step) grace_step = 1 ;
if ((( (float) ecu_data.cable_speed / (float)(speed_actual / mpm_to_rpm) ) < belt_slip_factor)
&& ecu_data.running )
{
belt_slip_time += run_interval ;
if (belt_slip_time > slip_detection_grace)
{
ecu_data.belt_slip = 1 ;
ecu_data.starting = 0 ;
ecu_data.running = 0 ;
ecu_data.stopping = 0 ;
ecu_data.stopped = 1 ;
ecu_data.user_enable = 0 ;
torque_target = 0 ;
belt_slip_time = 0 ;
}
}
else if (belt_slip_time >= grace_step)
{
belt_slip_time -= grace_step ;
}
}
}
//const float stall_belt_speed_factor = 0.5 ;
void cable_stall_detection(uint16_t run_interval , int16_t min_cable_speed)
{
static uint32_t last_run_time = 0 ;
if (ENABLE_STALL_DETECTION && ENABLE_TACHO_OUTPUT && (time_ms - last_run_time) > run_interval)
{
last_run_time = time_ms ;
// code body
if ((ecu_data.cable_speed < min_cable_speed) && ecu_data.running && torque_target > 990 )
{
ecu_data.cable_stall = 1 ;
ecu_data.starting = 0 ;
ecu_data.running = 0 ;
ecu_data.stopping = 0 ;
ecu_data.stopped = 1 ;
ecu_data.user_enable = 0 ;
torque_target = 0 ;
}
}
}
So a new problem cropped up, same thing, variable being ignored at random. Making it "static" solves this. The variable in only used in this one C file. So why look at code anywhere else to determine it's usefulness ?
OK so the original function I copied off the net used "char" or "int" (can't remember), my compiler complained, so I changed to uint8_t to shut it up
Using uint8_t also makes the code less efficient because on a 32 bit machine it has to add (at least) an extra `uxtb` (or `and #0xFF` etc) to the loop.
What does this mean? I'm interested. Are you saying that 16 bits is the smallest a 32 bit (ARM) machine will work with anyway? I am aware that an ARM0+ has the ability to transfer two peices af data on it's 32 bit bus at once due to the thumb instruction set (and that is as clever as I get) so I tend to not oversize variables where practical.
Compilers are rarely broken. [...] As we never get any code, it isn't very helpful to reply to these threads.
These are the compiler options I don't know if I need the first two, for a micro controller application they sound like trouble.
If you are talking about -ffunction-sections and -fdata-sections, they are extremely useful for MCUs with limited resources.
(Attachment Link)
void motor_control(uint16_t timeout)
{
static volatile uint32_t last_run = 0 ;
Code: [Select]
void motor_control(uint16_t timeout)
{
static volatile uint32_t last_run = 0 ;
This line does not make sense. Declaring last_run as volatile does not make it global. Variables defined within a function are local to that function.
Using uint8_t also makes the code less efficient because on a 32 bit machine it has to add (at least) an extra `uxtb` (or `and #0xFF` etc) to the loop.
What does this mean? I'm interested. Are you saying that 16 bits is the smallest a 32 bit (ARM) machine will work with anyway? I am aware that an ARM0+ has the ability to transfer two peices af data on it's 32 bit bus at once due to the thumb instruction set (and that is as clever as I get) so I tend to not oversize variables where practical.
No, 32 bits is the smallest size 32 bit Arm machines (or MIPS, RISC-V, Power, SPARC etc) have arithmetic operations for.
If you specify that some variable can only have values from 0 to 255, and then you add 1 to it, then the 32 bit add (the only kind it has) can result in an answer of 256, and extra work is needed to check for that and convert overflowed values back into range.
So they eliminate dead code. Well all of my code is bare metal written by me. So are we talking about actual statements in my code that are deemed unnecessary or is this cleaning up code generated by the compiler? Basically will try to optimize out things?

What effect does the debug type of compile have? this was set. I have now changed it to release.
I am pretty sure you love your hand-crafted well-thought-out code (who doesn't!) and would despise anyone who dares to call it "dead code"
To the best of my knowledge and experience, -ffunction-sections option works on per-function basis -- a function either makes its way into the final binary, or not. The option does not clip any code from a function itself. It does not re-arrange if() or while() blocks, or similar.
Well it's not a case of loving my code, I try to write efficiently as I don't want the compiler to have to do my job. What I am saying is, are these options likely to lead to any issues? You know like increasing the optimization settings do sometimes.
I don't want to hit a brick wall due to some optimization
void can_exd_filter_setup(Can * instance , uint32_t id , uint8_t filter_element_n , void * variable )
{
can_0_rx_variable_index[filter_element_n + CAN_0_STD_MESSAGE_FILTER_NUM ] = variable ;
void panic_fail(int errcode)
{
// simple error handler for the whole project.
// print errcode to uart, blink led errcode times, beep a sound, whatever.
}
void can_exd_filter_setup(Can * instance , uint32_t id , uint8_t filter_element_n , void * variable )
{
if(filter_element_n >= N_ELEM(can_0_rx_variable_index))
panic_fail(42);
can_0_rx_variable_index[filter_element_n + CAN_0_STD_MESSAGE_FILTER_NUM ] = variable ;
it is complicated as 2 months of CAN Open code are required to run anything. The vast majority of the code (120'000 chars when I last looked) are simply to get me to a point where I can run a motor.
static int16_t i16_bit_var ;
...
switch(number_bytes)
{
case 0x82 :
mymemcpy(&i16_bit_var , &can_data[4] , number_bytes) ;
Is C++ better in this respect? I have been trying to get started in it for a long time and it's a natural step.

The variable is only defined in motor.c as "volatile int32_t nominal_current".
A function defined in motor.c and declared in motor.h has a line that says:
nominal_current = 28000 ;
The function which sets up other variables too that are possibly also being optimized out but not causing an issue is called in main() before while(1){main program loop}.
A function called control() defined in motor.c and declared in motor.h uses nominal_current in 4 calculations and is called in the while(1) loop in main()
So if I pause after the function has run to set the variable up it has a value of 28000, if I pause in the control() function it's value is now "0".
Here, I quickly found a serious bug which overruns other variables in .bss:Code: [Select]static int16_t i16_bit_var ;
...
switch(number_bytes)
{
case 0x82 :
mymemcpy(&i16_bit_var , &can_data[4] , number_bytes) ;
You are copying 130(!!!) bytes from something that is only 4 bytes long (can_data[4 to 7]), to something that is only 2 bytes long (int16), overrunning the target by 128 bytes.
Always with memcpy, be super careful with the length, and prefer the pattern of using sizeof target as the final parameter, instead of some int n:
mymemcpy(&i16_bit_var, &can_data[4], sizeof i16_bit_var);
Homework: find the rest of such bugs and fix them.
Now you see how changing compiler settings or adding random keywords causes the compiler/linker to place variables in different order, so that something else gets corrupted and the bug seems to move elsewhere. Therefore by doing such changes, you are wasting time.