Skip to content

Commit 055ebf3

Browse files
Merge pull request #211 from runger1101001/dev
add routines for 1-pwm in driver code
2 parents c9fd8d0 + 6f90ee2 commit 055ebf3

File tree

3 files changed

+44
-1
lines changed

3 files changed

+44
-1
lines changed

src/BLDCMotor.cpp

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -345,7 +345,7 @@ void BLDCMotor::move(float new_target) {
345345
// when switching to a 2-component representation.
346346
if( controller!=MotionControlType::angle_openloop && controller!=MotionControlType::velocity_openloop )
347347
shaft_angle = shaftAngle(); // read value even if motor is disabled to keep the monitoring updated but not in openloop mode
348-
// get angular velocity
348+
// get angular velocity TODO the velocity reading probably also shouldn't happen in open loop modes?
349349
shaft_velocity = shaftVelocity(); // read value even if motor is disabled to keep the monitoring updated
350350

351351
// if disabled do nothing

src/drivers/hardware_api.h

Lines changed: 22 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -19,6 +19,17 @@ typedef struct GenericDriverParams {
1919
float dead_zone;
2020
} GenericDriverParams;
2121

22+
/**
23+
* Configuring PWM frequency, resolution and alignment
24+
* - Stepper driver - 2PWM setting
25+
* - hardware specific
26+
*
27+
* @param pwm_frequency - frequency in hertz - if applicable
28+
* @param pinA pinA pwm pin
29+
*
30+
* @return -1 if failed, or pointer to internal driver parameters struct if successful
31+
*/
32+
void* _configure1PWM(long pwm_frequency, const int pinA);
2233

2334
/**
2435
* Configuring PWM frequency, resolution and alignment
@@ -80,6 +91,17 @@ void* _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const
8091
*/
8192
void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l);
8293

94+
/**
95+
* Function setting the duty cycle to the pwm pin (ex. analogWrite())
96+
* - Stepper driver - 2PWM setting
97+
* - hardware specific
98+
*
99+
* @param dc_a duty cycle phase A [0, 1]
100+
* @param dc_b duty cycle phase B [0, 1]
101+
* @param params the driver parameters
102+
*/
103+
void _writeDutyCycle1PWM(float dc_a, void* params);
104+
83105
/**
84106
* Function setting the duty cycle to the pwm pin (ex. analogWrite())
85107
* - Stepper driver - 2PWM setting

src/drivers/hardware_specific/generic_mcu.cpp

Lines changed: 21 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -5,6 +5,18 @@
55
__attribute__((weak)) void analogWrite(uint8_t pin, int value){ };
66
#endif
77

8+
// function setting the high pwm frequency to the supplied pin
9+
// - Stepper motor - 1PWM setting
10+
// - hardware speciffic
11+
// in generic case dont do anything
12+
__attribute__((weak)) void* _configure1PWM(long pwm_frequency, const int pinA) {
13+
GenericDriverParams* params = new GenericDriverParams {
14+
.pins = { pinA },
15+
.pwm_frequency = pwm_frequency
16+
};
17+
return params;
18+
}
19+
820
// function setting the high pwm frequency to the supplied pins
921
// - Stepper motor - 2PWM setting
1022
// - hardware speciffic
@@ -59,6 +71,15 @@ __attribute__((weak)) void* _configure6PWM(long pwm_frequency, float dead_zone,
5971
}
6072

6173

74+
// function setting the pwm duty cycle to the hardware
75+
// - Stepper motor - 1PWM setting
76+
// - hardware speciffic
77+
__attribute__((weak)) void _writeDutyCycle1PWM(float dc_a, void* params){
78+
// transform duty cycle from [0,1] to [0,255]
79+
analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_a);
80+
}
81+
82+
6283
// function setting the pwm duty cycle to the hardware
6384
// - Stepper motor - 2PWM setting
6485
// - hardware speciffic

0 commit comments

Comments
 (0)