kleiax/Bachelorarbeit-Rover
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases 1 Wiki Activity

Added move Controll

Browse Source
This commit is contained in:
kleiax
2021-03-11 13:20:19 +01:00
parent 96c1ef6699
commit 887b71ae5e
6 changed files with 128 additions and 134 deletions
Download Patch File Download Diff File Expand all files Collapse all files
platformio.ini
+3
View File
@@ -13,4 +13,7 @@ platform = espressif32
board = esp32doit-devkit-v1
framework = arduino
monitor_speed = 115200
monitor_port = COM12
lib_deps = stempedia/DabbleESP32@^1.5.1
upload_port = COM12
src/main.cpp
+9 -126
View File
@@ -1,153 +1,36 @@
#include <Arduino.h>
#include "config.h"
#include "motorControl.h"
#define CUSTOM_SETTINGS
#define INCLUDE_GAMEPAD_MODULE
#include <DabbleESP32.h>
// Functions
void setMotorDirection(uint8_t motor ,uint8_t direction = FORWARD);
void setMotorSpeed(uint8_t motor, uint8_t speed);
MotorControl left_motor;
void setup() {
Serial.begin(115200); // make sure your Serial Monitor is also set at this baud rate.
Dabble.begin("Kleiax-Rover"); //set bluetooth name of your device
//SETUP PINS
pinMode(M_DIR_11, OUTPUT);
pinMode(M_DIR_12, OUTPUT);
pinMode(M_DIR_21, OUTPUT);
pinMode(M_DIR_22, OUTPUT);
digitalWrite(M_DIR_11, LOW);
digitalWrite(M_DIR_12, LOW);
digitalWrite(M_DIR_21, LOW);
digitalWrite(M_DIR_22, LOW);
ledcSetup(PWM_CHANNEL_M1, PWM_FREQ, PWM_RES);
ledcSetup(PWM_CHANNEL_M2, PWM_FREQ, PWM_RES);
ledcAttachPin(M_PWM_1, PWM_CHANNEL_M1);
ledcAttachPin(M_PWM_2, PWM_CHANNEL_M2);
ledcWrite(M_PWM_1, 0);
ledcWrite(M_PWM_2, 0);
//Start speed
setMotorSpeed(BOTH_MOTOR, 255);
left_motor.init(M_PWM_1, PWM_CHANNEL_M1, M_DIR_11, M_DIR_12);
}
void loop() {
delay(500);
left_motor.runMotorControl();
left_motor.toString();
Dabble.processInput(); //this function is used to refresh data obtained from smartphone.Hence calling this function is mandatory in order to get data properly from your mobile.
//Serial.print("KeyPressed: ");
if (GamePad.isUpPressed()) {
Serial.print("Up\n");
setMotorDirection(BOTH_MOTOR, FORWARD);
left_motor.setTargetSpeed(100);
} else if (GamePad.isDownPressed()) {
Serial.print("Down\n");
setMotorDirection(BOTH_MOTOR, BACKWARD);
} else if (GamePad.isLeftPressed()) {
Serial.print("Left\n");
setMotorDirection(LEFT_MOTOR, BACKWARD);
setMotorDirection(RIGHT_MOTOR, FORWARD);
} else if (GamePad.isRightPressed()) {
Serial.print("Right\n");
setMotorDirection(LEFT_MOTOR, FORWARD);
setMotorDirection(RIGHT_MOTOR, BACKWARD);
} else if (GamePad.isSquarePressed()) {
setMotorDirection(LEFT_MOTOR, FORWARD);
Serial.print("1\n");
} else if (GamePad.isTrianglePressed()) {
setMotorDirection(LEFT_MOTOR, BACKWARD);
Serial.print("2\n");
left_motor.setTargetSpeed(-100);
} else if (GamePad.isCirclePressed()) {
setMotorDirection(RIGHT_MOTOR, FORWARD);
Serial.print("3\n");
} else if (GamePad.isCrossPressed()) {
setMotorDirection(RIGHT_MOTOR, BACKWARD);
Serial.print("4\n");
} else {
setMotorDirection(BOTH_MOTOR, STOP);
left_motor.setTargetSpeed(0);
}
}
void setMotorDirection(uint8_t motor ,uint8_t direction) {
if (motor == LEFT_MOTOR) {
if (direction == FORWARD) {
digitalWrite(M_DIR_11, HIGH);
digitalWrite(M_DIR_12, LOW);
} else if (direction == BACKWARD) {
digitalWrite(M_DIR_11, LOW);
digitalWrite(M_DIR_12, HIGH);
} else if (direction == STOP) {
digitalWrite(M_DIR_11, LOW);
digitalWrite(M_DIR_12, LOW);
} else {
digitalWrite(M_DIR_21, LOW);
digitalWrite(M_DIR_22, LOW);
Serial.print("setDirection: undefined direction\n");
}
} else if (motor == RIGHT_MOTOR) {
if (direction == FORWARD) {
digitalWrite(M_DIR_21, HIGH);
digitalWrite(M_DIR_22, LOW);
} else if (direction == BACKWARD) {
digitalWrite(M_DIR_21, LOW);
digitalWrite(M_DIR_22, HIGH);
} else if (direction == STOP) {
digitalWrite(M_DIR_21, LOW);
digitalWrite(M_DIR_22, LOW);
} else {
digitalWrite(M_DIR_21, LOW);
digitalWrite(M_DIR_22, LOW);
Serial.print("setDirection: undefined direction\n");
}
} else if (motor == BOTH_MOTOR) {
if (direction == FORWARD) {
digitalWrite(M_DIR_11, HIGH);
digitalWrite(M_DIR_12, LOW);
digitalWrite(M_DIR_21, HIGH);
digitalWrite(M_DIR_22, LOW);
} else if (direction == BACKWARD) {
digitalWrite(M_DIR_11, LOW);
digitalWrite(M_DIR_12, HIGH);
digitalWrite(M_DIR_21, LOW);
digitalWrite(M_DIR_22, HIGH);
} else if (direction == STOP) {
digitalWrite(M_DIR_11, LOW);
digitalWrite(M_DIR_12, LOW);
digitalWrite(M_DIR_21, LOW);
digitalWrite(M_DIR_22, LOW);
}else {
digitalWrite(M_DIR_11, LOW);
digitalWrite(M_DIR_12, LOW);
digitalWrite(M_DIR_21, LOW);
digitalWrite(M_DIR_22, LOW);
Serial.print("setDirection: undefined direction\n");
}
} else {
Serial.print("setDirection: undefined motor\n");
}
}
void setMotorSpeed(uint8_t motor, uint8_t speed) {
if (motor == LEFT_MOTOR) {
ledcWrite(PWM_CHANNEL_M1, speed);
} else if (motor == RIGHT_MOTOR) {
ledcWrite(PWM_CHANNEL_M2, speed);
} else if (motor == BOTH_MOTOR) {
ledcWrite(PWM_CHANNEL_M1, speed);
ledcWrite(PWM_CHANNEL_M2, speed);
} else {
ledcWrite(PWM_CHANNEL_M1, 0);
ledcWrite(PWM_CHANNEL_M2, 0);
Serial.print("setSpeed: undefined motor \n");
}
}
src/mototControl.cpp → src/motorControl.cpp
+27 -7
View File
@@ -4,7 +4,11 @@
#include "config.h"
MotorControl::MotorControl(uint8_t pwm_pin, uint8_t pwm_channel, uint8_t direction_pin_1, uint8_t direction_pin_2) {
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;
@@ -74,7 +78,7 @@ void MotorControl::runMotorControl() {
this->last_millis = millis();
}
void MotorControl::setTargetSpeed(uint8_t speed) {
void MotorControl::setTargetSpeed(int16_t speed) {
//TODO: Exceptionhandling
if (speed <= 100 || speed <= -100) {
this->target_speed = speed;
@@ -91,12 +95,28 @@ 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->target_speed = speed;
this->speed = speed;
} else {
Serial.println("Invalid Argument in MotorControl::setSpeed");
return;
}
if (speed == 0) {
@@ -104,28 +124,28 @@ void MotorControl::setSpeed(int16_t speed) {
digitalWrite(this->direction_pin_1, LOW);
digitalWrite(this->direction_pin_2, LOW);
ledcWrite(this->pwm_channel, 0);
this->speed = speed;
return;
}
uint8_t pwm_val = map(abs(speed), 0, 100, PWM_MIN, PWM_MAX);
if (this->direction == 1 && speed < 0){ // new direction backward
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 && speed > 0){ // new direction forward
} 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);
this->speed = speed;
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;
src/motorControl.h
+7 -1
View File
@@ -2,14 +2,20 @@
#define MOTOR_CONTROL_H
#include <cstdint>
#include <string>
class MotorControl {
public:
MotorControl(uint8_t pwm_pin, uint8_t pwm_channel, uint8_t direction_1, uint8_t direction_2);
MotorControl();
void init(uint8_t pwm_pin, uint8_t pwm_channel, uint8_t direction_1, uint8_t direction_2);
void runMotorControl();
void setTargetSpeed(int16_t speed);
void stop();
void emergencyStop();
void toString();
int16_t getSpeed();
bool isTargetSpeedReached();
private:
void setSpeed(int16_t speed);
src/moveControl.cpp
+57
View File
@@ -0,0 +1,57 @@
#include "moveControl.h"
#include <Arduino.h>
MoveControl::MoveControl() {
}
void MoveControl::init(MotorControl *left_motor, MotorControl *right_motor) {
this->left_motor = left_motor;
this->right_motor = right_motor;
}
void MoveControl::rotate(int16_t degree) {
this->left_motor->setTargetSpeed(0);
this->right_motor->setTargetSpeed(0);
while ( !(left_motor->isTargetSpeedReached() && right_motor->isTargetSpeedReached()) ) {
delay(5);
}
uint8_t left = 0;
uint8_t right = 0;
if (degree < 0) {
left = -this->speed;
right = this->speed;
} else if (degree > 0) {
left = this->speed;
right = -this->speed;
}
this->left_motor->setTargetSpeed(this->speed);
this->right_motor->setTargetSpeed(this->speed);
delay(abs(degree));
this->left_motor->setTargetSpeed(0);
this->right_motor->setTargetSpeed(0);
}
void MoveControl::rotate(int16_t degree, uint8_t radius) {
}
void MoveControl::forward() {
this->left_motor->setTargetSpeed(this->speed);
this->right_motor->setTargetSpeed(this->speed);
}
void MoveControl::backward() {
this->left_motor->setTargetSpeed(-this->speed);
this->right_motor->setTargetSpeed(-this->speed);
}
void MoveControl::setSpeed(uint8_t speed) {
this->speed = speed;
}
src/moveControl.h
+25
View File
@@ -0,0 +1,25 @@
#ifndef MOVE_CONTROL_H
#define MOVE_CONTROL_H
#include <cstdint>
#include "motorControl.h"
class MoveControl {
public:
MoveControl();
void init(MotorControl *left_motor, MotorControl *right_motor);
void rotate(int16_t degree);
void rotate(int16_t degree, uint8_t radius);
void forward();
void backward();
void setSpeed(uint8_t speed);
private:
MotorControl *left_motor;
MotorControl *right_motor;
uint8_t speed = 0;
};
#endif // MOVE_CONTROL_H