From b9eefb2c10890b465fb317af17afe034236063df Mon Sep 17 00:00:00 2001 From: EmaMaker Date: Wed, 16 Dec 2020 19:21:52 +0100 Subject: [PATCH] use 5kHz as pwm frequency for motors So we don't get the motors stopping to turn when changing direction at high speeds because of current limit on the vnh7070 --- src/motors_movement/motor.cpp | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/src/motors_movement/motor.cpp b/src/motors_movement/motor.cpp index cb1589f..da8596e 100644 --- a/src/motors_movement/motor.cpp +++ b/src/motors_movement/motor.cpp @@ -11,6 +11,7 @@ Motor::Motor(int a, int b, int pwm, int angle_){ pinMode(pinA, OUTPUT); pinMode(pinB, OUTPUT); pinMode(pinPwm, OUTPUT); + analogWriteFrequency(pinPwm, 5000); } Motor::Motor(){ } @@ -38,12 +39,14 @@ void Motor::drive(int speed){ } void Motor::test(){ - digitalWrite(pinA, 1); - digitalWrite(pinB, 0); - analogWrite(pinPwm, 255); - delay(1500); digitalWrite(pinA, 0); digitalWrite(pinB, 1); analogWrite(pinPwm, 255); delay(1500); + + digitalWrite(pinA, 1); + digitalWrite(pinB, 0); + analogWrite(pinPwm, 255); + delay(1500); + } \ No newline at end of file