From f01e3d3ed95da8d21816b7732e348a557ea43fe4 Mon Sep 17 00:00:00 2001 From: nichols-cam <55548027+nichols-cam@users.noreply.github.com> Date: Wed, 12 Apr 2023 22:52:29 -0400 Subject: [PATCH] Add files for Arduino control of agent Currently only controllable with a PS2 controller --- .../AgentControl/AgentControl.ino | 117 ++++++++++++++++++ .../AgentControl/Mecanum.cpp | 68 ++++++++++ .../AgentControl/Mecanum.h | 20 +++ .../AgentControl/Motor.cpp | 53 ++++++++ Arduino Control of Agent/AgentControl/Motor.h | 24 ++++ 5 files changed, 282 insertions(+) create mode 100644 Arduino Control of Agent/AgentControl/AgentControl.ino create mode 100644 Arduino Control of Agent/AgentControl/Mecanum.cpp create mode 100644 Arduino Control of Agent/AgentControl/Mecanum.h create mode 100644 Arduino Control of Agent/AgentControl/Motor.cpp create mode 100644 Arduino Control of Agent/AgentControl/Motor.h diff --git a/Arduino Control of Agent/AgentControl/AgentControl.ino b/Arduino Control of Agent/AgentControl/AgentControl.ino new file mode 100644 index 0000000..458a548 --- /dev/null +++ b/Arduino Control of Agent/AgentControl/AgentControl.ino @@ -0,0 +1,117 @@ +#include "Mecanum.h" + +// PS2 Controller Arduino Library: https://github.com/madsci1016/Arduino-PS2X/blob/master/PS2X_lib/PS2X_lib.cpp +#include + +// Define pins for motor controllers +#define m1_pwm 4 +#define m1_gpio 30 +#define m2_pwm 7 +#define m2_gpio 34 +#define m3_pwm 2 +#define m3_gpio 22 +#define m4_pwm 3 +#define m4_gpio 26 + +// Define pins for PS2 controller breakout board +#define ps2_data 10 +#define ps2_cmd 11 +#define ps2_att 12 +#define ps2_clk 13 + +PS2X ps2x; +int error = 1; + +// Motor(int speedPin, int directionPin, [bool inverse = false]) +Motor m1 = Motor(m1_pwm, m1_gpio); +Motor m2 = Motor(m2_pwm, m2_gpio, true); +Motor m3 = Motor(m3_pwm, m3_gpio); +Motor m4 = Motor(m4_pwm, m4_gpio, true); + +// Mecanum(Motor &m1, Motor &m2, Motor &m3, Motor &m4) +Mecanum drivetrain = Mecanum(m1, m2, m3, m4); + +void setup() { + Serial.begin(9600); + // Attempts to connect to PS2 controller a maximum of 50 times + for (int i = 0; i < 50 && error; i++) { + // config_gamepad(clock, command, attention, data, pressures?, rumble?) + error = ps2x.config_gamepad(ps2_clk, ps2_cmd, ps2_att, ps2_data, false, false); + delay(500); + } +} + +void loop() { + if (error == 1) { + Serial.println("Error, no controller"); + return; // Skip if no controller + } + ps2x.read_gamepad(); // Must be called frequently to read updated values + float lx = 2 * (ps2x.Analog(PSS_LX) / 255.0) - 1; // Left stick x-axis + float ly = -(2 * (ps2x.Analog(PSS_LY) / 255.0) - 1); // Left stick y-axis + float rx = 2 * (ps2x.Analog(PSS_RX) / 255.0) - 1; // Right stick x-axis + // Above values are adjusted to be between -1.0 and 1.0 + + // Use of the D-Pad on the PS2 controller will override the left stick. + // This is to test with digital power (max on or off), while the stick allows for analog power. + float pad_x = 0.0; + float pad_y = 0.0; + if (ps2x.Button(PSB_PAD_UP) || ps2x.Button(PSB_PAD_DOWN)) { + pad_y = ps2x.Button(PSB_PAD_UP) ? 1.0 : -1.0; + } else if (ps2x.Button(PSB_PAD_RIGHT) || ps2x.Button(PSB_PAD_LEFT)) { + pad_x = ps2x.Button(PSB_PAD_RIGHT) ? 1.0 : -1.0; + } + + // Controls the drivetrain based on either the D-Pad or the left stick. + // Right stick still works for turning in either case. + // drivetrain.drive(float x, float y, float z) + if (pad_x || pad_y) { + drivetrain.drive(pad_y, pad_x, rx); // x and y intentionally swapped + } else { + drivetrain.drive(ly, lx, rx); // x and y intentionally swapped + } + + // For debugging purposes via the Serial Monitor: +// printCommandVelocities(); +// printRawSpeeds(); +// printObservedSpeeds(); + + delay(50); +} + +// Displays command velocities that are sent to the drivetrain. +// Values should be between -1.0 and 1.0. +void printCommandVelocities() { + Serial.print("XVel: "); + Serial.print(drivetrain.getXVel()); + Serial.print("\tYVel: "); + Serial.print(drivetrain.getYVel()); + Serial.print("\tZVel: "); + Serial.println(drivetrain.getZVel()); +} + +// Displays the raw PWM data sent to each motor. +// Values will be between 0 and 255, where either 0 or 255 can be max speed. +void printRawSpeeds() { + Serial.print("M1R: "); + Serial.print(drivetrain.m1.getRawSpeed()); + Serial.print("\tM2R: "); + Serial.print(drivetrain.m2.getRawSpeed()); + Serial.print("\tM3R: "); + Serial.print(drivetrain.m3.getRawSpeed()); + Serial.print("\tM4R: "); + Serial.println(drivetrain.m4.getRawSpeed()); +} + +// Displays the desired speeds of each motor relative to the robot. +// 255 is full speed forward, -255 is full speed reverse, and 0 is not moving. +void printObservedSpeeds() { + Serial.print("M1O: "); + Serial.print(drivetrain.m1.getObservedSpeed()); + Serial.print("\tM2O: "); + Serial.print(drivetrain.m2.getObservedSpeed()); + Serial.print("\tM3O: "); + Serial.print(drivetrain.m3.getObservedSpeed()); + Serial.print("\tM4O: "); + Serial.println(drivetrain.m4.getObservedSpeed()); +} diff --git a/Arduino Control of Agent/AgentControl/Mecanum.cpp b/Arduino Control of Agent/AgentControl/Mecanum.cpp new file mode 100644 index 0000000..0c64a7b --- /dev/null +++ b/Arduino Control of Agent/AgentControl/Mecanum.cpp @@ -0,0 +1,68 @@ +#include "Mecanum.h" + +// Defines the drivetrain by all four motors +Mecanum::Mecanum(Motor &m1, Motor &m2, Motor &m3, Motor &m4): m1(m1), m2(m2), m3(m3), m4(m4) { + this->m1 = m1; // Front left motor + this->m2 = m2; // Front right motor + this->m3 = m3; // Back left motor + this->m4 = m4; // Back right motor +} + +void Mecanum::init() { + // x, y, and z should be between -1.0 and 1.0 + this->x = 0; // Forward/reverse + this->y = 0; // Strafing + this->z = 0; // Turning +} + +// Controls each motor according to the command velocity inputs. +// Inputs should be between -1.0 and 1.0. +void Mecanum::drive(float x, float y, float z) { + this->x = x; + this->y = y; + this->z = z; + + // Calculates desired velocity of each motor based on all inputs + float m1v = x + y + z; + float m2v = x - y - z; + float m3v = x - y + z; + float m4v = x + y - z; + + scaleMotors(&m1v, &m2v, &m3v, &m4v); + + // Sends speed commands to each motor. + // Input should be between -255 and 255. + this->m1.drive(m1v); + this->m2.drive(m2v); + this->m3.drive(m3v); + this->m4.drive(m4v); +} + +// Takes calculated velocities of each motor and scales so that all are between -1.0 and 1.0. +// Then multiply by 255 for PWM use later. +void Mecanum::scaleMotors(float* m1v, float* m2v, float* m3v, float* m4v) { + float max = 1; + if (abs(*m1v) > max) max = abs(*m1v); + if (abs(*m2v) > max) max = abs(*m2v); + if (abs(*m3v) > max) max = abs(*m3v); + if (abs(*m4v) > max) max = abs(*m4v); + *m1v *= 255.0 / max; + *m2v *= 255.0 / max; + *m3v *= 255.0 / max; + *m4v *= 255.0 / max; +} + +// Returns x velocity that was sent to the motor +float Mecanum::getXVel() { + return this->x; +} + +// Returns y velocity that was sent to the motor +float Mecanum::getYVel() { + return this->y; +} + +// Returns z velocity that was sent to the motor +float Mecanum::getZVel() { + return this->z; +} diff --git a/Arduino Control of Agent/AgentControl/Mecanum.h b/Arduino Control of Agent/AgentControl/Mecanum.h new file mode 100644 index 0000000..3df3835 --- /dev/null +++ b/Arduino Control of Agent/AgentControl/Mecanum.h @@ -0,0 +1,20 @@ +#ifndef SD1_MECANUM_H // this is so that the header file isn't included more than once +#define SD1_MECANUM_H + +#include "Motor.h" + +class Mecanum { + private: + float x, y, z; + void init(); + void scaleMotors (float* m1v, float* m2v, float* m3v, float* m4v); + public: + Motor m1, m2, m3, m4; + Mecanum(Motor &m1, Motor &m2, Motor &m3, Motor &m4); + void drive(float x, float y, float z); + float getXVel(); + float getYVel(); + float getZVel(); +}; + +#endif diff --git a/Arduino Control of Agent/AgentControl/Motor.cpp b/Arduino Control of Agent/AgentControl/Motor.cpp new file mode 100644 index 0000000..bba4985 --- /dev/null +++ b/Arduino Control of Agent/AgentControl/Motor.cpp @@ -0,0 +1,53 @@ +#include "Motor.h" + +// Defines pins for the motor and if the control should be inverted (defaults to false). +// Generally, motors that face the same direction should have the same inverse value. +Motor::Motor(int speedPin, int directionPin, bool inverse) { + this->speedPin = speedPin; + this->directionPin = directionPin; + this->inverse = inverse; + this->init(); +} + +void Motor::init() { + pinMode(this->speedPin, OUTPUT); + pinMode(this->directionPin, OUTPUT); + this->speed = 0; + this->direction = false; +} + +// Controls the speed and direction of the motor based on the input. +// Input should be between -255 and 255. +void Motor::drive(int speed) { + this->observedSpeed = speed; + + // Sets direction based off the sign of the input. + // Changes direction and speed according to motor inversion status. + if (!this->inverse) { + this->direction = speed <= 0; + this->speed = constrain(speed > 0 ? speed : 255 + speed, 0, 255); + } else { + this->direction = speed > 0; + this->speed = constrain(speed > 0 ? 255 - speed : -speed, 0, 255); + } + + digitalWrite(this->directionPin, this->direction); + analogWrite(this->speedPin, this->speed); +} + +// Returns a value from -255 to 255, indicating speed and direction relative to robot, accounting for inversion. +// 255 is full speed forward, -255 is full speed reverse, and 0 is not moving. +int Motor::getObservedSpeed() { + return observedSpeed; +} + +// Returns the raw PWM data sent to the motor. +// Should be between 0 and 255. +int Motor::getRawSpeed() { + return this->speed; +} + +// Returns a boolean indicating the value sent to the direction pin of the motor. +bool Motor::getDirection() { + return this->direction; +} diff --git a/Arduino Control of Agent/AgentControl/Motor.h b/Arduino Control of Agent/AgentControl/Motor.h new file mode 100644 index 0000000..4699ad7 --- /dev/null +++ b/Arduino Control of Agent/AgentControl/Motor.h @@ -0,0 +1,24 @@ +#ifndef SD1_MOTOR_H // this is so that the header file isn't included more than once +#define SD1_MOTOR_H + +#include "Arduino.h" + +class Motor { + private: + int speedPin; + int directionPin; + bool inverse; + int speed; + int observedSpeed; + bool direction; + void init(); + + public: + Motor(int speedPin, int directionPin, bool inverse = false); + void drive(int speed); + int getObservedSpeed(); + int getRawSpeed(); + bool getDirection(); +}; + +#endif