cleaned Motion class

This commit is contained in:
hhaupt 2024-06-14 02:23:12 +02:00
parent b1bb29aa1c
commit 2b0cd00ddd
2 changed files with 44 additions and 15 deletions

View File

@ -23,16 +23,30 @@
#define CHANNEL_RIGHT LEDC_CHANNEL_4
#define DUTY_RES LEDC_TIMER_13_BIT // Set duty resolution to 13 bits
#define FREQUENCY (5000) // Frequency in Hertz. Set frequency at 5 kHz
struct moveCMD{
uint32_t duration;
MotionDetection* imu;
};
#define DEFAULT_BASE_VALUE 3900
class Motor{
public:
Motor(uint8_t pin, ledc_timer_t timer, ledc_channel_t channel);
/**
* @brief Initializes the motor
*/
void begin(void);
/**
* @brief Set the Speed by changing the pwm. To avoid current peaks, a linear ramp-up is used.
*
* @attention it is requried at any time to use that method to access the motors or methods of the motionclass to avoid such peaks.
*
* @param duty the duty cyle that should be set, can be between 0-8192
*/
void setSpeed(uint16_t duty);
/**
* @brief returns the currently activ speed
*
* @return current speedvalue of the motor
*/
uint16_t getSpeed(void);
protected:
uint8_t pin;
@ -45,8 +59,8 @@ class Motor{
class Motion{
protected:
static MotionDetection* imuInst;
static inline uint16_t RIGHT_MOTOR_DUTY = 3900;
static inline uint16_t LEFT_MOTOR_DUTY = 3900;
static inline uint16_t RIGHT_MOTOR_DUTY = DEFAULT_BASE_VALUE;
static inline uint16_t LEFT_MOTOR_DUTY = DEFAULT_BASE_VALUE;
static const int MOTOR_RIGHT_PIN = 11;
static const int MOTOR_LEFT_PIN = 12;
static void moveTask(void * args);
@ -61,7 +75,7 @@ protected:
static inline int correctionThreshold = 150;
public:
//Shared Timer to sync movement
//Instances of the motors, so they can also be used from outside to set values for the motors directly.
static inline Motor left = Motor(MOTOR_LEFT_PIN,TIMER,CHANNEL_LEFT);
static inline Motor right = Motor(MOTOR_RIGHT_PIN,TIMER,CHANNEL_RIGHT);
@ -84,7 +98,7 @@ public:
* @param baseValue The value that is used to start with the calibrated movement. Defaults to 3900.
* If the Dezibot is not moving forward at all increasing the value may help. If the robot is just jumping up and down but not forward, try a lower value.
*/
static void move(uint32_t moveForMs=0,uint baseValue=3900);
static void move(uint32_t moveForMs=0,uint baseValue=DEFAULT_BASE_VALUE);
/**
* @brief Rotate clockwise for a certain amount of time.
@ -92,7 +106,7 @@ public:
* @param rotateForMs Representing the duration of rotating clockwise in milliseconds, or 0 to rotate until another movecmd is issued. Default is 0
* @param baseValue The value that is used to start with the calibrated movement (not released yet, currently just the used value)
*/
static void rotateClockwise(uint32_t rotateForMs=0,uint baseValue=3900);
static void rotateClockwise(uint32_t rotateForMs=0,uint baseValue=DEFAULT_BASE_VALUE);
/**
* @brief Rotate anticlockwise for a certain amount of time.
@ -100,7 +114,7 @@ public:
* @param rotateForMs Representing the duration of rotating anticlockwise in milliseconds or 0 to let the robot turn until another movecommand is issued. Default is 0.
* @param baseValue The value that is used to start with the calibrated movement (not released yet, currently just the used value).
*/
static void rotateAntiClockwise(uint32_t rotateForMs=0,uint baseValue=3900);
static void rotateAntiClockwise(uint32_t rotateForMs=0,uint baseValue=DEFAULT_BASE_VALUE);
/**
* @brief stops any current movement, no matter if timebased or endless
@ -114,7 +128,7 @@ public:
* @param moveForMs how many ms should the robot move, or 0 to let the robot move until another move command is mentioned, default is 0
* @param baseValue the duty value that is used for the movement, default is 0
*/
static void moveWithoutCorrection(uint32_t moveForMs=0, uint baseValue = 3900);
static void moveWithoutCorrection(uint32_t moveForMs=0, uint baseValue = DEFAULT_BASE_VALUE);
};

View File

@ -23,9 +23,24 @@ void Motor::begin(void){
};
void Motor::setSpeed(uint16_t duty){
this->duty = duty;
int difference = duty-this->getSpeed();
if (difference > 0){
for(int i = 0;i<difference;i+=difference/20){
this->duty += difference/20;
ledc_set_duty(LEDC_MODE,this->channel,duty);
ledc_update_duty(LEDC_MODE,this->channel);
delayMicroseconds(5);
}
} else {
for(int i = 0;i>difference;i-=abs(difference/20)){
this->duty -= abs(difference/20);
ledc_set_duty(LEDC_MODE,this->channel,duty);
ledc_update_duty(LEDC_MODE,this->channel);
delayMicroseconds(5);
}
}
};
uint16_t Motor::getSpeed(void){