diff --git a/.gitignore b/.gitignore index 9d990ee..5957b40 100644 --- a/.gitignore +++ b/.gitignore @@ -331,3 +331,5 @@ ASALocalRun/ *.sln *.filters *.vcxproj +MATE_2019/Main/enc_temp_folder/ +MATE_2019/Arduino/calibration/ diff --git a/MATE_2019/Arduino/mainBoard/mainBoard.ino b/MATE_2019/Arduino/mainBoard/mainBoard.ino index 3bf0ecf..89f576f 100644 --- a/MATE_2019/Arduino/mainBoard/mainBoard.ino +++ b/MATE_2019/Arduino/mainBoard/mainBoard.ino @@ -5,28 +5,25 @@ #include #include #include - -#include +#include #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 +#define SerialConnection Serial1 LSM9DS1 imu; +SF imuFusion; + +const int imuReadingNum = 5; -Servo FR; -Servo BR; -Servo BL; -Servo FL; +float imuReadings[imuReadingNum*2]; +int imuReadIndex = 0; -Servo UL; -Servo UR; -Servo UB; +float pitchTotal = 0.0f; +float rollTotal = 0.0f; -Servo shoulderTilt; -Servo wristTilt; -Servo wristTwist; +int loops = 0; const int buzzer = 17; const int water1 = 20; @@ -43,8 +40,12 @@ String disabledCommand = ":1500;1500;1500;1500;1500;1500;1500;1500;1500;1500;0"; void setup() { + for (int i = 0; i < imuReadingNum * 2; ++i) + { + imuReadings[i] = 0.0f; + } - delay(6000); + delay(1000); Wire.begin(); SerialConnection.begin(115200); @@ -54,19 +55,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 +63,7 @@ void setup() while (!imu.begin()) { - delay(500); + delay(100); } digitalWrite(buzzer, 0); @@ -86,7 +74,7 @@ void loop() static bool wireInit = false; while (!wireInit) { - Wire.beginTransmission(14); + Wire.beginTransmission(10); Wire.write("1500"); Wire.write("1500"); Wire.write(':'); @@ -112,7 +100,7 @@ void loop() delay(90); ++timer; - if (timer > 50) + if (timer > 5) { wireInit = true; } @@ -125,23 +113,70 @@ void loop() float ay = 0.0; float az = 0.0; + float gx = 0.0; + float gy = 0.0; + float gz = 0.0; + + float mx = 0.0; + float my = 0.0; + float mz = 0.0; + + float deltaT; + if (imu.accelAvailable()) { - // Updates ax, ay, and az + // Updates Accelerometer data imu.readAccel(); - ax = imu.ax; - ay = imu.ay; - az = imu.az; + ax = imu.ax;//imu.calcAccel(imu.ax); + ay = imu.ay;//imu.calcAccel(imu.ay); + az = imu.az;//imu.calcAccel(imu.az); } - //if (imu.gyroAvailable()) + if (imu.gyroAvailable()) { - // Updates gx, gy, and gz - //imu.readGyro(); + // Updates Gyroscope data + imu.readGyro(); + gx = imu.calcGyro(imu.gx) * DEG_TO_RAD; + gy = imu.calcGyro(imu.gy) * DEG_TO_RAD; + gz = imu.calcGyro(imu.gz) * DEG_TO_RAD; } - float roll = atan2(ay, az) * 180 / PI; - float pitch = atan2(-ax, sqrt(ay * ay + az * az)) * 180 / PI; + if (imu.magAvailable()) + { + // Updates Compass data + imu.readMag(); + mx = imu.mx * 0.1; + my = imu.my * 0.1; + mz = imu.mz * 0.1; + } + + deltaT = imuFusion.deltatUpdate(); + imuFusion.MahonyUpdate(gx, gy, gz, ax, ay, az, deltaT); + + pitchTotal -= imuReadings[imuReadIndex]; + imuReadings[imuReadIndex] = atan2(-ax, sqrt(ay * ay + az * az)) * RAD_TO_DEG;//imuFusion.getPitch(); + pitchTotal += imuReadings[imuReadIndex]; + + rollTotal -= imuReadings[imuReadIndex+1]; + imuReadings[imuReadIndex+1] = atan2(ay, az) * RAD_TO_DEG;//imuFusion.getRoll(); + rollTotal += imuReadings[imuReadIndex+1]; + + imuReadIndex += 2; + if (imuReadIndex >= imuReadingNum*2) + { + imuReadIndex = 0; + } + + float pitch = pitchTotal / imuReadingNum; + float roll = rollTotal / imuReadingNum; + + //float pitch = imuFusion.getPitch(); + //float roll = imuFusion.getRoll(); + + //float roll = atan2(ay, az) * RAD_TO_DEG; + //float pitch = atan2(-ax, sqrt(ay * ay + az * az)) * RAD_TO_DEG; + + ++loops; char cstr[16]; itoa(timer, cstr, 10); @@ -153,16 +188,8 @@ void loop() if (SerialConnection.read() == ':') { timer = 0; // Reset timer if valid data received - // Only send data back if data was received -// if (digitalRead(water4) && digitalRead(water3) && digitalRead(water2) && digitalRead(water1)) -// { -// writeString("0"); -// } -// else -// { -// writeString("1"); -// } + String info; info = disabledCommand; @@ -171,7 +198,26 @@ void loop() info.toCharArray(driveCommands, COMMAND_SIZE - 1); drive(driveCommands); - writeString(info); + // Send back Water Sensor data + if (digitalRead(water4) && digitalRead(water3) && digitalRead(water2) && digitalRead(water1)) + { + writeString("0;"); + } + else + { + writeString("1;"); + } + + + // Send back IMU data + writeString(String(pitch,2)); + writeString(";"); + writeString(String(roll,2)); + + writeString(";"); + writeString(String(loops)); + + loops = 0; digitalWrite(buzzer, 1); @@ -184,12 +230,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 +293,5 @@ void drive(char array[]) Wire.write(commands[7]); Wire.write(':'); Wire.endTransmission(); + } diff --git a/MATE_2019/Main/Headers/SerialPort.h b/MATE_2019/Main/Headers/SerialPort.h index b047790..8211320 100644 --- a/MATE_2019/Main/Headers/SerialPort.h +++ b/MATE_2019/Main/Headers/SerialPort.h @@ -1,9 +1,6 @@ -/* Zain Ul Mustafa 2017 */ - #ifndef SERIALPORT_H #define SERIALPORT_H -#define ARDUINO_WAIT_TIME 2000 #define MAX_DATA_LENGTH 255 #include @@ -19,9 +16,11 @@ class SerialPort DWORD errors; public: - SerialPort(const char *portName, const int baudRate); + SerialPort(); ~SerialPort(); + void openSerialPort(const char *portName, const int baudRate); + void closeSerialPort(); int readSerialPort(char *buffer, unsigned int buf_size); bool writeSerialPort(char *buffer, unsigned int buf_size); bool isConnected(); diff --git a/MATE_2019/Main/Headers/Utils.h b/MATE_2019/Main/Headers/Utils.h index 90d5d87..c3cbab3 100644 --- a/MATE_2019/Main/Headers/Utils.h +++ b/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); diff --git a/MATE_2019/Main/Sources/SerialPort.cpp b/MATE_2019/Main/Sources/SerialPort.cpp index 1314809..2b0167e 100644 --- a/MATE_2019/Main/Sources/SerialPort.cpp +++ b/MATE_2019/Main/Sources/SerialPort.cpp @@ -1,25 +1,21 @@ -#include "..\Headers\SerialPort.h" +#include "../Headers/SerialPort.h" -SerialPort::SerialPort(const char *portName, const int baudRate) +SerialPort::SerialPort() +{ + this->connected = false; + this->handler = INVALID_HANDLE_VALUE; +} + +SerialPort::~SerialPort() { closeSerialPort(); } + +void SerialPort::openSerialPort(const char *portName, const int baudRate) { this->connected = false; this->handler = CreateFileA(static_cast(portName), GENERIC_READ | GENERIC_WRITE, 0, NULL, OPEN_EXISTING, FILE_ATTRIBUTE_NORMAL, NULL); - if (this->handler == INVALID_HANDLE_VALUE) - { - if (GetLastError() == ERROR_FILE_NOT_FOUND) - { - printf("ERROR: Handle was not attached. Reason: %s not available\n", - portName); - } - else - { - printf("ERROR!!!"); - } - } - else + if (this->handler != INVALID_HANDLE_VALUE) { DCB dcbSerialParameters = {0}; @@ -43,19 +39,15 @@ SerialPort::SerialPort(const char *portName, const int baudRate) { this->connected = true; PurgeComm(this->handler, PURGE_RXCLEAR | PURGE_TXCLEAR); - Sleep(ARDUINO_WAIT_TIME); } } } } -SerialPort::~SerialPort() +void SerialPort::closeSerialPort() { - if (this->connected) - { - this->connected = false; - CloseHandle(this->handler); - } + this->connected = false; + CloseHandle(this->handler); } int SerialPort::readSerialPort(char *buffer, unsigned int buf_size) diff --git a/MATE_2019/Main/Sources/Utils.cpp b/MATE_2019/Main/Sources/Utils.cpp index 143d237..fdeacb4 100644 --- a/MATE_2019/Main/Sources/Utils.cpp +++ b/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) { diff --git a/MATE_2019/Main/main.cpp b/MATE_2019/Main/main.cpp index ce2114e..503792b 100644 --- a/MATE_2019/Main/main.cpp +++ b/MATE_2019/Main/main.cpp @@ -1,30 +1,28 @@ #include -#include ".\Headers\Gamepad.h" -#include ".\Headers\PID.h" -#include ".\Headers\SerialPort.h" -#include ".\Headers\Utils.h" +#include "Headers/Gamepad.h" +#include "Headers/PID.h" +#include "Headers/SerialPort.h" +#include "Headers/Utils.h" using namespace std; +std::string port; + bool disabled = false; 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; int yawOffset = 0; -// Change the name of the port with the port name of your computer -// Must remember that the backslashes are essential so do not remove them -const char* port = "\\\\.\\COM3"; -SerialPort arduino(port, 115200); +SerialPort arduino; Gamepad gamepad1 = Gamepad(1); Gamepad gamepad2 = Gamepad(2); @@ -36,96 +34,100 @@ void transferData(string data) arduino.writeSerialPort(charArray, data.size() - 1); delete[] charArray; + float* sensors = new float[3]; + // Wait for arduino to process command - Sleep(90); + Sleep(110); // Expects 0 or 1, if water has been detected arduino.readSerialPort(output, MAX_DATA_LENGTH); cout << " Received: "; - for (char c : output) + bool checked = false; + int i = 0; + + char* pch; + pch = strtok(output,";"); + + int j = 0; + while (pch != nullptr) { - cout << c; + sensors[j] = atof(pch); + pch = strtok(nullptr, ";"); + ++j; } - cout << endl << endl; - /*for (char c : output) + // Process data from Arduino + pitch = sensors[2]; + roll = sensors[1]; + + if (sensors[0] == 1) { - if (c == '1') - { - waterLeak = true; - } - else - { - waterLeak = false; - } + cout << "WATER DETECTED" << endl; } - if (waterLeak) + for (char c : output) { - for (int i = 0; i < 50; ++i) + cout << c; + if (!checked) { - cout << "WATER "; + if (c == '\0') + { + arduino.closeSerialPort(); + cout << "CONNECTION LOST: ATTEMPTING TO RECONNECT" << endl; + arduino.openSerialPort(port.c_str(), 115200); + } + checked = true; } - }*/ + output[i++] = '\0'; + } + cout << endl << endl; } // TODO split out drive and arm to their own files // rx ly lx void teleop(double FWD, double STR, double RCW) { - //cout << FWD << "\t" << STR << "\t" << RCW << endl; + // cout << FWD << "\t" << STR << "\t" << RCW << endl; // : 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.015, 0.0005, 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.008, 0.001, 0.0); + rollPID.setContinuous(false); + rollPID.setOutputLimits(-1.0, 1.0); + rollPID.setSetpoint(rollSetpoint); - // Let driver adjust angle of robot if necessary + // Reset level angle of robot if necessary. if (gamepad1.getButtonPressed(xButtons.A)) { - pitchSetpoint += -0.01; - } - else if (gamepad1.getButtonPressed(xButtons.Y)) - { - pitchSetpoint += 0.01; - } - - if (gamepad1.getButtonPressed(xButtons.B)) - { - rollSetpoint += -0.01; - } - else if (gamepad1.getButtonPressed(xButtons.X)) - { - rollSetpoint += 0.01; + pitchSetpoint = pitch; + rollSetpoint = roll; } - // Will not reach full power diagonally because of controller input (depending + // Note: Will not reach full power diagonally because of controller input (depending // on controller) const double rad45 = 45.0 * 3.14159 / 180.0; // 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 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 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.7 * (-(gamepad1.rightTrigger() - gamepad1.leftTrigger()) - + pitchPID.getOutput(pitch)); double* vals[] = {&FR, &BR, &BL, &FL, &UL, &UR, &UB}; @@ -200,12 +202,19 @@ void teleop(double FWD, double STR, double RCW) int main() { + std::cout << "Attempting to connect" << std::endl; + int i = 0; while (!arduino.isConnected()) { - cout << " Error in Arduino port name" << endl << endl; - } + port = R"(\\.\COM)" + std::to_string(i++); - cout << " Arduino connection made" << endl << endl; + arduino.openSerialPort(port.c_str(), 115200); + if (i > 15) + { + i = 0; + } + } + std::cout << "Connected" << std::endl; if (gamepad1.connected()) {