@@ -108,12 +108,28 @@ int StepperMotor::initFOC() {
108108 // alignment necessary for encoders!
109109 // sensor and motor alignment - can be skipped
110110 // by setting motor.sensor_direction and motor.zero_electric_angle
111- _delay (500 );
112111 if (sensor){
113112 exit_flag *= alignSensor ();
114113 // added the shaft_angle update
115114 sensor->update ();
116- shaft_angle = sensor->getAngle ();
115+ shaft_angle = shaftAngle ();
116+
117+ // aligning the current sensor - can be skipped
118+ // checks if driver phases are the same as current sense phases
119+ // and checks the direction of measuremnt.
120+ if (exit_flag){
121+ if (current_sense){
122+ if (!current_sense->initialized ) {
123+ motor_status = FOCMotorStatus::motor_calib_failed;
124+ SIMPLEFOC_DEBUG (" MOT: Init FOC error, current sense not initialized"
125+ exit_flag = 0 ;
126+ }else {
127+ exit_flag *= alignCurrentSense ();
128+ }
129+ }
130+ else { SIMPLEFOC_DEBUG (" MOT: No current sense."
131+ }
132+
117133 } else {
118134 SIMPLEFOC_DEBUG (" MOT: No sensor."
119135 if ((controller == MotionControlType::angle_openloop || controller == MotionControlType::velocity_openloop)){
@@ -136,6 +152,26 @@ int StepperMotor::initFOC() {
136152 return exit_flag;
137153}
138154
155+ // Calibrate the motor and current sense phases
156+ int StepperMotor::alignCurrentSense () {
157+ int exit_flag = 1 ; // success
158+
159+ SIMPLEFOC_DEBUG (" MOT: Align current sense."
160+
161+ // align current sense and the driver
162+ exit_flag = current_sense->driverAlign (voltage_sensor_align, modulation_centered);
163+ if (!exit_flag){
164+ // error in current sense - phase either not measured or bad connection
165+ SIMPLEFOC_DEBUG (" MOT: Align error!"
166+ exit_flag = 0 ;
167+ }else {
168+ // output the alignment status flag
169+ SIMPLEFOC_DEBUG (" MOT: Success: "
170+ }
171+
172+ return exit_flag > 0 ;
173+ }
174+
139175// Encoder alignment to electrical 0 angle
140176int StepperMotor::alignSensor () {
141177 int exit_flag = 1 ; // success
@@ -261,8 +297,6 @@ void StepperMotor::loopFOC() {
261297
262298 // if open-loop do nothing
263299 if ( controller==MotionControlType::angle_openloop || controller==MotionControlType::velocity_openloop ) return ;
264- // shaft angle
265- shaft_angle = shaftAngle ();
266300
267301 // if disabled do nothing
268302 if (!enabled) return ;
@@ -271,7 +305,40 @@ void StepperMotor::loopFOC() {
271305 // This function will not have numerical issues because it uses Sensor::getMechanicalAngle()
272306 // which is in range 0-2PI
273307 electrical_angle = electricalAngle ();
274-
308+ switch (torque_controller) {
309+ case TorqueControlType::voltage:
310+ // no need to do anything really
311+ break ;
312+ case TorqueControlType::dc_current:
313+ if (!current_sense) return ;
314+ // read overall current magnitude
315+ current.q = current_sense->getDCCurrent (electrical_angle);
316+ // filter the value values
317+ current.q = LPF_current_q (current.q );
318+ // calculate the phase voltage
319+ voltage.q = PID_current_q (current_sp - current.q );
320+ // d voltage - lag compensation
321+ if (_isset (phase_inductance)) voltage.d = _constrain ( -current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit);
322+ else voltage.d = 0 ;
323+ break ;
324+ case TorqueControlType::foc_current:
325+ if (!current_sense) return ;
326+ // read dq currents
327+ current = current_sense->getFOCCurrents (electrical_angle);
328+ // filter values
329+ current.q = LPF_current_q (current.q );
330+ current.d = LPF_current_d (current.d );
331+ // calculate the phase voltages
332+ voltage.q = PID_current_q (current_sp - current.q );
333+ voltage.d = PID_current_d (-current.d );
334+ // d voltage - lag compensation - TODO verify
335+ // if(_isset(phase_inductance)) voltage.d = _constrain( voltage.d - current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit);
336+ break ;
337+ default :
338+ // no torque control selected
339+ SIMPLEFOC_DEBUG (" MOT: no torque control selected!"
340+ break ;
341+ }
275342 // set the phase voltage - FOC heart function :)
276343 setPhaseVoltage (voltage.q , voltage.d , electrical_angle);
277344}
@@ -310,56 +377,70 @@ void StepperMotor::move(float new_target) {
310377 // estimate the motor current if phase reistance available and current_sense not available
311378 if (!current_sense && _isset (phase_resistance)) current.q = (voltage.q - voltage_bemf)/phase_resistance;
312379
313- // choose control loop
380+ // upgrade the current based voltage limit
314381 switch (controller) {
315382 case MotionControlType::torque:
316- if (!_isset (phase_resistance)) voltage.q = target; // if voltage torque control
317- else voltage.q = target*phase_resistance + voltage_bemf;
318- voltage.q = _constrain (voltage.q , -voltage_limit, voltage_limit);
319- // set d-component (lag compensation if known inductance)
320- if (!_isset (phase_inductance)) voltage.d = 0 ;
321- else voltage.d = _constrain ( -target*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit);
383+ if (torque_controller == TorqueControlType::voltage){ // if voltage torque control
384+ if (!_isset (phase_resistance)) voltage.q = target;
385+ else voltage.q = target*phase_resistance + voltage_bemf;
386+ voltage.q = _constrain (voltage.q , -voltage_limit, voltage_limit);
387+ // set d-component (lag compensation if known inductance)
388+ if (!_isset (phase_inductance)) voltage.d = 0 ;
389+ else voltage.d = _constrain ( -target*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit);
390+ }else {
391+ current_sp = target; // if current/foc_current torque control
392+ }
322393 break ;
323394 case MotionControlType::angle:
395+ // TODO sensor precision: this calculation is not numerically precise. The target value cannot express precise positions when
396+ // the angles are large. This results in not being able to command small changes at high position values.
397+ // to solve this, the delta-angle has to be calculated in a numerically precise way.
324398 // angle set point
325399 shaft_angle_sp = target;
326400 // calculate velocity set point
327401 shaft_velocity_sp = feed_forward_velocity + P_angle ( shaft_angle_sp - shaft_angle );
328- shaft_velocity_sp = _constrain (shaft_velocity_sp, -velocity_limit, velocity_limit);
329- // calculate the torque command
402+ shaft_velocity_sp = _constrain (shaft_velocity_sp,-velocity_limit, velocity_limit);
403+ // calculate the torque command - sensor precision: this calculation is ok, but based on bad value from previous calculation
330404 current_sp = PID_velocity (shaft_velocity_sp - shaft_velocity); // if voltage torque control
331405 // if torque controlled through voltage
332- // use voltage if phase-resistance not provided
333- if (!_isset (phase_resistance)) voltage.q = current_sp;
334- else voltage.q = _constrain ( current_sp*phase_resistance + voltage_bemf , -voltage_limit, voltage_limit);
335- // set d-component (lag compensation if known inductance)
336- if (!_isset (phase_inductance)) voltage.d = 0 ;
337- else voltage.d = _constrain ( -current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit);
406+ if (torque_controller == TorqueControlType::voltage){
407+ // use voltage if phase-resistance not provided
408+ if (!_isset (phase_resistance)) voltage.q = current_sp;
409+ else voltage.q = _constrain ( current_sp*phase_resistance + voltage_bemf , -voltage_limit, voltage_limit);
410+ // set d-component (lag compensation if known inductance)
411+ if (!_isset (phase_inductance)) voltage.d = 0 ;
412+ else voltage.d = _constrain ( -current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit);
413+ }
338414 break ;
339415 case MotionControlType::velocity:
340- // velocity set point
416+ // velocity set point - sensor precision: this calculation is numerically precise.
341417 shaft_velocity_sp = target;
342418 // calculate the torque command
343419 current_sp = PID_velocity (shaft_velocity_sp - shaft_velocity); // if current/foc_current torque control
344420 // if torque controlled through voltage control
345- // use voltage if phase-resistance not provided
346- if (!_isset (phase_resistance)) voltage.q = current_sp;
347- else voltage.q = _constrain ( current_sp*phase_resistance + voltage_bemf , -voltage_limit, voltage_limit);
348- // set d-component (lag compensation if known inductance)
349- if (!_isset (phase_inductance)) voltage.d = 0 ;
350- else voltage.d = _constrain ( -current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit);
421+ if (torque_controller == TorqueControlType::voltage){
422+ // use voltage if phase-resistance not provided
423+ if (!_isset (phase_resistance)) voltage.q = current_sp;
424+ else voltage.q = _constrain ( current_sp*phase_resistance + voltage_bemf , -voltage_limit, voltage_limit);
425+ // set d-component (lag compensation if known inductance)
426+ if (!_isset (phase_inductance)) voltage.d = 0 ;
427+ else voltage.d = _constrain ( -current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit);
428+ }
351429 break ;
352430 case MotionControlType::velocity_openloop:
353- // velocity control in open loop
431+ // velocity control in open loop - sensor precision: this calculation is numerically precise.
354432 shaft_velocity_sp = target;
355433 voltage.q = velocityOpenloop (shaft_velocity_sp); // returns the voltage that is set to the motor
356- voltage.d = 0 ; // TODO d-component lag-compensation
434+ voltage.d = 0 ;
357435 break ;
358436 case MotionControlType::angle_openloop:
359- // angle control in open loop
437+ // angle control in open loop -
438+ // TODO sensor precision: this calculation NOT numerically precise, and subject
439+ // to the same problems in small set-point changes at high angles
440+ // as the closed loop version.
360441 shaft_angle_sp = target;
361442 voltage.q = angleOpenloop (shaft_angle_sp); // returns the voltage that is set to the motor
362- voltage.d = 0 ; // TODO d-component lag-compensation
443+ voltage.d = 0 ;
363444 break ;
364445 }
365446}
0 commit comments