Add files for Arduino control of agent

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 <PS2X_lib.h>
+
+// 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());
+}

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;
+}

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

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;
+}

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