155 lines
4.3 KiB
C++
155 lines
4.3 KiB
C++
#include "motorControl.h"
|
|
|
|
#include <Arduino.h>
|
|
|
|
#include "config.h"
|
|
|
|
MotorControl::MotorControl() {
|
|
|
|
}
|
|
|
|
void MotorControl::init(uint8_t pwm_pin, uint8_t pwm_channel, uint8_t direction_pin_1, uint8_t direction_pin_2) {
|
|
this->pwm_pin = pwm_pin;
|
|
this->pwm_channel = pwm_channel;
|
|
this->direction_pin_1 = direction_pin_1;
|
|
this->direction_pin_2 = direction_pin_2;
|
|
|
|
pinMode(this->direction_pin_1, OUTPUT);
|
|
pinMode(this->direction_pin_2, OUTPUT);
|
|
|
|
digitalWrite(this->direction_pin_1, LOW);
|
|
digitalWrite(this->direction_pin_2, LOW);
|
|
|
|
ledcSetup(this->pwm_channel, PWM_FREQ, PWM_RES);
|
|
ledcAttachPin(this->pwm_pin, this->pwm_channel);
|
|
ledcWrite(this->pwm_channel, 0);
|
|
}
|
|
|
|
void MotorControl::runMotorControl() {
|
|
uint64_t now_millis = millis();
|
|
if (now_millis - this->last_millis > UPDATE_TIME) {
|
|
|
|
// Absolute difference between target_speed and speed
|
|
uint8_t abs_difference = abs(this->target_speed - this->speed);
|
|
|
|
// Difference between target_speed and speed
|
|
int16_t difference = this->target_speed - this->speed;
|
|
|
|
// Check that the target speed is close to 0 and that the abs_difference is lower than SPEED_STEPS
|
|
if (abs(this->target_speed) < SPEED_STEPS && abs_difference < SPEED_STEPS) {
|
|
this->setSpeed(0);
|
|
return;
|
|
}
|
|
|
|
// Correct speed
|
|
if (abs_difference < SPEED_STEPS) {
|
|
return;
|
|
}
|
|
|
|
// Positive or negative tagret speed
|
|
if (this->target_speed >= 0) {
|
|
|
|
// Positive or negative speed
|
|
if (this->speed >= 0) {
|
|
if (difference > 0) {
|
|
this->accelerate(SPEED_STEPS);
|
|
} else {
|
|
this->accelerate(-SPEED_STEPS);
|
|
}
|
|
} else {
|
|
this->accelerate(SPEED_STEPS);
|
|
}
|
|
|
|
} else {
|
|
|
|
// Positive or negative speed
|
|
if (this->speed >= 0) {
|
|
this->accelerate(-SPEED_STEPS);
|
|
} else {
|
|
if (difference > 0) {
|
|
this->accelerate(SPEED_STEPS);
|
|
} else {
|
|
this->accelerate(-SPEED_STEPS);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
this->last_millis = millis();
|
|
}
|
|
|
|
void MotorControl::setTargetSpeed(int16_t speed) {
|
|
//TODO: Exceptionhandling
|
|
if (speed <= 100 || speed <= -100) {
|
|
this->target_speed = speed;
|
|
} else {
|
|
Serial.println("Invalid Argument in MotorControl::setTargetSpeed");
|
|
}
|
|
}
|
|
|
|
void MotorControl::stop() {
|
|
this->target_speed = 0;
|
|
}
|
|
|
|
void MotorControl::emergencyStop() {
|
|
setSpeed(0);
|
|
}
|
|
|
|
void MotorControl::toString() {
|
|
Serial.printf("Target speed: %d, speed: %d, direction: %d \n", this->target_speed, this->speed, this->direction);
|
|
}
|
|
|
|
int16_t MotorControl::getSpeed() {
|
|
return this->speed;
|
|
}
|
|
|
|
bool MotorControl::isTargetSpeedReached() {
|
|
if (this->target_speed == this->speed) {
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
void MotorControl::setSpeed(int16_t speed) {
|
|
//TODO: Exceptionhandling
|
|
if (speed <= 100 || speed <= -100) {
|
|
this->speed = speed;
|
|
} else {
|
|
Serial.println("Invalid Argument in MotorControl::setSpeed");
|
|
return;
|
|
}
|
|
|
|
if (speed == 0) {
|
|
this->direction = 0;
|
|
digitalWrite(this->direction_pin_1, LOW);
|
|
digitalWrite(this->direction_pin_2, LOW);
|
|
ledcWrite(this->pwm_channel, 0);
|
|
return;
|
|
}
|
|
|
|
uint8_t pwm_val = map(abs(speed), 0, 100, PWM_MIN, PWM_MAX);
|
|
|
|
if ((this->direction == 1 || this->direction == 0) && speed < 0){ // new direction backward
|
|
this->direction = 2;
|
|
digitalWrite(this->direction_pin_1, LOW);
|
|
digitalWrite(this->direction_pin_2, HIGH);
|
|
} else if ((this->direction == 2 || this->direction == 0) && speed > 0){ // new direction forward
|
|
this->direction = 1;
|
|
digitalWrite(this->direction_pin_1, HIGH);
|
|
digitalWrite(this->direction_pin_2, LOW);
|
|
}
|
|
|
|
ledcWrite(this->pwm_channel, pwm_val);
|
|
Serial.printf("pwm_val: %d, ", pwm_val);
|
|
}
|
|
|
|
void MotorControl::accelerate(int8_t acc) {
|
|
//TODO: Exceptionhandling
|
|
//TODO: make a stop befor a direction change
|
|
if (abs(acc) > 2 * SPEED_STEPS) {
|
|
Serial.println("Invalid Argument in MotorControl::accelerate");
|
|
return;
|
|
}
|
|
|
|
this->setSpeed(speed + acc);
|
|
} |