Added PID Balancing

Author: JoeF140

MATE_2019/Arduino/mainBoard/mainBoard.ino

@@ -6,28 +6,13 @@
 #include <SPI.h>
 #include <SparkFunLSM9DS1.h>
 
-#include <Servo.h>
-
 #define LSM9DS1_M 0x1E   // Would be 0x1C if SDO_M is LOW
 #define LSM9DS1_AG 0x6B  // Would be 0x6A if SDO_AG is LOW
 
 #define SerialConnection Serial
 
 LSM9DS1 imu;
 
-Servo FR;
-Servo BR;
-Servo BL;
-Servo FL;
-
-Servo UL;
-Servo UR;
-Servo UB;
-
-Servo shoulderTilt;
-Servo wristTilt;
-Servo wristTwist;
-
 const int buzzer = 17;
 const int water1 = 20;
 const int water2 = 21;
@@ -54,19 +39,6 @@ void setup()
   imu.settings.device.mAddress = LSM9DS1_M;
   imu.settings.device.agAddress = LSM9DS1_AG;
 
-  FR.attach(27);
-  BR.attach(14);
-  BL.attach(15);
-  FL.attach(16);
-
-  UL.attach(25);
-  UR.attach(24);
-  UB.attach(26);
-
-//  shoulderTilt.attach(9);
-//  wristTilt.attach(10);
-//  wristTwist.attach(11);
-
   pinMode(buzzer, OUTPUT);
   pinMode(water4, INPUT);
 
@@ -75,7 +47,7 @@ void setup()
 
   while (!imu.begin())
   {
-    delay(500);
+    delay(100);
   }
 
   digitalWrite(buzzer, 0);
@@ -171,7 +143,18 @@ void loop()
       info.toCharArray(driveCommands, COMMAND_SIZE - 1);
       drive(driveCommands);
 
-      writeString(info);
+      writeString(":");
+      if (roll < 10.0 && roll > 0.0)
+      {
+        writeString("0");
+      }
+      writeString(String(roll, 3));
+      writeString(";");
+      if (pitch < 10.0 && pitch > 0.0)
+      {
+        writeString("0");
+      }
+      writeString(String(pitch, 3));
 
       digitalWrite(buzzer, 1);
 
@@ -184,12 +167,12 @@ void loop()
     }
   }
 
-  // Only run if a command has been received within 25 ticks
-  if (timer > 25)
+  // Only run if a command has been received within 100 ticks
+  /*if (timer > 100)
   {
     disabledCommand.toCharArray(driveCommands, COMMAND_SIZE - 1); 
     drive(driveCommands);
-  }
+  }*/
   //Rough timer counting
   delay(1);
   digitalWrite(buzzer, 0);
@@ -247,4 +230,5 @@ void drive(char array[])
   Wire.write(commands[7]);
   Wire.write(':');
   Wire.endTransmission();
+  
 }

MATE_2019/Main/Headers/Utils.h

@@ -6,6 +6,8 @@
 class Utils
 {
  public:
+  static std::string charToString(const char* val, int start, int stop);
+
   // Converts a number from range a-b to range c-d
   static double convertRange(double oldMin, double oldMax, double newMin,
                              double newMax, double oldValue);

MATE_2019/Main/Sources/Utils.cpp

@@ -1,5 +1,15 @@
 #include "..\Headers\Utils.h"
 
+std::string Utils::charToString(const char* val, int start, int stop)
+{
+  std::string s = "";
+  for (int i = start; i <= stop; ++i)
+  {
+    s += val[i];
+  }
+  return s;
+}
+
 double Utils::convertRange(double oldMin, double oldMax, double newMin,
                            double newMax, double oldValue)
 {

MATE_2019/Main/main.cpp

@@ -15,9 +15,8 @@ bool waterLeak = false;
 char output[MAX_DATA_LENGTH];
 string imu;
 string prevIMU;
-int pitch = 0;  // Heading up and down
-int roll = 0;   // Angle side to side relative to ground
-
+float pitch = 0;  // Angle forward to back
+float roll = 0;   // Angle side to side
 double pitchSetpoint = 0.0;
 double rollSetpoint = 0.0;
 
@@ -45,6 +44,10 @@ void transferData(string data)
 
   bool checked = false;
   int i = 0;
+
+  pitch = atof(Utils::charToString(output, 1, 6).c_str());
+  roll = atof(Utils::charToString(output, 8, 13).c_str());
+
   for (char c : output)
   {
     cout << c;
@@ -90,15 +93,15 @@ void teleop(double FWD, double STR, double RCW)
   // : is verification character for arduino
   string data = ":";
 
-  // PID pitchPID(0.007, 0.0, 0.0);
-  // pitchPID.setContinuous(false);
-  // pitchPID.setOutputLimits(-1.0, 1.0);
-  // pitchPID.setSetpoint(pitchSetpoint);
+  PID pitchPID(0.07, 0.0, 0.0);
+  pitchPID.setContinuous(false);
+  pitchPID.setOutputLimits(-1.0, 1.0);
+  pitchPID.setSetpoint(pitchSetpoint);
 
-  // PID rollPID(0.007, 0.0, 0.0);
-  // rollPID.setContinuous(false);
-  // rollPID.setOutputLimits(-1.0, 1.0);
-  // rollPID.setSetpoint(rollSetpoint);
+  PID rollPID(0.07, 0.0, 0.0);
+  rollPID.setContinuous(false);
+  rollPID.setOutputLimits(-1.0, 1.0);
+  rollPID.setSetpoint(rollSetpoint);
 
   // Let driver adjust angle of robot if necessary
   if (gamepad1.getButtonPressed(xButtons.A))
@@ -126,17 +129,17 @@ void teleop(double FWD, double STR, double RCW)
   // heading adjusts where front is
   double heading = -rad45;
 
-  // FR AND FL ARE SWAPPED
-  // BR IS INVERTED?
-  // WORKING VALUES 2/13/2020 W/ EXTENSION CORDS
   double FR = (-STR * sin(heading) - FWD * sin(heading) + RCW);  // A
   double BR = (STR * cos(heading) + FWD * sin(heading) - RCW);   // B
   double BL = (-STR * sin(heading) + FWD * cos(heading) - RCW);  // C
   double FL = (-STR * cos(heading) + FWD * cos(heading) - RCW);  // D
 
-  double UL = -(gamepad1.rightTrigger() - gamepad1.leftTrigger());
-  double UR = -(gamepad1.rightTrigger() - gamepad1.leftTrigger());
-  double UB = -(gamepad1.rightTrigger() - gamepad1.leftTrigger()) * 0.4;
+  double UL = -(gamepad1.rightTrigger() - gamepad1.leftTrigger()) +
+              pitchPID.getOutput(pitch) + rollPID.getOutput(roll);
+  double UR = -(gamepad1.rightTrigger() - gamepad1.leftTrigger()) +
+              pitchPID.getOutput(pitch) - rollPID.getOutput(roll);
+  double UB = 0.4 * (-(gamepad1.rightTrigger() - gamepad1.leftTrigger()) -
+                     pitchPID.getOutput(pitch));
 
   double* vals[] = {&FR, &BR, &BL, &FL, &UL, &UR, &UB};