diff --git a/.gitignore b/.gitignore index 8b46cae..3d40d6c 100644 --- a/.gitignore +++ b/.gitignore @@ -6,5 +6,4 @@ src/*.ino.cpp .vscode/ src/config/config.h - .DS_Store \ No newline at end of file diff --git a/src/config/pins.h b/src/config/pins.h index c6e89e9..2875fc1 100644 --- a/src/config/pins.h +++ b/src/config/pins.h @@ -4,6 +4,19 @@ #define PIN_LED_INBUILT 2 // --------------------------------------------------------- Motors and Encoders +#define PIN_MOT_R1 26 +#define PIN_MOT_R2 33 +#define PIN_MOT_L1 25 +#define PIN_MOT_L2 27 + +#define PIN_PWM_R 13 +#define PIN_PWM_L 12 + +#define PIN_ENC_RA 32 +#define PIN_ENC_RB 35 + +#define PIN_ENC_LA 34 +#define PIN_ENC_LB 14 // ----------------------------------------------------- Unused or reserved pins #define PIN_TX 1 diff --git a/src/define.h b/src/define.h index 90897d9..b523fad 100644 --- a/src/define.h +++ b/src/define.h @@ -18,3 +18,6 @@ #include "utilities/memory/memory.h" // *** Modules ************************************************** + +// Motors +#include "modules/motors/motors.h" \ No newline at end of file diff --git a/src/modules/motors/motors.cpp b/src/modules/motors/motors.cpp new file mode 100644 index 0000000..0ef095e --- /dev/null +++ b/src/modules/motors/motors.cpp @@ -0,0 +1,4 @@ + +#include "motors.h" + +SW_Motors motors; diff --git a/src/modules/motors/motors.h b/src/modules/motors/motors.h new file mode 100644 index 0000000..fbb242e --- /dev/null +++ b/src/modules/motors/motors.h @@ -0,0 +1,5 @@ +#pragma once + +#include "./src/robot_motors.h" + +extern SW_Motors motors; diff --git a/src/modules/motors/src/robot_encoder.cpp b/src/modules/motors/src/robot_encoder.cpp new file mode 100644 index 0000000..8912fd3 --- /dev/null +++ b/src/modules/motors/src/robot_encoder.cpp @@ -0,0 +1,186 @@ +/** + * @brief Swarm Robot Motor Controller Library + * @author Nuwan Jaliyagoda + * @version 2.0.0 + * @url N/A + * + * ------------------------------------------------------------------------------ + */ + +#include "robot_encoder.h" +#include "config/pins.h" + +RobotEncoder *RobotEncoder::encoders[MAX_ESP32_ENCODERS] = {NULL, NULL, NULL, + NULL, + NULL, NULL, NULL, NULL}; + +bool RobotEncoder::attachedInterrupt = false; +pcnt_isr_handle_t RobotEncoder::user_isr_handle = NULL; + +RobotEncoder::RobotEncoder() +{ + attached = false; + aPinNumber = (gpio_num_t)0; + bPinNumber = (gpio_num_t)0; + working = false; + direction = false; + unit = (pcnt_unit_t)0; // -1 * +} + +RobotEncoder::~RobotEncoder() +{ +} + +/* Decode what PCNT's unit originated an interrupt + * and pass this information together with the event type + * the main program using a queue. + */ +static void IRAM_ATTR pcnt_example_intr_handler(void *arg) +{ + RobotEncoder *ptr; + + uint32_t intr_status = PCNT.int_st.val; + int i; + + for (i = 0; i < PCNT_UNIT_MAX; i++) + { + if (intr_status & (BIT(i))) + { + ptr = RobotEncoder::encoders[i]; + /* Save the PCNT event type that caused an interrupt + to pass it to the main program */ + + int status = 0; + if (PCNT.status_unit[i].h_lim_lat) + { + status = ptr->r_enc_config.counter_h_lim; + } + if (PCNT.status_unit[i].l_lim_lat) + { + status = ptr->r_enc_config.counter_l_lim; + } + // pcnt_counter_clear(ptr->unit); + PCNT.int_clr.val = BIT(i); // clear the interrupt + ptr->count = status + ptr->count; + } + } +} + +void RobotEncoder::attach(int a, int b, boolean fq) +{ + if (attached) + { + Serial.println("All ready attached, FAIL!"); + return; + } + int index = 0; + for (; index < MAX_ESP32_ENCODERS; index++) + { + if (RobotEncoder::encoders[index] == NULL) + { + encoders[index] = this; + break; + } + } + if (index == MAX_ESP32_ENCODERS) + { + Serial.println("Too many encoders, FAIL!"); + return; + } + + // Set data now that pin attach checks are done + fullQuad = fq; + unit = (pcnt_unit_t)index; + this->aPinNumber = (gpio_num_t)a; + this->bPinNumber = (gpio_num_t)b; + + // Set up the IO state of hte pin + gpio_pad_select_gpio(aPinNumber); + gpio_pad_select_gpio(bPinNumber); + gpio_set_direction(aPinNumber, GPIO_MODE_INPUT); + gpio_set_direction(bPinNumber, GPIO_MODE_INPUT); + gpio_pulldown_en(aPinNumber); + gpio_pulldown_en(bPinNumber); + + // Set up encoder PCNT configuration + r_enc_config.pulse_gpio_num = aPinNumber; // Rotary Encoder Chan A + r_enc_config.ctrl_gpio_num = bPinNumber; // Rotary Encoder Chan B + + r_enc_config.unit = unit; + r_enc_config.channel = PCNT_CHANNEL_0; + + r_enc_config.pos_mode = fullQuad ? PCNT_COUNT_DEC : PCNT_COUNT_DIS; // Count Only On Rising-Edges + r_enc_config.neg_mode = PCNT_COUNT_INC; // Discard Falling-Edge + + r_enc_config.lctrl_mode = PCNT_MODE_KEEP; // Rising A on HIGH B = CW Step + r_enc_config.hctrl_mode = PCNT_MODE_REVERSE; // Rising A on LOW B = CCW Step + + r_enc_config.counter_h_lim = INT16_MAX; + r_enc_config.counter_l_lim = INT16_MIN; + + pcnt_unit_config(&r_enc_config); + + // Filter out bounces and noise + pcnt_set_filter_value(unit, 250); // Filter Runt Pulses + pcnt_filter_enable(unit); + + /* Enable events on maximum and minimum limit values */ + pcnt_event_enable(unit, PCNT_EVT_H_LIM); + pcnt_event_enable(unit, PCNT_EVT_L_LIM); + + pcnt_counter_pause(unit); // Initial PCNT init + pcnt_counter_clear(unit); + /* Register ISR handler and enable interrupts for PCNT unit */ + if (RobotEncoder::attachedInterrupt == false) + { + RobotEncoder::attachedInterrupt = true; + esp_err_t er = pcnt_isr_register(pcnt_example_intr_handler, (void *)NULL, (int)0, + (pcnt_isr_handle_t *)&RobotEncoder::user_isr_handle); + if (er != ESP_OK) + { + Serial.println("Encoder wrap interupt failed"); + } + } + pcnt_intr_enable(unit); + pcnt_counter_resume(unit); +} + +void RobotEncoder::attachHalfQuad(int aPintNumber, int bPinNumber) +{ + attach(aPintNumber, bPinNumber, true); +} +void RobotEncoder::attachSingleEdge(int aPintNumber, int bPinNumber) +{ + attach(aPintNumber, bPinNumber, false); +} + +void RobotEncoder::setCount(int32_t value) +{ + count = value - getCountRaw(); +} +int32_t RobotEncoder::getCountRaw() +{ + int16_t c; + pcnt_get_counter_value(unit, &c); + return c; +} +int32_t RobotEncoder::getCount() +{ + return getCountRaw() + count; +} + +int32_t RobotEncoder::clearCount() +{ + count = 0; + return pcnt_counter_clear(unit); +} + +int32_t RobotEncoder::pauseCount() +{ + return pcnt_counter_pause(unit); +} + +int32_t RobotEncoder::resumeCount() +{ + return pcnt_counter_resume(unit); +} \ No newline at end of file diff --git a/src/modules/motors/src/robot_encoder.h b/src/modules/motors/src/robot_encoder.h new file mode 100644 index 0000000..0eafe60 --- /dev/null +++ b/src/modules/motors/src/robot_encoder.h @@ -0,0 +1,51 @@ +/** + * @brief Swarm Robot Motor Controller Library + * @author Nuwan Jaliyagoda + * @version 2.0.0 + * @url N/A + * + * ------------------------------------------------------------------------------ + */ + +#pragma once + +#include +#include +#include "driver/pcnt.h" + +#define MAX_ESP32_ENCODERS PCNT_UNIT_MAX + +class RobotEncoder +{ +private: + void attach(int aPintNumber, int bPinNumber, boolean fullQuad); + boolean attached = false; + + static pcnt_isr_handle_t user_isr_handle; // user's ISR service handle + bool direction; + bool working; + +public: + RobotEncoder(); + ~RobotEncoder(); + void attachHalfQuad(int aPintNumber, int bPinNumber); + void attachSingleEdge(int aPintNumber, int bPinNumber); + + int32_t getCount(); + int32_t getCountRaw(); + int32_t clearCount(); + int32_t pauseCount(); + int32_t resumeCount(); + + boolean isAttached() { return attached; } + void setCount(int32_t value); + static RobotEncoder *encoders[MAX_ESP32_ENCODERS]; + static bool attachedInterrupt; + gpio_num_t aPinNumber; + gpio_num_t bPinNumber; + pcnt_unit_t unit; + bool fullQuad = false; + int countsMode = 2; + volatile int32_t count = 0; + pcnt_config_t r_enc_config; +}; \ No newline at end of file diff --git a/src/modules/motors/src/robot_motors.cpp b/src/modules/motors/src/robot_motors.cpp new file mode 100644 index 0000000..baa8c6f --- /dev/null +++ b/src/modules/motors/src/robot_motors.cpp @@ -0,0 +1,247 @@ +/** + * @brief Swarm Robot Motor Controller Library + * @author Nuwan Jaliyagoda + * @version 2.0.0 + * @url N/A + * + * ------------------------------------------------------------------------------ + */ + +#include "robot_motors.h" +#include "config/pins.h" + +#define LEDC_RESOLUTION_BITS 8 +#define LEDC_BASE_FREQ 5000 +#define LEDC_CHANNEL_A 8 +#define LEDC_CHANNEL_B 9 + +/* + * Motor module + */ +SW_Motors::SW_Motors() +{ + encoderL.setCount(0); + encoderL.attachHalfQuad(PIN_ENC_LA, PIN_ENC_LB); + + encoderR.setCount(0); + encoderR.attachHalfQuad(PIN_ENC_RA, PIN_ENC_RB); +} + +SW_Motors::~SW_Motors() +{ + ledcDetachPin(PIN_PWM_R); + ledcDetachPin(PIN_PWM_L); +} + +/** + * Setup the motor module + */ +void SW_Motors::begin() +{ + // TODO: Try drive the robot using 1 directional pin per motor + pinMode(PIN_MOT_R1, OUTPUT); + pinMode(PIN_MOT_R2, OUTPUT); + pinMode(PIN_MOT_L1, OUTPUT); + pinMode(PIN_MOT_L2, OUTPUT); + + ledcSetup(LEDC_CHANNEL_A, LEDC_BASE_FREQ, LEDC_RESOLUTION_BITS); + ledcSetup(LEDC_CHANNEL_B, LEDC_BASE_FREQ, LEDC_RESOLUTION_BITS); + + ledcAttachPin(PIN_PWM_R, LEDC_CHANNEL_A); + ledcAttachPin(PIN_PWM_L, LEDC_CHANNEL_B); + + ledcWrite(LEDC_CHANNEL_A, 0); + ledcWrite(LEDC_CHANNEL_B, 0); + + Serial.println(">> Motors\t:enabled,pwm"); + + this->write(0, 0); +} + +/** + * Core motor controlling function + * @param leftSpeed speed in range [-MAX_MOTOR_SPEED, MAX_MOTOR_SPEED] + * @param rightSpeed speed in range [-MAX_MOTOR_SPEED, MAX_MOTOR_SPEED] + */ +void SW_Motors::write(int16_t leftSpeed, int16_t rightSpeed) +{ + // Adjustment to remove the drift + // TODO implement a linear correction + if (leftSpeed > 30) + leftSpeed += leftCorrection; + else if (leftSpeed < -30) + leftSpeed -= leftCorrection; + + if (rightSpeed > 30) + rightSpeed += rightCorrection; + else if (rightSpeed < -30) + rightSpeed -= rightCorrection; + + // map the speed with correct & acceptable range + leftSpeed = constrain(leftSpeed, -1 * MAX_MOTOR_SPEED, MAX_MOTOR_SPEED); + rightSpeed = constrain(rightSpeed, -1 * MAX_MOTOR_SPEED, MAX_MOTOR_SPEED); + + // Serial.printf("M: %d %d\n", leftSpeed, rightSpeed); + + // motor rotating directions + this->leftMotorDir = (leftSpeed >= 0) ? 1 : 0; + this->rightMotorDir = (rightSpeed >= 0) ? 1 : 0; + + // check motor directions + if (this->leftMotorDir != this->leftMotorDirOld) + { + // Direction changed + digitalWrite(PIN_MOT_L1, (this->leftMotorDir) ? LOW : HIGH); + digitalWrite(PIN_MOT_L2, (this->leftMotorDir) ? HIGH : LOW); + this->leftMotorDirOld = this->leftMotorDir; + } + if (this->rightMotorDir != this->rightMotorDirOld) + { + // Direction changed + digitalWrite(PIN_MOT_R1, (this->rightMotorDir) ? LOW : HIGH); + digitalWrite(PIN_MOT_R2, (this->rightMotorDir) ? HIGH : LOW); + this->rightMotorDirOld = this->rightMotorDir; + } + + this->rightMotorSpeed = abs(rightSpeed); + this->leftMotorSpeed = abs(leftSpeed); + + // Analog write function for ESP32 + ledcWrite(LEDC_CHANNEL_A, this->leftMotorSpeed); + ledcWrite(LEDC_CHANNEL_B, this->rightMotorSpeed); +} + +/** + * Stop motors immediately + */ +void SW_Motors::stop() +{ + this->write(0, 0); +} + +/** + * Stop motors after a delay + * @param delay delay in milliseconds + */ +void SW_Motors::stop(int16_t d) +{ + delay(d); + this->write(0, 0); +} + +/** + * Pause motors for while + */ +void SW_Motors::pause() +{ + ledcWrite(LEDC_CHANNEL_A, 0); + ledcWrite(LEDC_CHANNEL_B, 0); +} + +/** + * Resume motors + */ +void SW_Motors::resume() +{ + ledcWrite(LEDC_CHANNEL_A, this->leftMotorSpeed); + ledcWrite(LEDC_CHANNEL_B, this->rightMotorSpeed); +} + +/** + * A test function to test motors + */ +void SW_Motors::test() +{ + // Clockwise rotation + Serial.println(F("robot: CCW")); + this->write(-200, 200); + delay(500); + this->stop(1500); + + // Counter Clockwise rotation + Serial.println(F("robot: CW")); + this->write(200, -200); + delay(500); + this->stop(1500); + + // Forward movement + Serial.println(F("robot: forward++")); + for (int i = 0; i < 255; i++) + { + this->write(i, i); + delay(25); + } + delay(500); + + Serial.println(F("robot: forward--")); + for (int i = 255; i > 0; i--) + { + this->write(i, i); + delay(25); + } + this->stop(500); + delay(2000); + + // Backward movement + Serial.println(F("robot: backward++")); + for (int i = 0; i < 255; i++) + { + this->write(-i, -i); + delay(25); + } + delay(500); + + Serial.println(F("robot: backward--")); + for (int i = 255; i > 0; i--) + { + this->write(-i, -i); + delay(25); + } + this->stop(500); + delay(2000); +} + +/** + * Print the encoder readings + */ +void SW_Motors::encoder_print() +{ + int32_t countL = encoderL.getCount(); + int32_t countR = encoderR.getCount(); + Serial.printf("Encoder L: %d, R: %d\n", countL, countR); + delay(100); +} + +/** + * Reset the encoder readings to 0 + */ +void SW_Motors::encoder_reset() +{ + encoderL.clearCount(); + encoderR.clearCount(); +} + +/** + * Read encoder values and return as a pointers + * @param *left Pointer variable to receive left encoder reading + * @param *right Pointer variable to receive right encoder reading + */ +void SW_Motors::encoder_read(int32_t *left, int32_t *right) +{ + *left = encoderL.getCount(); + *right = encoderR.getCount(); +} + +/** + * Read encoder values and return as a Struct + * + * @return EncoderReadings + */ +EncoderReadings SW_Motors::encoder_read() +{ + struct EncoderReadings readings; + readings.left = encoderL.getCount(); + readings.right = encoderR.getCount(); + + return readings; +} \ No newline at end of file diff --git a/src/modules/motors/src/robot_motors.h b/src/modules/motors/src/robot_motors.h new file mode 100644 index 0000000..5d82e77 --- /dev/null +++ b/src/modules/motors/src/robot_motors.h @@ -0,0 +1,62 @@ +/** + * @brief Swarm Robot Motor Controller Library + * @author Nuwan Jaliyagoda, Pasan Tennakoon, Dilshani Karunarathna + * @version 2.0.0 + * @url N/A + * + * ------------------------------------------------------------------------------ + */ + +#ifndef SW_MOTORS_H +#define SW_MOTORS_H + +#include + +#include "config/definitions.h" +#include "robot_encoder.h" + +#define MAX_MOTOR_SPEED 255 +#define MIN_MOTOR_SPEED 30 + +struct EncoderReadings +{ + int left; + int right; +}; +class SW_Motors +{ +private: + uint8_t RIGHT_DEFAULT = 90; + uint8_t LEFT_DEFAULT = 90; + + boolean leftMotorDir = 1, rightMotorDir = 1; + boolean leftMotorDirOld = 0, rightMotorDirOld = 0; + int16_t rightMotorSpeed = 0, leftMotorSpeed = 0; + + RobotEncoder encoderL; + RobotEncoder encoderR; + +public: + SW_Motors(); + ~SW_Motors(); + + int8_t leftCorrection = 0; + int8_t rightCorrection = 0; + + void begin(); + void write(int16_t left, int16_t right); + void stop(); + void stop(int16_t delay); + + void pause(); + void resume(); + + void test(); + + void encoder_print(); + void encoder_reset(); + void encoder_read(int32_t *left, int32_t *right); + EncoderReadings encoder_read(); +}; + +#endif diff --git a/src/src.cpp b/src/src.cpp index e891551..6f49ff8 100644 --- a/src/src.cpp +++ b/src/src.cpp @@ -27,16 +27,17 @@ void setup() // Scan available I2C devices i2c_scan(); + + motors.begin(); } void loop() { // put your main code here, to run repeatedly: - digitalWrite(PIN_LED_INBUILT, HIGH); - delay(1000); - digitalWrite(PIN_LED_INBUILT, LOW); + motors.write(100, 100); delay(1000); + motors.encoder_print(); } #endif \ No newline at end of file