diff --git a/embedded/.vscode/.BROWSE.C_CPP.2.DB b/embedded/.vscode/.BROWSE.C_CPP.2.DB new file mode 100644 index 0000000..10e623f Binary files /dev/null and b/embedded/.vscode/.BROWSE.C_CPP.2.DB differ diff --git a/embedded/.vscode/settings.json b/embedded/.vscode/settings.json index 73f51b7..18b24fc 100755 --- a/embedded/.vscode/settings.json +++ b/embedded/.vscode/settings.json @@ -1,6 +1,6 @@ { "terminal.integrated.env.windows": { - "PATH": "C:\\Users\\glenn\\.platformio\\penv\\Scripts;C:\\Users\\glenn\\.platformio\\penv;C:\\Program Files (x86)\\Intel\\iCLS Client\\;C:\\watcom-1.3\\binnt;C:\\watcom-1.3\\binw;C:\\ProgramData\\Oracle\\Java\\javapath;C:\\Program Files\\Intel\\iCLS Client\\;C:\\windows\\system32;C:\\windows;C:\\windows\\System32\\Wbem;C:\\windows\\System32\\WindowsPowerShell\\v1.0\\;C:\\Program Files (x86)\\NVIDIA Corporation\\PhysX\\Common;C:\\WINDOWS\\system32;C:\\WINDOWS;C:\\WINDOWS\\System32\\Wbem;C:\\WINDOWS\\System32\\WindowsPowerShell\\v1.0\\;C:\\Program Files\\MATLAB\\R2017a\\bin;C:\\Program Files\\MATLAB\\R2016b\\bin;C:\\Program Files\\Java\\jdk1.8.0_121\\bin;C:\\Users\\glenn\\;C:\\Program Files (x86)\\Intel\\Intel(R) Management Engine Components\\DAL;C:\\Program Files\\Intel\\Intel(R) Management Engine Components\\DAL;C:\\Program Files (x86)\\Intel\\Intel(R) Management Engine Components\\IPT;C:\\Program Files\\Intel\\Intel(R) Management Engine Components\\IPT;C:\\Program Files\\Git\\cmd;C:\\Program Files\\LLVM\\bin;C:\\WINDOWS\\System32\\OpenSSH\\;C:\\Program Files\\Intel\\WiFi\\bin\\;C:\\Program Files\\Common Files\\Intel\\WirelessCommon\\;C:\\Program Files\\Java\\jdk1.8.0_121\\bin;C:\\F;C:\\Users\\glenn\\AppData\\Roaming\\npm;C:\\Users\\glenn\\AppData\\Local\\Microsoft\\WindowsApps;C:\\Program Files\\Microsoft VS Code\\bin;C:\\Users\\glenn\\AppData\\Local\\atom\\bin;%USERPROFILE%\\AppData\\Local\\Microsoft\\WindowsApps;;C:\\Program Files (x86)\\Intel\\iCLS Client\\;C:\\watcom-1.3\\binnt;C:\\watcom-1.3\\binw;C:\\ProgramData\\Oracle\\Java\\javapath;C:\\Program Files\\Intel\\iCLS Client\\;C:\\windows\\system32;C:\\windows;C:\\windows\\System32\\Wbem;C:\\windows\\System32\\WindowsPowerShell\\v1.0\\;C:\\Program Files (x86)\\NVIDIA Corporation\\PhysX\\Common;C:\\WINDOWS\\system32;C:\\WINDOWS;C:\\WINDOWS\\System32\\Wbem;C:\\WINDOWS\\System32\\WindowsPowerShell\\v1.0\\;C:\\Program Files\\MATLAB\\R2017a\\bin;C:\\Program Files\\MATLAB\\R2016b\\bin;C:\\Program Files\\Java\\jdk1.8.0_121\\bin;C:\\Users\\glenn\\;C:\\Program Files (x86)\\Intel\\Intel(R) Management Engine Components\\DAL;C:\\Program Files\\Intel\\Intel(R) Management Engine Components\\DAL;C:\\Program Files (x86)\\Intel\\Intel(R) Management Engine Components\\IPT;C:\\Program Files\\Intel\\Intel(R) Management Engine Components\\IPT;C:\\Program Files\\Git\\cmd;C:\\Program Files\\LLVM\\bin;C:\\WINDOWS\\System32\\OpenSSH\\;C:\\Program Files\\Intel\\WiFi\\bin\\;C:\\Program Files\\Common Files\\Intel\\WirelessCommon\\;C:\\Program Files\\Java\\jdk1.8.0_121\\bin;C:\\F;C:\\Users\\glenn\\AppData\\Roaming\\npm;C:\\Users\\glenn\\AppData\\Local\\Microsoft\\WindowsApps;C:\\Program Files\\Microsoft VS Code\\bin;C:\\Users\\glenn\\AppData\\Local\\atom\\bin;%USERPROFILE%\\AppData\\Local\\Microsoft\\WindowsApps;", + "PATH": "C:\\Users\\lbettina\\.platformio\\penv\\Scripts;C:\\Users\\lbettina\\.platformio\\penv;C:\\Program Files (x86)\\Common Files\\Siemens\\ACE\\Bin;C:\\Program Files (x86)\\SIEMENS\\SIMATIC.NET\\SimNetCom;C:\\Program Files\\Common Files\\Siemens\\Automation\\Simatic OAM\\bin;C:\\Program Files (x86)\\Common Files\\Oracle\\Java\\javapath;C:\\ProgramData\\Oracle\\Java\\javapath;C:\\Windows\\system32;C:\\Windows;C:\\Windows\\System32\\Wbem;C:\\Windows\\System32\\WindowsPowerShell\\v1.0\\;C:\\Program Files (x86)\\CONTACT Workspace Manager\\bin\\cdbwscall;C:\\Program Files (x86)\\Sennheiser\\SoftphoneSDK\\;C:\\Program Files (x86)\\Microsoft SQL Server\\Client SDK\\ODBC\\110\\Tools\\Binn\\;C:\\Program Files (x86)\\Microsoft SQL Server\\120\\Tools\\Binn\\;C:\\Program Files (x86)\\Microsoft SQL Server\\120\\DTS\\Binn\\;C:\\Users\\lbettina\\AppData\\Local\\Microsoft\\WindowsApps;C:\\Users\\lbettina\\AppData\\Local\\Programs\\Microsoft VS Code\\bin;C:\\Users\\lbettina\\AppData\\Local\\GitHubDesktop\\bin;C:\\Program Files (x86)\\Common Files\\Siemens\\ACE\\Bin;C:\\Program Files (x86)\\SIEMENS\\SIMATIC.NET\\SimNetCom;C:\\Program Files\\Common Files\\Siemens\\Automation\\Simatic OAM\\bin;C:\\Program Files (x86)\\Common Files\\Oracle\\Java\\javapath;C:\\ProgramData\\Oracle\\Java\\javapath;C:\\Windows\\system32;C:\\Windows;C:\\Windows\\System32\\Wbem;C:\\Windows\\System32\\WindowsPowerShell\\v1.0\\;C:\\Program Files (x86)\\CONTACT Workspace Manager\\bin\\cdbwscall;C:\\Program Files (x86)\\Sennheiser\\SoftphoneSDK\\;C:\\Program Files (x86)\\Microsoft SQL Server\\Client SDK\\ODBC\\110\\Tools\\Binn\\;C:\\Program Files (x86)\\Microsoft SQL Server\\120\\Tools\\Binn\\;C:\\Program Files (x86)\\Microsoft SQL Server\\120\\DTS\\Binn\\;C:\\Users\\lbettina\\AppData\\Local\\Microsoft\\WindowsApps;C:\\Users\\lbettina\\AppData\\Local\\Programs\\Microsoft VS Code\\bin;C:\\Users\\lbettina\\AppData\\Local\\GitHubDesktop\\bin", "PLATFORMIO_CALLER": "vscode" }, "C_Cpp.errorSquiggles": "Disabled", diff --git a/embedded/lib/Chassis/Chassis.cpp b/embedded/lib/Chassis/Chassis.cpp index 54e33a4..1cbe0b6 100755 --- a/embedded/lib/Chassis/Chassis.cpp +++ b/embedded/lib/Chassis/Chassis.cpp @@ -1,7 +1,7 @@ /* Chassis.cpp - Library for mechatronic component chassis. The chassis contains 2 DC-lego-motors which power the movement of the chassis - and 5 IR-sensors which work as internal navigation components. + and 5 sens_IR-sensors which work as internal navigation components. Created by Glenn Huber, 03.05.2018 Basecode by Robert Paly */ @@ -9,8 +9,9 @@ #include "Arduino.h" #include "Chassis.h" -Chassis::Chassis(int givenSpeed, float kp, float ki, float kd, int motorNumRight, int motorNumLeft, int sensorPin_0, int sensorPin_1, int sensorPin_2, int sensorPin_3, int sensorPin_4) { - if (DEBUGGER == true) Serial.print("Initializing chassis..."); +Chassis::Chassis(int givenSpeed, float kp, float ki, float kd, int motorNumRight, int motorNumLeft, int sensorPin_0, int sensorPin_1, int sensorPin_2, int sensorPin_3, int sensorPin_4) +{ + LOG1("Initializing chassis..."); //Initializing sensors. sensorPin0 = sensorPin_0; sensorPin1 = sensorPin_1; @@ -34,42 +35,42 @@ Chassis::Chassis(int givenSpeed, float kp, float ki, float kd, int motorNumRight previousSensorValue[3] = 0; previousSensorValue[4] = 0; - currentState.sensor[0] = 0; - currentState.sensor[1] = 0; - currentState.sensor[2] = 0; - currentState.sensor[3] = 0; - currentState.sensor[4] = 0; + sensor[0] = 0; + sensor[1] = 0; + sensor[2] = 0; + sensor[3] = 0; + sensor[4] = 0; //Initializing motors. numRight = motorNumRight; numLeft = motorNumLeft; AFMS = Adafruit_MotorShield(); motorRight = AFMS.getMotor(numRight); - motorLeft = AFMS.getMotor(numLeft); + motorLeft = AFMS.getMotor(numLeft); AFMS.begin(); - currentState.speed = givenSpeed; + speed = givenSpeed; //Initializing PID values. KP = kp; KI = ki; KD = kd; - error = 0; - PValue = 0; - IValue = 0; - DValue = 0; - PIDValue = 0; - previousPIDValue= 0; - previousError = 0; - previousI = 0; - korr = -0.01; - korr1 = 0; - korr2 = 0; - leftMotorSpeed = 0; + error = 0; + PValue = 0; + IValue = 0; + DValue = 0; + PIDValue = 0; + previousPIDValue = 0; + previousError = 0; + previousI = 0; + korr = -0.01; + korr1 = 0; + korr2 = 0; + leftMotorSpeed = 0; rightMotorSpeed = 0; leftMotorSpeed1 = 0; - rightMotorSpeed1= 0; + rightMotorSpeed1 = 0; targetSpeedLeft = 0; - targetSpeedRight= 0; + targetSpeedRight = 0; //Initializing timevariables millisOfDriving = 0; @@ -80,63 +81,93 @@ Chassis::Chassis(int givenSpeed, float kp, float ki, float kd, int motorNumRight turnCounter = 0; crossCounterLeft = 0; crossCounterRight = 0; - if (DEBUGGER == true) Serial.println(" complete!"); + + LOG3(" complete!"); } -void Chassis::loop(ChassisState *state) { - if (state->IR == false) { - currentState.IR = false; - state->IR = true; +void Chassis::loop(bool sens_IRR, int speedd, String directionn, int direcErrr) +{ + if (sens_IRR == false) + { + sens_IR = false; + sens_IRR = true; } - currentState.speed = state->speed; - currentState.direction = state->direction; - readSensor(); + speed = speedd; + direc = directionn; + readSensor(); - state->directionError = currentState.directionError; - state->sensor[0] = currentState.sensor[0]; - state->sensor[1] = currentState.sensor[1]; - state->sensor[2] = currentState.sensor[2]; - state->sensor[3] = currentState.sensor[3]; - state->sensor[4] = currentState.sensor[4]; + direcErr = direcErrr; + sensor[0] = sensor[0]; + sensor[1] = sensor[1]; + sensor[2] = sensor[2]; + sensor[3] = sensor[3]; + sensor[4] = sensor[4]; calculatePID(); - - state->PidValue = currentState.PidValue; - - if (currentState.actionDone == true) { - state->actionDone = currentState.actionDone; - currentState.actionDone = false; - if (DEBUGGER == true) Serial.println("Inside actiondone statement"); - if (DEBUGGER == true) Serial.print("currentState.actionDone: "); if (DEBUGGER == true) Serial.println(currentState.actionDone); + + state->PidValue = ppidValue; + + if (actionDone == true) + { + state->actionDone = actionDone; + actionDone = false; + + LOG3("actionDone: "+String(actionDone)); } } -void Chassis::readSensor() { +void Chassis::readSensor() +{ //Read sensor values - if (DEBUGGER == true) Serial.println("sensorreading"); + + LOG3("sensorreading"); sensor0 = digitalRead(sensorPin0); sensor1 = digitalRead(sensorPin1); sensor2 = digitalRead(sensorPin2); sensor3 = digitalRead(sensorPin3); sensor4 = digitalRead(sensorPin4); - previousSensorValue[0] = currentState.sensor[0]; - previousSensorValue[1] = currentState.sensor[1]; - previousSensorValue[2] = currentState.sensor[2]; - previousSensorValue[3] = currentState.sensor[3]; - previousSensorValue[4] = currentState.sensor[4]; - if (DEBUGGER == true) Serial.print("previousValues: "); if (DEBUGGER == true) Serial.print(previousSensorValue[0]); if (DEBUGGER == true) Serial.print(previousSensorValue[1]); if (DEBUGGER == true) Serial.print(previousSensorValue[2]); if (DEBUGGER == true) Serial.print(previousSensorValue[3]); if (DEBUGGER == true) Serial.println(previousSensorValue[4]); + previousSensorValue[0] = sensor[0]; + previousSensorValue[1] = sensor[1]; + previousSensorValue[2] = sensor[2]; + previousSensorValue[3] = sensor[3]; + previousSensorValue[4] = sensor[4]; + + LOG3("previousValues: "); + + LOG3(previousSensorValue[0]); + + LOG3(previousSensorValue[1]); + + LOG3(previousSensorValue[2]); + + LOG3(previousSensorValue[3]); - currentState.sensor[0] = sensor0; - currentState.sensor[1] = sensor1; - currentState.sensor[2] = sensor2; - currentState.sensor[3] = sensor3; - currentState.sensor[4] = sensor4; - if (DEBUGGER == true) Serial.print("actualValues: "); if (DEBUGGER == true) Serial.print(currentState.sensor[0]); if (DEBUGGER == true) Serial.print(currentState.sensor[1]); if (DEBUGGER == true) Serial.print(currentState.sensor[2]); if (DEBUGGER == true) Serial.print(currentState.sensor[3]); if (DEBUGGER == true) Serial.println(currentState.sensor[4]); + LOG3(previousSensorValue[4]); + + sensor[0] = sensor0; + sensor[1] = sensor1; + sensor[2] = sensor2; + sensor[3] = sensor3; + sensor[4] = sensor4; + + LOG3("actualValues: "); + + LOG3(sensor[0]); + + LOG3(sensor[1]); + + LOG3(sensor[2]); + + LOG3(sensor[3]); + + LOG3(sensor[4]); //If no fullline is detected let the sensors read the lines for normal motorcontrol - if (currentState.fullLine == false) { - if (DEBUGGER == true) Serial.println("Normal sensorreading"); + if (currentState.fullLine == false) + { + + LOG3("Normal sensorreading"); if ((sensor0 == 0) && (sensor1 == 0) && (sensor2 == 0) && (sensor3 == 0) && (sensor4 == 1)) error = 4; else if ((sensor0 == 0) && (sensor1 == 0) && (sensor2 == 0) && (sensor3 == 1) && (sensor4 == 1)) @@ -155,99 +186,119 @@ void Chassis::readSensor() { error = -3; else if ((sensor0 == 1) && (sensor1 == 0) && (sensor2 == 0) && (sensor3 == 0) && (sensor4 == 0)) error = -4; - else if ((sensor0 == 1) && (sensor1 == 1) && (sensor2 == 1) && (sensor3 == 0) && (sensor4 == 0))//90°-Kurve + else if ((sensor0 == 1) && (sensor1 == 1) && (sensor2 == 1) && (sensor3 == 0) && (sensor4 == 0)) //90°-Kurve error = -10; else if ((sensor0 == 0) && (sensor1 == 0) && (sensor2 == 1) && (sensor3 == 1) && (sensor4 == 1)) //90°-Kurve error = 10; - else if ((sensor0 == 1) && (sensor1 == 1) && (sensor2 == 1) && (sensor3 == 1) && (sensor4 == 1)) { + else if ((sensor0 == 1) && (sensor1 == 1) && (sensor2 == 1) && (sensor3 == 1) && (sensor4 == 1)) + { currentState.fullLine = true; } //Conditions for lost navigation lines else if ((sensor0 == 0) && (sensor1 == 0) && (sensor2 == 0) && (sensor3 == 0) && (sensor4 == 0)) { - if (error == -4) - error = -5; //If outer sensor loses line, turn hard. + if (error == -4) + error = -5; //If outer sensor loses line, turn hard. else if (error == 4) error = 5; } } //Conditions for fulllines - if (currentState.fullLine == true) { - if ((((sensor0 == 0) && (sensor4 == 0)) || (currentState.direction == "stop")) && (currentState.IR == true)) { + if (currentState.fullLine == true) + { + if ((((sensor0 == 0) && (sensor4 == 0)) || (currentState.dsens_IRection == "stop")) && (sens_IR == true)) + { currentState.fullLine = false; - if (DEBUGGER == true) Serial.println("changing actiondone because of fulline"); - currentState.actionDone = true; + + LOG3("changing actiondone because of fulline"); + actionDone = true; } - if (currentState.direction == "right") { + if (currentState.dsens_IRection == "right") + { error = 5; - if (DEBUGGER == true) Serial.print("Error: Cross"); if (DEBUGGER == true) Serial.println(error); + + LOG3("Error: Cross"); + + LOG3(error); } - if (currentState.direction == "straight") { + if (currentState.dsens_IRection == "straight") + { error = 0; - if (DEBUGGER == true) Serial.print("Error: Cross"); if (DEBUGGER == true) Serial.println(error); + + LOG3("Error: Cross"); + + LOG3(error); } - if (currentState.direction == "left") { + if (currentState.dsens_IRection == "left") + { error = -5; - if (DEBUGGER == true) Serial.print("Error: Cross"); if (DEBUGGER == true) Serial.println(error); + + LOG3("Error: Cross"); + + LOG3(error); } - if (currentState.direction == "stop") { + if (currentState.dsens_IRection == "stop") + { error = 0; - if (DEBUGGER == true) Serial.println("error = 0"); + + LOG3("error = 0"); } } - currentState.directionError = error; + dsens_IRectionError = error; } -void Chassis::calculatePID() { +void Chassis::calculatePID() +{ //previousPIDValue = PIDValue; - PValue = error; - IValue = IValue + error; - DValue = PIDValue - previousPIDValue; - IValue = constrain(IValue, -300, 300); + PValue = error; + IValue = IValue + error; + DValue = PIDValue - previousPIDValue; + IValue = constrain(IValue, -300, 300); //Print out values of the controller - if (DEBUGGER == true) Serial.print(" I= "); - if (DEBUGGER == true) Serial.print(IValue); - if (DEBUGGER == true) Serial.print(" P= "); - if (DEBUGGER == true) Serial.print(PValue); - if (DEBUGGER == true) Serial.print(" D= "); - if (DEBUGGER == true) Serial.print(DValue); - + LOG3(" I= " + String(IValue) + " P= " + String(PValue) + " D= " + String(DValue)); PIDValue = (KP * PValue) + (KI * IValue) * (KD * DValue); previousError = error; - if ((sensor0 == 0) && (sensor1 == 0) && (sensor2 == 1) && (sensor3 == 0) && (sensor4 == 0)) { - if ((IValue > 10) || (IValue < -10)) { + if ((sensor0 == 0) && (sensor1 == 0) && (sensor2 == 1) && (sensor3 == 0) && (sensor4 == 0)) + { + if ((IValue > 10) || (IValue < -10)) + { IValue = 0.95 * IValue; } - else { + else + { IValue = 0; } } - currentState.PidValue = PIDValue; + ppidValue = PIDValue; - if (DEBUGGER == true) Serial.print("PID = "); - if (DEBUGGER == true) Serial.println(currentState.PidValue); + LOG3("PID = " + String(PIDValue)); } -void Chassis::motorControl(float sonarFactor) { - if (currentState.speed == 0) { - if (DEBUGGER == true) Serial.println("stopping inside motorcontrol"); +void Chassis::motorControl(float sonarFactor) +{ + if (speed == 0) + { + + LOG3("stopping inside motorcontrol"); stop(); } - else { - if (DEBUGGER == true) Serial.println("Motorcontrol"); + else + { + + LOG3("Motorcontrol"); korr2 = 1 - korr; korr1 = 1 + korr; - leftMotorSpeed = currentState.speed + currentState.PidValue; - rightMotorSpeed= currentState.speed - currentState.PidValue; - leftMotorSpeed1= constrain(leftMotorSpeed, 0, currentState.speed + 50); - rightMotorSpeed1=constrain(rightMotorSpeed, 0, currentState.speed + 50); + leftMotorSpeed = speed + ppidValue; + rightMotorSpeed = speed - ppidValue; + leftMotorSpeed1 = constrain(leftMotorSpeed, 0, speed + 50); + rightMotorSpeed1 = constrain(rightMotorSpeed, 0, speed + 50); //Calculate the speed for both motors individualy - if (DEBUGGER == true) Serial.print("SonarFactor inside MotorControl: "); if (DEBUGGER == true) Serial.println(sonarFactor); + LOG3("SonarFactor inside MotorControl: " + String(sonarFactor)); targetSpeedLeft = sonarFactor * korr2 * leftMotorSpeed1; - targetSpeedRight= sonarFactor * korr1 * rightMotorSpeed1; + targetSpeedRight = sonarFactor * korr1 * rightMotorSpeed1; motorLeft->run(FORWARD); motorRight->run(FORWARD); @@ -259,194 +310,221 @@ void Chassis::motorControl(float sonarFactor) { //Drivespeed settings motorLeft->setSpeed(targetSpeedLeft); motorRight->setSpeed(targetSpeedRight); - - if (DEBUGGER == true) Serial.println("Speed of motors: "); - if (DEBUGGER == true) Serial.print(targetSpeedLeft); if (DEBUGGER == true) Serial.print(" || "); if (DEBUGGER == true) Serial.println(targetSpeedRight); + LOG3("Speed of motors: " + String(targetSpeedLeft) + " || " + String(targetSpeedRight)); } } -int Chassis::getStartSpeedLeft() { - if (targetSpeedLeft < 30) { +int Chassis::getStartSpeedLeft() +{ + if (targetSpeedLeft < 30) + { return 0; } - else if ((targetSpeedLeft >= 30) && (targetSpeedLeft < 60)) { + else if ((targetSpeedLeft >= 30) && (targetSpeedLeft < 60)) + { return 100; } - else { + else + { return 60; } } -int Chassis::getStartSpeedRight() { - if (targetSpeedRight < 30) { +int Chassis::getStartSpeedRight() +{ + if (targetSpeedRight < 30) + { return 0; } - else if ((targetSpeedRight >= 30) && (targetSpeedRight < 60)) { + else if ((targetSpeedRight >= 30) && (targetSpeedRight < 60)) + { return 100; } - else { + else + { return 60; } } -void Chassis::stop() { +void Chassis::stop() +{ motorLeft->setSpeed(0); - motorRight->setSpeed(0); - if (DEBUGGER == true) Serial.println("Vehicle stopped"); + motorRight->setSpeed(0); + LOG3("Vehicle stopped"); } -void Chassis::driveBack() { +void Chassis::driveBack() +{ motorLeft->run(BACKWARD); motorRight->run(BACKWARD); motorLeft->setSpeed(PUSH_SPEED); motorRight->setSpeed(PUSH_SPEED); - motorLeft->setSpeed(currentState.speed); - motorRight->setSpeed(currentState.speed); + motorLeft->setSpeed(speed); + motorRight->setSpeed(speed); } -void Chassis::driveBackLimited(unsigned int driveTime, unsigned long startTime) { +void Chassis::driveBackLimited(unsigned int driveTime, unsigned long startTime) +{ unsigned long actualMillis = millis(); millisOfDriving = actualMillis - startTime; - if (millisOfDriving > driveTime) { + if (millisOfDriving > driveTime) + { motorLeft->setSpeed(0); motorRight->setSpeed(0); - currentState.actionDone = true; - if (DEBUGGER == true) Serial.println("Changing actiondone because of driveback"); - currentState.IR = true; + actionDone = true; + LOG3("Changing actiondone because of driveback"); + sens_IR = true; } - else { - currentState.IR = false; + else + { + sens_IR = false; motorLeft->run(BACKWARD); motorRight->run(BACKWARD); motorLeft->setSpeed(PUSH_SPEED); - motorLeft->setSpeed(currentState.speed); + motorLeft->setSpeed(speed); motorRight->setSpeed(PUSH_SPEED); - motorRight->setSpeed(currentState.speed); - if (DEBUGGER == true) Serial.println("Driving back"); + motorRight->setSpeed(speed); + LOG3("Driving back"); } } -void Chassis::driveStraightLimited(unsigned int driveTime, unsigned long startTime) { +void Chassis::driveStraightLimited(unsigned int driveTime, unsigned long startTime) +{ unsigned long actualMillis = millis(); millisOfDriving = actualMillis - startTime; - if (DEBUGGER == true) Serial.print("millifOfDriving: "); - if (DEBUGGER == true) Serial.println(millisOfDriving); - if (millisOfDriving > driveTime) { + LOG3("millifOfDriving: "); + LOG3(millisOfDriving); + if (millisOfDriving > driveTime) + { motorRight->setSpeed(0); motorLeft->setSpeed(0); - currentState.actionDone = true; - if (DEBUGGER == true) Serial.println("Changing actiondone because of drivestraight"); - if (DEBUGGER == true) Serial.println("ifcase inside drive straight"); - currentState.IR = true; + actionDone = true; + LOG3("Changing actiondone because of drivestraight"); + LOG3("ifcase inside drive straight"); + sens_IR = true; } - else { - currentState.IR = false; + else + { + sens_IR = false; motorLeft->run(FORWARD); motorRight->run(FORWARD); motorLeft->setSpeed(PUSH_SPEED); motorRight->setSpeed(PUSH_SPEED); - motorLeft->setSpeed(currentState.speed); - motorRight->setSpeed(currentState.speed); - if (DEBUGGER == true) Serial.println("elsecase inside drive straight"); + motorLeft->setSpeed(speed); + motorRight->setSpeed(speed); + LOG3("elsecase inside drive straight"); } } -void Chassis::driveStraight() { +void Chassis::driveStraight() +{ motorLeft->run(FORWARD); motorRight->run(FORWARD); motorLeft->setSpeed(PUSH_SPEED); motorRight->setSpeed(PUSH_SPEED); - motorLeft->setSpeed(currentState.speed); - motorRight->setSpeed(currentState.speed); + motorLeft->setSpeed(speed); + motorRight->setSpeed(speed); } -void Chassis::turnLeftLimited(unsigned int curveTime, unsigned long startTime) { +void Chassis::turnLeftLimited(unsigned int curveTime, unsigned long startTime) +{ motorLeft->setSpeed(0); unsigned long actualMillis = millis(); millisOfDriving = actualMillis - startTime; - if (DEBUGGER == true) Serial.print("millifOfDriving: "); - if (DEBUGGER == true) Serial.println(millisOfDriving); - if (millisOfDriving > curveTime) { + LOG3("millifOfDriving: "); + LOG3(millisOfDriving); + if (millisOfDriving > curveTime) + { motorRight->setSpeed(0); - currentState.actionDone = true; - if (DEBUGGER == true) Serial.println("Changing actiondone because of turnleft"); - if (DEBUGGER == true) Serial.println("ifcase inside turn left"); - currentState.IR = true; + actionDone = true; + LOG3("Changing actiondone because of turnleft"); + LOG3("ifcase inside turn left"); + sens_IR = true; } - else { - currentState.IR = false; + else + { + sens_IR = false; motorRight->run(FORWARD); motorRight->setSpeed(PUSH_SPEED); - motorRight->setSpeed(currentState.speed); - if (DEBUGGER == true) Serial.println("elsecase inside turn left"); + motorRight->setSpeed(speed); + LOG3("elsecase inside turn left"); } } -void Chassis::turnLeft() { +void Chassis::turnLeft() +{ motorLeft->run(RELEASE); motorRight->run(FORWARD); motorRight->setSpeed(PUSH_SPEED); - motorRight->setSpeed(currentState.speed); - if (DEBUGGER == true) Serial.println("Turnleft"); + motorRight->setSpeed(speed); + LOG3("Turnleft"); } -void Chassis::turnRightLimited(unsigned int curveTime, unsigned long startTime) { +void Chassis::turnRightLimited(unsigned int curveTime, unsigned long startTime) +{ motorRight->setSpeed(0); unsigned long actualMillis = millis(); millisOfDriving = actualMillis - startTime; - if (DEBUGGER == true) Serial.print("millifOfDriving: "); - if (DEBUGGER == true) Serial.println(millisOfDriving); - if (millisOfDriving > curveTime) { + LOG3("millifOfDriving: "); + LGO3(millisOfDriving); + if (millisOfDriving > curveTime) + { motorLeft->setSpeed(0); - currentState.actionDone = true; - if (DEBUGGER == true) Serial.println("Changing actiondone because of turnright"); - if (DEBUGGER == true) Serial.println("ifcase inside turn right"); - currentState.IR = true; + actionDone = true; + LOG3("Changing actiondone because of turnright"); + LOG3("ifcase inside turn right"); + sens_IR = true; } - else { - currentState.IR = false; + else + { + sens_IR = false; motorLeft->run(FORWARD); motorLeft->setSpeed(PUSH_SPEED); - motorLeft->setSpeed(currentState.speed); - if (DEBUGGER == true) Serial.println("elsecase inside turn right"); + motorLeft->setSpeed(speed); + LOG3("elsecase inside turn right"); } } -void Chassis::turnRight() { +void Chassis::turnRight() +{ motorRight->run(RELEASE); motorLeft->run(FORWARD); motorLeft->setSpeed(PUSH_SPEED); - motorLeft->setSpeed(currentState.speed); - if (DEBUGGER == true) Serial.println("Turnright"); + motorLeft->setSpeed(speed); + LOG3("Turnright"); } -void Chassis::turnAround(int sensor) { +void Chassis::turnAround(int sensor) +{ int i = sensor; - if (DEBUGGER == true) Serial.println("Inside turnaround"); - if (currentState.actionDone == false) { + LOG3("Inside turnaround"); + if (actionDone == false) + { motorLeft->run(BACKWARD); motorRight->run(FORWARD); motorLeft->setSpeed(PUSH_SPEED); - motorLeft->setSpeed(currentState.speed); + motorLeft->setSpeed(speed); motorRight->setSpeed(PUSH_SPEED); - motorRight->setSpeed(currentState.speed); - if (DEBUGGER == true) Serial.println("Turnaround"); - if (DEBUGGER == true) Serial.print("turnCounter: "); if (DEBUGGER == true) Serial.println(turnCounter); - if (DEBUGGER == true) Serial.print("currentsensor: "); if (DEBUGGER == true) Serial.println(currentState.sensor[i]); - if (DEBUGGER == true) Serial.print("Sensor: "); if (DEBUGGER == true) Serial.println(i); - currentState.IR = false; + motorRight->setSpeed(speed); + LOG3("Turnaround"); + LOG3("turnCounter: " + String(turnCounter)); + LOG3("currentsensor: " + String(sensor[i]); + LOG3("Sensor: " + String(i)); + sens_IR =false; } - if ((currentState.sensor[i] == 0) && (turnCounter == 0)) { + if ((sensor[i] == 0) && (turnCounter == 0)) + { turnCounter++; - if (DEBUGGER == true) Serial.println("turncounter incrementing"); + LOG3("turncounter incrementing"); } - if ((previousSensorValue[i] == 0) && (currentState.sensor[i] == 1) && (turnCounter == 1)) { - currentState.actionDone = true; - if (DEBUGGER == true) Serial.println("Changing actiondone because of turnaround"); - currentState.IR = true; + if ((previousSensorValue[i] == 0) && (sensor[i] == 1) && (turnCounter == 1)) + { + actionDone = true; + LOG3("Changing actiondone because of turnaround"); + sens_IR = true; turnCounter = 0; motorLeft->setSpeed(0); motorRight->setSpeed(0); - if (DEBUGGER == true) Serial.println("Turnaround complete"); + LOG3("Turnaround complete"); } } \ No newline at end of file diff --git a/embedded/lib/Chassis/Chassis.h b/embedded/lib/Chassis/Chassis.h index 27d4af9..0d040b2 100755 --- a/embedded/lib/Chassis/Chassis.h +++ b/embedded/lib/Chassis/Chassis.h @@ -11,9 +11,11 @@ #include "Arduino.h" #include "Modular.h" #include "Adafruit_MotorShield.h" -#include "Configuration.h" +#include "ChassisConfiguration.h" -struct ChassisState { +/* +struct ChassisState +{ int speed; int directionError = 0; int virtualError = 0; @@ -24,84 +26,92 @@ struct ChassisState { bool fullLine = false; String direction = ""; }; +*/ class Chassis : public Component { - public: - Chassis(int givenSpeed, float kp, float ki, float kd, int motorNumRight, int motorNumLeft, int sensorPin0, int sensorPin1, int sensorPin2, int sensorPin3, int sensorPin4); - void loop(ChassisState *state); - void readSensor(); - void calculatePID(); - void motorControl(float sonarFactor); - void turnLeftLimited(unsigned int curveTime, unsigned long startTime); - void turnLeft(); - void turnRightLimited(unsigned int curveTime, unsigned long startTime); - void turnRight(); - void driveStraightLimited(unsigned int driveTime, unsigned long startTime); - void driveStraight(); - void turnAround(int sensor); - void driveBack(); - void driveBackLimited(unsigned int driveTime, unsigned long startTime); - int getStartSpeedLeft(); - int getStartSpeedRight(); - void stop(); - ChassisState currentState; + public: + Chassis(); + Chassis(int givenSpeed, float kp, float ki, float kd, int motorNumRight, int motorNumLeft, int sensorPin0, int sensorPin1, int sensorPin2, int sensorPin3, int sensorPin4); + void loop(ChassisState *state); + void readSensor(); + void calculatePID(); + void motorControl(float sonarFactor); + void turnLeftLimited(unsigned int curveTime, unsigned long startTime); + void turnLeft(); + void turnRightLimited(unsigned int curveTime, unsigned long startTime); + void turnRight(); + void driveStraightLimited(unsigned int driveTime, unsigned long startTime); + void driveStraight(); + void turnAround(int sensor); + void driveBack(); + void driveBackLimited(unsigned int driveTime, unsigned long startTime); + int getStartSpeedLeft(); + int getStartSpeedRight(); + void stop(); + ChassisState currentState; - protected: - //Motor objects - Adafruit_MotorShield AFMS; - Adafruit_DCMotor *motorRight; - Adafruit_DCMotor *motorLeft; - int numRight; - int numLeft; + protected: + //Motor objects + Adafruit_MotorShield AFMS; + Adafruit_DCMotor *motorRight; + Adafruit_DCMotor *motorLeft; + int numRight; + int numLeft; + bool actionDone = false; + int speed; + String direc; + int direcErr; - //Sensor variables - int sensor0; - int sensor1; - int sensor2; - int sensor3; - int sensor4; + //Sensor variables + int sensor[5]; + bool sens_IR = true; + int sensor0; + int sensor1; + int sensor2; + int sensor3; + int sensor4; - int previousSensorValue[5]; + int previousSensorValue[5]; - int sensorPin0; - int sensorPin1; - int sensorPin2; - int sensorPin3; - int sensorPin4; - + int sensorPin0; + int sensorPin1; + int sensorPin2; + int sensorPin3; + int sensorPin4; - //PID variables - float KP; - float KI; - float KD; - int error; - float PValue; - float IValue; - float DValue; - float PIDValue; - float previousPIDValue; - float previousError; - float previousI; - float korr; - float korr1; - float korr2; - float leftMotorSpeed; - float rightMotorSpeed; - float leftMotorSpeed1; - float rightMotorSpeed1; - float targetSpeedLeft; - float targetSpeedRight; + //PID variables + float KP; + float KI; + float KD; + int error; + float PValue; + float IValue; + float DValue; + float PIDValue; + float ppidValue; + float previousPIDValue; + float previousError; + float previousI; + float korr; + float korr1; + float korr2; + float leftMotorSpeed; + float rightMotorSpeed; + float leftMotorSpeed1; + float rightMotorSpeed1; + float targetSpeedLeft; + float targetSpeedRight; - //Timevariables - unsigned long millisOfDriving; - unsigned long previousMillis; - int timeLoopCounter; + //Timevariables + unsigned long millisOfDriving; + unsigned long previousMillis; + int timeLoopCounter; - //Counter - int turnCounter; - int crossCounterLeft; - int crossCounterRight; + //Counter + int turnCounter; + int crossCounterLeft; + int crossCounterRight; }; #endif \ No newline at end of file diff --git a/embedded/lib/Chassis/ChassisConfiguration.h b/embedded/lib/Chassis/ChassisConfiguration.h new file mode 100644 index 0000000..7b25ea5 --- /dev/null +++ b/embedded/lib/Chassis/ChassisConfiguration.h @@ -0,0 +1,19 @@ +#ifndef CHASSISCONFIGURATION_H +#define CHASSISCONFIGURATION_H + +#define RIGHT_MOTOR 1 +#define LEFT_MOTOR 2 +#define PIN_SENSOR_0 13 +#define PIN_SENSOR_1 12 +#define PIN_SENSOR_2 11 +#define PIN_SENSOR_3 10 +#define PIN_SENSOR_4 9 +#define SPEED 70 +#define REDUCED_SPEED 60 +#define PUSH_SPEED 80 +// BACKWARD??? + + + + +#endif \ No newline at end of file diff --git a/embedded/lib/Configuration/LogConfiguration.h b/embedded/lib/Configuration/LogConfiguration.h new file mode 100644 index 0000000..8fac8a0 --- /dev/null +++ b/embedded/lib/Configuration/LogConfiguration.h @@ -0,0 +1,28 @@ +#ifndef LOGCONFIGURATION_H +#define LOGCONFIGURATION_H + +#define LOGLEVELCONFIGURATION 2 // can have values from 0-3, 0-without, 1 error, 2 info, 3 verbose debugging + +#if LOGLEVELCONFIGURATION == 3 +#define LOG1(logg1) Serial.println(logg1) +#define LOG2(logg2) Serial.println(logg2) +#define LOG3(logg3) Serial.println(logg3) +#elif LOGLEVELCONFIGURATION == 2 +#define LOG1(logg1) Serial.println(logg1) +#define LOG2(logg2) Serial.println(logg2) +#define LOG3(logg3) +#elif LOGLEVELCONFIGURATION == 1 +#define LOG1(logg1) Serial.println(logg1) +#define LOG2(logg2) +#define LOG3(logg3) +#elif LOGLEVELCONFIGURATION == 0 +#define LOG1(logg1) +#define LOG2(logg2) +#define LOG3(logg3) +#else +#define LOG1(logg1) Serial.println("wrong log level value\nlogging levels: 0-without, 1 error, 2 info, 3 verbose debugging") +#define LOG2(logg2) Serial.println("wrong log level value\nlogging levels: 0-without, 1 error, 2 info, 3 verbose debugging") +#define LOG3(logg3) Serial.println("wrong log level value\nlogging levels: 0-without, 1 error, 2 info, 3 verbose debugging") +#endif + +#endif \ No newline at end of file diff --git a/embedded/lib/Configuration/MainConfiguration.h b/embedded/lib/Configuration/MainConfiguration.h new file mode 100644 index 0000000..049787d --- /dev/null +++ b/embedded/lib/Configuration/MainConfiguration.h @@ -0,0 +1,62 @@ +#ifndef MAINCONFIGURATION_H +#define MAINCONFIGURATION_H + +/* + file that contains the default configuration for the main function + Note: + SENSOR_* is concerning the sensor + *ITERATION* is an iteration variable (e.g- for loops) + *SECONDS* is stating, that the value is given in seconds and will be recalculated for milliseconds (for func delay()) +*/ + +#define MAX_NUMBEROFSERVABLE_SMARTBOX_REQUESTS 10 +#define DIST_TO_BOX 0.5 // distance to SmartBox in cm + + +// below is copied from Configuration.h +//Setup for chassis ///////////////// +#define RIGHT_MOTOR 1 +#define LEFT_MOTOR 2 +#define PIN_SENSOR_0 13 +#define PIN_SENSOR_1 12 +#define PIN_SENSOR_2 11 +#define PIN_SENSOR_3 10 +#define PIN_SENSOR_4 9 +#define SPEED 70 +#define REDUCED_SPEED 60 +#define PUSH_SPEED 80 +///////////////////////////////////// + +//Setup for PID ///////////////////// +#define K_P 11 +#define K_I 0.3 +#define K_D 0 +///////////////////////////////////// + +//Setup for sonar /////////////////// +#define SONAR_SERVO_PIN 5 +#define SONAR_TRIGGER_PIN 15 +#define SONAR_ECHO_PIN 14 +#define SONAR_MAX_DISTANCE 9999 +#define MIN_ERROR -5 +#define MAX_ERROR 5 +#define MIN_TURN_ANGLE 180 +#define MAX_TURN_ANGLE 0 +//////////////////////////////////// + +//Setup for Hoist ////////////////// +#define HOIST_SERVO_PIN 6 +#define HOIST_SERVO_DELAY 30 +#define HOIST_POSITION_MIN 168 +#define HOIST_POISITION_MAX 65 +//////////////////////////////////// + +//Setup for Vision ///////////////// +#define VISION_SERVO_PIN 5 +#define VISION_DELAY_FACTOR 6 +#define VISION_TOLERANCE_LEFT 165 +#define VISION_TOLERANCE_RIGHT 155 +#define VISION_START_ANGLE 90 +//////////////////////////////////// + +#endif \ No newline at end of file diff --git a/embedded/lib/Configuration/Configuration.h b/embedded/lib/Configuration_old/Configuration.h old mode 100755 new mode 100644 similarity index 94% rename from embedded/lib/Configuration/Configuration.h rename to embedded/lib/Configuration_old/Configuration.h index 2363a81..914453b --- a/embedded/lib/Configuration/Configuration.h +++ b/embedded/lib/Configuration_old/Configuration.h @@ -1,73 +1,78 @@ -/* - Library to contain the configuratioindetails of the different modules - Created by Glenn Huber, 03.05.2018 -*/ - -//Option to activate the Serial.print -#define DEBUGGER true - -//Setup for chassis ///////////////// -#define RIGHT_MOTOR 1 -#define LEFT_MOTOR 2 -#define PIN_SENSOR_0 13 -#define PIN_SENSOR_1 12 -#define PIN_SENSOR_2 11 -#define PIN_SENSOR_3 10 -#define PIN_SENSOR_4 9 -#define SPEED 70 -#define REDUCED_SPEED 60 -#define PUSH_SPEED 80 -///////////////////////////////////// - -//Setup for PID ///////////////////// -#define K_P 11 -#define K_I 0.3 -#define K_D 0 -///////////////////////////////////// - -//Setup for sonar /////////////////// -#define SONAR_SERVO_PIN 5 -#define SONAR_TRIGGER_PIN 15 -#define SONAR_ECHO_PIN 14 -#define SONAR_MAX_DISTANCE 9999 -#define MIN_ERROR -5 -#define MAX_ERROR 5 -#define MIN_TURN_ANGLE 180 -#define MAX_TURN_ANGLE 0 -//////////////////////////////////// - -//Setup for Hoist ////////////////// -#define HOIST_SERVO_PIN 6 -#define HOIST_SERVO_DELAY 30 -#define HOIST_POSITION_MIN 168 -#define HOIST_POISITION_MAX 65 -//////////////////////////////////// - -//Setup for Vision ///////////////// -#define VISION_SERVO_PIN 5 -#define VISION_DELAY_FACTOR 6 -#define VISION_TOLERANCE_LEFT 165 -#define VISION_TOLERANCE_RIGHT 155 -#define VISION_START_ANGLE 90 -//////////////////////////////////// - -//Setup for webAPI ///////////////// -#define SECRET_SSID "DigitalLab" -#define SECRET_PASS "digital42HSR" -#define KEY_INDEX 1 -//Ip-Address ////////////////////// -#define IP1 192 -#define IP2 168 -#define IP3 1 -#define IP4 4 -#define IP4_Idefix 3 -#define IP4_Obelix 27 -//WiFi pins ////////////////////// -#define WIFI_CS 8 -#define WIFI_IRQ 7 -#define WIFI_RST 4 -#define WIFI_EN 2 -//Ports ////////////////////////// -#define LISTEN_PORT 80 -#define SENDING_PORT 1880 -////////////////////////////////// \ No newline at end of file +/* + Library to contain the configuratioindetails of the different modules + Created by Glenn Huber, 03.05.2018 + THIS FILE IS DEPRECATED, THEREFORE EVERYTHING IS COMMENTED OUT! +*/ + +/* +//Option to activate the Serial.print +#define DEBUGGER true + +//Setup for chassis ///////////////// +#define RIGHT_MOTOR 1 +#define LEFT_MOTOR 2 +#define PIN_SENSOR_0 13 +#define PIN_SENSOR_1 12 +#define PIN_SENSOR_2 11 +#define PIN_SENSOR_3 10 +#define PIN_SENSOR_4 9 +#define SPEED 70 +#define REDUCED_SPEED 60 +#define PUSH_SPEED 80 +///////////////////////////////////// + +//Setup for PID ///////////////////// +#define K_P 11 +#define K_I 0.3 +#define K_D 0 +///////////////////////////////////// + +//Setup for sonar /////////////////// +#define SONAR_SERVO_PIN 5 +#define SONAR_TRIGGER_PIN 15 +#define SONAR_ECHO_PIN 14 +#define SONAR_MAX_DISTANCE 9999 +#define MIN_ERROR -5 +#define MAX_ERROR 5 +#define MIN_TURN_ANGLE 180 +#define MAX_TURN_ANGLE 0 +//////////////////////////////////// + +//Setup for Hoist ////////////////// +#define HOIST_SERVO_PIN 6 +#define HOIST_SERVO_DELAY 30 +#define HOIST_POSITION_MIN 168 +#define HOIST_POISITION_MAX 65 +//////////////////////////////////// + +//Setup for Vision ///////////////// +#define VISION_SERVO_PIN 5 +#define VISION_DELAY_FACTOR 6 +#define VISION_TOLERANCE_LEFT 165 +#define VISION_TOLERANCE_RIGHT 155 +#define VISION_START_ANGLE 90 +//////////////////////////////////// + +//Setup for webAPI ///////////////// +#define SECRET_SSID "DigitalLab" +#define SECRET_PASS "digital42HSR" +#define KEY_INDEX 1 +//Ip-Address ////////////////////// +#define IP1 192 +#define IP2 168 +#define IP3 1 +#define IP4 4 +#define IP4_Idefix 3 +#define IP4_Obelix 27 +//WiFi pins ////////////////////// +#define WIFI_CS 8 +#define WIFI_IRQ 7 +#define WIFI_RST 4 +#define WIFI_EN 2 +//Ports ////////////////////////// +#define LISTEN_PORT 80 +#define SENDING_PORT 1880 +////////////////////////////////// + + +*/ \ No newline at end of file diff --git a/embedded/lib/MQTTTasks/MQTTTasks.cpp b/embedded/lib/MQTTTasks/MQTTTasks.cpp new file mode 100644 index 0000000..e10b38f --- /dev/null +++ b/embedded/lib/MQTTTasks/MQTTTasks.cpp @@ -0,0 +1,396 @@ +#include "MQTTTasks.h" + +MQTTTasks::MQTTTasks() +{ + myJSONStr tmp; + for (int i = 0; i < MAX_JSON_MESSAGES_SAVED; i++) + { + messages[i] = tmp; + } + mqtt_class_counter = 0; + mqtt_class_counter_full = 0; + isEmpty = false; + urgentMessage = 0; + startIteration = 0; +} + +myJSONStr MQTTTasks::getLastMessage() +{ + if (!isEmpty) + return messages[mqtt_class_counter]; + else + { + LOG1("no messages"); + LOG2("there has been no messages saved"); + } +} +myJSONStr MQTTTasks::doLastMessage() +{ + myJSONStr tmp = getLastMessage(); + deleteMessage(0); + return tmp; +} + +myJSONStr MQTTTasks::doLastUrgentMessage() +{ + myJSONStr tmp; + for (int i = 0; i < MAX_JSON_MESSAGES_SAVED; i++) + { + if (getDesiredLastMessage(i).urgent) + { + tmp = getDesiredLastMessage(i); + deleteMessage(i); + return tmp; + break; + } + } + return tmp; +} + +bool MQTTTasks::hasUrgentMessage() +{ + if (urgentMessage == 0) + return false; + else + return true; +} + +myJSONStr MQTTTasks::getDesiredLastMessage(int fromLast) +{ + myJSONStr tmp; + if (!isEmpty) + { + if ((fromLast <= MAX_JSON_MESSAGES_SAVED) && (fromLast >= 0)) + { + int res = mqtt_class_counter - fromLast; + if (res < 0) + tmp = messages[MAX_JSON_MESSAGES_SAVED + res]; + else + tmp = messages[res]; + } + else + { + LOG1("request of desired message failed"); + LOG2("you entered: " + String(fromLast) + ", but there is a maximum of: " + String(MAX_JSON_MESSAGES_SAVED) + "and a minimum of 0!"); + } + } + return tmp; +} + +myJSONStr MQTTTasks::getDesiredMessage(int certainCurrentIterator) +{ + myJSONStr tmp; + if (!isEmpty) + { + if (certainCurrentIterator >= 0) + { + int currIt = certainCurrentIterator % MAX_JSON_MESSAGES_SAVED; + tmp = messages[currIt]; + } + else + { + LOG1("request of desired message failed"); + LOG2("you entered: " + String(certainCurrentIterator) + ", but there is a minimum of 0!"); + } + } + return tmp; +} +myJSONStr MQTTTasks::getDoDesiredMessage(int certainCurrentIterator) +{ + myJSONStr tmp; + if (!isEmpty) + { + if (certainCurrentIterator >= 0) + { + int currIt = certainCurrentIterator % MAX_JSON_MESSAGES_SAVED; + tmp = doLastMessage(); + } + else + { + LOG1("request of desired message failed"); + LOG2("you entered: " + String(certainCurrentIterator) + ", but there is a minimum of 0!"); + } + } + return tmp; +} + +bool MQTTTasks::setStartforIterations(int fromCurrentIterator) +{ + if (fromCurrentIterator <= fromCurrentIterator) + { + startIteration = fromCurrentIterator; + return true; + } + + else + { + LOG2("Iterator is wrong"); + LOG3("you entered: " + String(fromCurrentIterator)); + return false; + } +} + +bool MQTTTasks::setCurrentIteratorforIterations() +{ + startIteration = mqtt_class_counter; + return true; +} + +myJSONStr MQTTTasks::iterateAndDoMessages() +{ + myJSONStr tmp; + if (startIteration == mqtt_class_counter) + { + ; // if default values from myJSONStr are returned, it will be shown here + } + else + { + tmp = getDesiredMessage(startIteration); + startIteration++; + } + return tmp; +} + +int MQTTTasks::returnCurrentIterator() // returns absolute counter, if MAX_JSON_MESSAGES_SAVED reached and first message is overridden it can be detected from outside! +{ + return (mqtt_class_counter + mqtt_class_counter_full * MAX_JSON_MESSAGES_SAVED); +} + +bool MQTTTasks::deleteMessage(int fromLast) // sets this struct to default +{ + myJSONStr tmp; + tmp.hostname = "del"; // to clearly see which one were deleted + if (!isEmpty) + { + if ((fromLast < MAX_JSON_MESSAGES_SAVED) && (fromLast >= 0)) + { + int res = mqtt_class_counter - fromLast; + if (res < 0) + { + if (messages[MAX_JSON_MESSAGES_SAVED + res].urgent) + urgentMessage--; + messages[MAX_JSON_MESSAGES_SAVED + res] = tmp; + LOG3("message " + String(MAX_JSON_MESSAGES_SAVED + res) + " deleted successfully"); + return true; + } + else + { + if (messages[res].urgent) + urgentMessage--; + messages[res] = tmp; + LOG3("message " + String(res) + " deleted successfully"); + return true; + } + } + else + { + LOG1("deleting failed"); + LOG2("you entered: " + String(fromLast) + ", but there is a maximum of: " + String(MAX_JSON_MESSAGES_SAVED) + "and a minimum of 0!"); + return false; + } + } +} + +bool MQTTTasks::addMessage(myJSONStr mess) +{ + if (mqtt_class_counter == (MAX_JSON_MESSAGES_SAVED - 1)) + { + mqtt_class_counter = 0; + mqtt_class_counter_full++; + messages[mqtt_class_counter] = mess; + isEmpty = false; + return true; + } + else + { + mqtt_class_counter++; // Attention: messages[0] is the MAX_JSON_MESSAGES_SAVED-th message! + messages[mqtt_class_counter] = mess; + isEmpty = false; + return true; + } + if (mess.urgent) + urgentMessage++; + LOG3("message added successfully"); +} + +MQTTTasks *MQTTTasks::operator=(MQTTTasks *other) // needed in main.cpp +{ + return this; +} + +String *MQTTTasks::returnMQTTtopics(myJSONStr passingMessage) +{ + String tmp[MAX_MQTT_TOPIC_DEPTH]; + int k1 = 0; // lower cut-bound + int k2 = 0; // upper cut-bound + int k3 = 0; // num of strings (must be below above!) + for (int i = 0; i < passingMessage.topic.length() - 2; i++) + { + if (passingMessage.topic[i] == '/') + { + k2 = i; + if (k3 == MAX_MQTT_TOPIC_DEPTH) + break; + else + { + tmp[k3] = passingMessage.topic.substring(k1, k2); + k1 = i + 1; + k3++; + } + } + } + k2 = passingMessage.topic.length(); + tmp[k3] = passingMessage.topic.substring(k1, k2); + delete stringpassing; + if (k3 > 0) + { + stringpassing = new String[k3 + 1]; + for (int i = 0; i <= k3; i++) + { + stringpassing[i] = tmp[i]; + } + return stringpassing; + } + else + { + LOG3("k3 has no reasonable value: " + String(k3)); + return nullptr; + } +} + + +myJSONStr *MQTTTasks::getBetween(int from, int to) // from index to index +{ + /* + if (from == MAX_JSON_MESSAGES_SAVED) + from = 0; + else + from++; + */ + int tmp_to = to % MAX_JSON_MESSAGES_SAVED; + int tmp_from = from % MAX_JSON_MESSAGES_SAVED; + int tmp_to_cycles = (int)to / MAX_JSON_MESSAGES_SAVED; // how many times messages overridden + int tmp_from_cycles = (int)from / MAX_JSON_MESSAGES_SAVED; // how many times messages overriddenF + if ((to < 0) || (from < 0) || (from == to) || (from > to)) // if invalid values + { + LOG1("wrong value entered (same or negative)"); + LOG2("entered from: " + String(from) + ", to: " + String(to) + "\t\t relative from: " + String(tmp_from) + ", to: " + String(tmp_to)); + LOG3("minimum is 0, maximum is " + String(MAX_JSON_MESSAGES_SAVED)); + return nullptr; + } + else + { + // LOG3("tmp_from_cycles :" + String(tmp_from_cycles) + "\t tmp_to_cycles: " + String(tmp_to_cycles) + "\t tmp_from: " + String(tmp_from) + "\t tmp_to: " + String(tmp_to)); // can be used for debugging + + if (from == to) + { + LOG2("nothing to do"); + LOG3("you entered twice the same value: " + String(from)); + return nullptr; + } + else if (tmp_from_cycles == tmp_to_cycles) + { + int tmp = tmp_to - tmp_from; + // returnBetween = new myJSONStr[tmp + 2]; + returnBetween[0].level = tmp + 1; // this[0].level is array size + for (int i = 1; i < tmp + 1; i++) + { + returnBetween[i] = messages[tmp_from + i]; + } + } + else if ((tmp_from_cycles + 1) < tmp_to_cycles) // if more then one time all messages overridden + { + // returnBetween = new myJSONStr[MAX_JSON_MESSAGES_SAVED + 2]; + returnBetween[0].level = MAX_JSON_MESSAGES_SAVED + 2; // this.level is array size + for (int i = 1; i < MAX_JSON_MESSAGES_SAVED + 1; i++) + { + returnBetween[i] = messages[i]; // not returnBetween = messages since then access to private variable of class + } + } + else if ((tmp_from_cycles + 1) == tmp_to_cycles) + { + // returnBetween = new myJSONStr[MAX_JSON_MESSAGES_SAVED - tmp_from + tmp_to + 2]; + returnBetween[0].level = MAX_JSON_MESSAGES_SAVED - tmp_from + tmp_to + 2; // this.level is array size + for (int i = 0; i < (MAX_JSON_MESSAGES_SAVED - tmp_from); i++) + { + returnBetween[i + 1] = messages[tmp_from + i]; + } + for (int i = 0; i < tmp_to + 1; i++) + { + returnBetween[i + 1 + MAX_JSON_MESSAGES_SAVED - tmp_from] = messages[i]; + } + } + else + LOG1("special case"); + + //LOG2("size to return: " + String(returnBetween[0].level)); // + //for (int i = 1; i < returnBetween[0].level; i++) + // LOG3("the " + String(i) + "-th Element to return is: " + returnBetween[i].hostname); + return returnBetween; + //delete[] returnBetween; + } +} + +void MQTTTasks::printAllMessages(byte choice) // 0 for hostname, 1 level, 2 request, 3 params +{ + String printable = ""; + int myModulo = 5; // number of how many entries in one row + switch (choice) + { + case 0: // hostname + for (int i = 0; i < MAX_JSON_MESSAGES_SAVED; i++) + { + //LOG1("in the save are the following (" + String(i) + "): " + messages[i].hostname); + printable += "\t[" + String(i) + "]: " + messages[i].hostname; + if ((i % myModulo == 0) && (i > 1)) + { + LOG1(printable); + printable = ""; + } + } + LOG1(printable); + break; + case 1: // level + for (int i = 0; i < MAX_JSON_MESSAGES_SAVED; i++) + { + //LOG1("in the save are the following (" + String(i) + "): " + messages[i].hostname); + printable += "\t[" + String(i) + "]: " + messages[i].level; + if ((i % myModulo == 0) && (i > 1)) + { + LOG1(printable); + printable = ""; + } + } + LOG1(printable); + break; + case 2: // request + for (int i = 0; i < MAX_JSON_MESSAGES_SAVED; i++) + { + //LOG1("in the save are the following (" + String(i) + "): " + messages[i].hostname); + printable += "\t[" + String(i) + "]: " + messages[i].request; + if ((i % myModulo == 0) && (i > 1)) + { + LOG1(printable); + printable = ""; + } + } + LOG1(printable); + break; + case 3: // params + for (int i = 0; i < MAX_JSON_MESSAGES_SAVED; i++) + { + //LOG1("in the save are the following (" + String(i) + "): " + messages[i].hostname); + printable += "\t[" + String(i) + "]: ["; + for (int j = 0; j < 3; j++) + printable += String(messages[i].vehicleParams[j]) + "; "; + printable += String(messages[i].vehicleParams[4]) + "]"; + if ((i % myModulo == 0) && (i > 1)) + { + LOG1(printable); + printable = ""; + } + } + LOG1(printable); + break; + } +} \ No newline at end of file diff --git a/embedded/lib/MQTTTasks/MQTTTasks.h b/embedded/lib/MQTTTasks/MQTTTasks.h new file mode 100644 index 0000000..6ff22d7 --- /dev/null +++ b/embedded/lib/MQTTTasks/MQTTTasks.h @@ -0,0 +1,45 @@ +#ifndef MQTTTASKS_H +#define MQTTTASKS_H + +#include +#include +// own files: +#include +#include +#include + +class MQTTTasks // this class saves all incoming messages in an struct-array and provides all necessary functions to access/handle with messages +{ +public: + MQTTTasks(); + myJSONStr getLastMessage(); // returns the most current/last message + myJSONStr doLastMessage(); // same as above, but calls deleteMessage + myJSONStr doLastUrgentMessage(); // same as above, but for urgent messages + myJSONStr getDesiredLastMessage(int fromLast); // returns the message, which is fromLast from current (e.g. getDesiredLastMessage(1) returns second last/current message) + myJSONStr getDesiredMessage(int certainCurrentIterator); // returns the message, which the certainCurrentIterator % MAX_JSON_MESSAGES_SAVED message in the array + myJSONStr getDoDesiredMessage(int certainCurrentIterator); // same as above, but does message at same time + bool setStartforIterations(int fromCurrentIterator); // used for iterations, sets Variable startIteration + bool setCurrentIteratorforIterations(); // same as above, but for mqtt_class_counter + myJSONStr iterateAndDoMessages(); // used for iterations, will return messages from startIteration to current + int returnCurrentIterator(); // returns absolute current value of message counter, 0 if empty, if MAX_JSON_MESSAGES_SAVED reached and first message is overridden it can be detected from outside! + bool deleteMessage(int fromLast); // deletes message, which is fromLast from current + bool addMessage(myJSONStr mess); // saves message, FIFO order + MQTTTasks *operator=(MQTTTasks *other); // copy constructor + myJSONStr *getBetween(int from, int to); // returns messages between indexes, to use for iterations, from index to index, index = returnCurrentIterator, 0-th Element.level is array size + String *returnMQTTtopics(myJSONStr passingMessage); // returns String-Array of topics from MQTT topic structure, strings divided by / + void printAllMessages(byte choice); // prints all saved messages on serial output; 0 for hostname, 1 level, 2 request, 3 params + bool hasUrgentMessage(); // shows if urgent messages arrived + +private: + myJSONStr messages[MAX_JSON_MESSAGES_SAVED]; // message save + int mqtt_class_counter; // iterator of last Element + int mqtt_class_counter_full; // same as above, but if first Element is overritten again, it increases, its equivivalent of how many times the messages have been rewritten + bool isEmpty; + myJSONStr returnBetween[MAX_JSON_MESSAGES_SAVED + 2]; + //myJSONStr *returnBetween; + String *stringpassing; + int urgentMessage; + int startIteration; +}; + +#endif \ No newline at end of file diff --git a/embedded/lib/MQTTTasks/MQTTTasksConfiguration.h b/embedded/lib/MQTTTasks/MQTTTasksConfiguration.h new file mode 100644 index 0000000..b503ec1 --- /dev/null +++ b/embedded/lib/MQTTTasks/MQTTTasksConfiguration.h @@ -0,0 +1,13 @@ +#ifndef MQTTTASKSCONFIGURATION_H +#define MQTTTASKSCONFIGURATION_H + +/* + file that contains the default MQTT message save configuration + Note: + +*/ + +#define MAX_JSON_MESSAGES_SAVED 20 // max num of saved JSON items, must be smaller than num of vehicles! +#define MAX_MQTT_TOPIC_DEPTH 5 // how many topics can be in row, e.g. SmartBox/SB1/level are 3 topic levels + +#endif \ No newline at end of file diff --git a/embedded/lib/NetworkManager/NetworkConfiguration.h b/embedded/lib/NetworkManager/NetworkConfiguration.h new file mode 100644 index 0000000..c0e16c0 --- /dev/null +++ b/embedded/lib/NetworkManager/NetworkConfiguration.h @@ -0,0 +1,26 @@ +#ifndef NETWORKCONFIGURATION_H +#define NETWORKCONFIGURATION_H + +/* + file that contains the default network configuration + Note: + DEFAULT_WIFI_* are WLAN Pins from Module on Adafruit Feather M0 (see Adafruit ATWINC1500 Feather) + DEFAULT_MQTT_* are MQTT defaults +*/ + +#define DEFAULT_WIFI_SSID "DigitalLab" // SSID to connect to +#define DEFAULT_WIFI_PASSWORD "digital42HSR" // Password to corresponding SSID +#define DEFAULT_HOSTNAME_SARTBOX "SmartBox" // for SmartBoxes, used for MQTT and WiFi, must be unique in Network +#define DEFAULT_HOSTNAME_VEHICLE "Vehicle" // for Vehicles, used for MQTT and WiFi, must be unique in Network +#define DEFAULT_WIFI_CS 8 // Pins for Adafruit ATWINC1500 Feather +#define DEFAULT_WIFI_IRQ 7 +#define DEFAULT_WIFI_RST 4 +#define DEFAULT_WIFI_EN 2 +#define DEFAULT_MQTT_BROKER_IP1 192 // broker IP distributed in 4 values +#define DEFAULT_MQTT_BROKER_IP2 168 +#define DEFAULT_MQTT_BROKER_IP3 1 +#define DEFAULT_MQTT_BROKER_IP4 7 +#define DEFAULT_MQTT_PORT 1883 // MQTT connection port +#define MAX_JSON_PARSE_SIZE 300 // max buffer size to parse JSON objects, size of the pool in bytes, can be calculated in https://arduinojson.org/v5/assistant/ + +#endif \ No newline at end of file diff --git a/embedded/lib/NetworkManager/NetworkManager.cpp b/embedded/lib/NetworkManager/NetworkManager.cpp new file mode 100644 index 0000000..e6cb10c --- /dev/null +++ b/embedded/lib/NetworkManager/NetworkManager.cpp @@ -0,0 +1,305 @@ +#include "NetworkManager.h" + +NetworkManager::NetworkManager() //Initialize DEFAULT serial & WiFi Module +{ + ssid = DEFAULT_WIFI_SSID; + pass = DEFAULT_WIFI_PASSWORD; + NetManTask_classPointer = &NetManTask; + if (is_vehicle) + { + hostname = DEFAULT_HOSTNAME_VEHICLE + String(random(0xffff), HEX); // Create a random client ID for vehicles + } + else + { + hostname = DEFAULT_HOSTNAME_SARTBOX + String(random(0xffff), HEX); // Create a random client ID for vehicles // set hostname to random ID + } + + WiFi.setPins(DEFAULT_WIFI_CS, DEFAULT_WIFI_IRQ, DEFAULT_WIFI_RST, DEFAULT_WIFI_EN); + + connectToWiFi(); // connect to WiFi + myClient = new WiFiClient; + + ip = WiFi.localIP(); + ssid = WiFi.SSID(); + WiFi.macAddress(mac); + WiFi.BSSID(macRouter); + rssi = WiFi.RSSI(); + encryption = WiFi.encryptionType(); + + IPAddress brokerIP(DEFAULT_MQTT_BROKER_IP1, DEFAULT_MQTT_BROKER_IP2, DEFAULT_MQTT_BROKER_IP3, DEFAULT_MQTT_BROKER_IP4); + this->brokerIP = brokerIP; + mQTT_port = DEFAULT_MQTT_PORT; + + myMQTTclient = new PubSubClient(brokerIP, DEFAULT_MQTT_PORT, callback2, *myClient); + connectToMQTT(); // connecting to MQTT-Broker +} + +NetworkManager::NetworkManager(IPAddress &broker, String *ssid2, String *pass2, byte *mmQTTport, byte pins[4]) //Initialize COSTOM serial & WiFi Module +{ + WiFi.setPins(pins[0], pins[1], pins[2], pins[3]); + ssid = *ssid2; + pass = *pass2; + NetManTask_classPointer = &NetManTask; + if (is_vehicle) + { + hostname = DEFAULT_HOSTNAME_VEHICLE + String(random(0xffff), HEX); // Create a random client ID for vehicles + } + else + { + hostname = DEFAULT_HOSTNAME_SARTBOX + String(random(0xffff), HEX); // Create a random client ID for vehicles // set hostname to random ID + } // set hostname to random ID + + connectToWiFi(); //connect to WiFi + myClient = new WiFiClient; + + ip = WiFi.localIP(); + ssid = WiFi.SSID(); + WiFi.macAddress(mac); + WiFi.BSSID(macRouter); + rssi = WiFi.RSSI(); + encryption = WiFi.encryptionType(); + + this->brokerIP = broker; + this->mQTT_port = *mmQTTport; + + myMQTTclient = new PubSubClient(brokerIP, mQTT_port, callback2, *myClient); + connectToMQTT(); // connecting to MQTT-Broker +} + +void NetworkManager::connectToWiFi() +{ + if (WiFi.status() == WL_NO_SHIELD) // check for the presence of the shield: + { + LOG1("NO WiFi shield present"); + LOG2("WiFi Library could not find WiFi shield. WiFi.status returned " + WiFi.status()); + LOG3("programm is not continuing"); + while (true) + ; // don't continue + } + String wifi_firmware = WiFi.firmwareVersion(); + LOG3("WiFi Firmware Version = " + wifi_firmware); + + while (WiFi.status() != WL_CONNECTED) // connect to Wifi network + { + LOG1("Attempting WLAN connection (WEP)..."); + LOG2("SSID: " + ssid); + if (WiFi.begin(ssid, pass) != WL_CONNECTED) + { + LOG1("WLAN connection failed"); + LOG2("trying again in 3 seconds"); + delay(3000); + } + else + WiFi.hostname(hostname.c_str()); + }; + LOG1("WLAN connected"); + LOG3("Board is connected to the Network"); +} + +void NetworkManager::connectToMQTT() +{ + while (!myMQTTclient->connected()) + { + LOG1("Attempting MQTT connection..."); + LOG2("MQTT Client ID: " + hostname); + if (myMQTTclient->connect(hostname.c_str())) + { + LOG1("MQTT connected"); + LOG2("Variable myMQTT has successfully connected with hostname: " + hostname); + } + else + { + LOG1("MQTT connection failed, error code: " + String(myMQTTclient->state())); + LOG2("trying again in 3 seconds"); + delay(3000); + } + } +} + +bool NetworkManager::publishMessage(const String &topic, const String &msg) // publishes a message to the server +{ + LOG3("try to publish message:" + msg); + if (WiFi.status() != WL_CONNECTED) + connectToWiFi(); + if (myMQTTclient->connected()) + { + myMQTTclient->publish(topic.c_str(), msg.c_str()); + LOG3("Publish to topic [" + topic + "] message:" + msg); + myMQTTclient->loop(); // This should be called regularly to allow the client to process incoming messages and maintain its connection to the server. + return true; + } + else + { + LOG1("MQTT not connected"); + LOG2("You are not connected to the MQTT Broker, publish fails: " + msg); + LOG3("Client ID: " + hostname); + connectToMQTT(); + return false; + }; +} + +bool NetworkManager::subscribe(const String &topic) // subscribes to a new MQTT topic +{ + if (WiFi.status() != WL_CONNECTED) + connectToWiFi(); + if (myMQTTclient->connected()) + { + myMQTTclient->subscribe(topic.c_str()); + LOG3("subscribing to:" + topic); + myMQTTclient->loop(); // This should be called regularly to allow the client to process incoming messages and maintain its connection to the server. + return true; + } + else + { + LOG1("MQTT not connected"); + LOG2("You are not connected to the MQTT Broker, subscription fails: " + topic); + LOG3("Client ID: " + hostname); + connectToMQTT(); + return false; + } +} + +bool NetworkManager::unsubscribe(const String &topic) +{ + if (WiFi.status() != WL_CONNECTED) + connectToWiFi(); + if (myMQTTclient->connected()) + { + myMQTTclient->unsubscribe(topic.c_str()); + myMQTTclient->loop(); // This should be called regularly to allow the client to process incoming messages and maintain its connection to the server. + return true; + } + else + { + LOG1("MQTT not connected"); + LOG2("You are not connected to the MQTT Broker, unsubscribe will fail: " + topic); + LOG3("Client ID: " + hostname); + connectToMQTT(); + return false; + } +} + +void callback2(char *topic, byte *payload, unsigned int length) // listens to incoming messages (published to Server) +{ + + String topic_str; + for (int i = 0; topic[i] != '\0'; i++) // iterate topic to topic_str + { + topic_str += topic[i]; + } + if (LOGLEVELCONFIGURATION > 2) + { + String msg = "Message arrived [" + topic_str + "]: \t message:"; + for (unsigned int i = 0; i < length; i++) // iterate message + { + msg += (char)payload[i]; + } + LOG3(msg); + } + + char inData[MAX_JSON_PARSE_SIZE]; // convert message to JSON object + for (unsigned int i = 0; i < length; i++) + inData[(i)] = (char)payload[i]; + + StaticJsonBuffer jsonBuffer; + JsonObject &my_JSON = jsonBuffer.parseObject(inData); // saves everything to my_JSON JSON Object + if (!my_JSON.success()) + { + LOG1("JSON parsing failed"); + LOG2("my_JSON parsing failed in callback 2"); + return; + } + myJSONStr temp; + temp.hostname = my_JSON.get("hostname"); + temp.level = my_JSON.get("level"); + temp.topic = my_JSON.get("topic"); + temp.request = my_JSON.get("request"); + // MORE TO ADD HERE + + NetManTask.addMessage(temp); // adds to message save + + if (LOGLEVELCONFIGURATION > 2) + { + LOG3("------ new JSON Message in JSarray Array ------"); + my_JSON.prettyPrintTo(Serial); // prints JSON to Serial Monitor, pay attention can output BS + LOG3("\n------ end of JSON Message ------"); + } +} + +void NetworkManager::loop() +{ + if (WiFi.status() != WL_CONNECTED) + connectToWiFi(); + if (!myMQTTclient->connected()) + connectToMQTT(); + myMQTTclient->loop(); +} + +String NetworkManager::getHostName() +{ + return this->hostname; +} + +IPAddress NetworkManager::getIP() +{ + if (WiFi.status() != WL_CONNECTED) + connectToWiFi(); + ip = WiFi.localIP(); + return ip; +} + +void NetworkManager::getInfo() // prints Information to Network +{ + myMQTTclient->loop(); // This should be called regularly to allow the client to process incoming messages and maintain its connection to the server. + ip = WiFi.localIP(); + ssid = WiFi.SSID(); + WiFi.macAddress(mac); + WiFi.BSSID(macRouter); + rssi = WiFi.RSSI(); + encryption = WiFi.encryptionType(); + if (Serial) + { + Serial.println(); + Serial.println(); + Serial.println("-------------------- Information --------------------"); + Serial.print("SSID: "); + Serial.println(ssid); + Serial.print("IP Address: "); + Serial.println(ip); + Serial.print("BSSID (MAC Address from Router): "); + Serial.print(macRouter[5], HEX); + Serial.print(":"); + Serial.print(macRouter[4], HEX); + Serial.print(":"); + Serial.print(macRouter[3], HEX); + Serial.print(":"); + Serial.print(macRouter[2], HEX); + Serial.print(":"); + Serial.print(macRouter[1], HEX); + Serial.print(":"); + Serial.println(macRouter[0], HEX); + Serial.print("MAC Address (Device): "); + Serial.print(mac[5], HEX); + Serial.print(":"); + Serial.print(mac[4], HEX); + Serial.print(":"); + Serial.print(mac[3], HEX); + Serial.print(":"); + Serial.print(mac[2], HEX); + Serial.print(":"); + Serial.print(mac[1], HEX); + Serial.print(":"); + Serial.println(mac[0], HEX); + Serial.print("RSSI (signal strenght in dBm): "); + Serial.println(rssi); + Serial.print("Encryption (WLAN): "); + Serial.println(encryption); + Serial.print("WLAN status (WiFi lib): "); + Serial.println(WiFi.status()); + Serial.println("-------------------- Information END --------------------"); + Serial.println(); + Serial.println(); + } + // Note: MQTT subscriptions and messages not displayed here! + myMQTTclient->loop(); // This should be called regularly to allow the client to process incoming messages and maintain its connection to the server. +} \ No newline at end of file diff --git a/embedded/lib/NetworkManager/NetworkManager.h b/embedded/lib/NetworkManager/NetworkManager.h new file mode 100644 index 0000000..21c5477 --- /dev/null +++ b/embedded/lib/NetworkManager/NetworkManager.h @@ -0,0 +1,56 @@ +#ifndef NETWORKMANAGER_H +#define NETWORKMANAGER_H + +#include +#include +#include +#include +#include +#include +// own files: +#include +#include +#include +#include + +const byte default_pins[4] = {DEFAULT_WIFI_CS, DEFAULT_WIFI_IRQ, DEFAULT_WIFI_RST, DEFAULT_WIFI_EN}; +const bool is_vehicle = true; // true if is vehicle, used for MQTT +static MQTTTasks NetManTask; // saves all messages, saves all incoming messages as JSON Objects, FIFO order, num of items: MAX_JSON_MESSAGES_SAVED + +void callback2(char *topic, byte *payload, unsigned int length); // needs to be outside class! + +class NetworkManager +{ +public: + NetworkManager(); // DEFAULT C'tor + NetworkManager(IPAddress &broker, String *ssid2 = (String *)DEFAULT_WIFI_SSID, String *pass2 = (String *)DEFAULT_WIFI_PASSWORD, byte *mmQTTport = (byte *)DEFAULT_MQTT_PORT, byte pins[4] = (byte *)default_pins); // COSTUM C'tor + bool publishMessage(const String &topic, const String &msg); // handles outgoing MQTT messages to Server + bool unsubscribe(const String &topic); + bool subscribe(const String &topic); // subsribes to MQTT topic on Server + void getInfo(); // prints out some information about that object + void loop(); // make client ready for receiving messages + String getHostName(); // returns hostname of this object + IPAddress getIP(); // return current IP Address + MQTTTasks *NetManTask_classPointer; // used to see saved Messages from outside this file + +private: + void connectToWiFi(); // connects to WiFi based on below stored attributes + void connectToMQTT(); // connects to MQTT Broker based on below stored attributes + // WIFI stuff + String ssid; + String pass; + String hostname; + IPAddress ip; + byte macRouter[6]; + byte mac[6]; + long rssi; + byte encryption; + WiFiServer *myServer; + WiFiClient *myClient; + // MQTT stuff + PubSubClient *myMQTTclient; + IPAddress brokerIP; + int mQTT_port; +}; + +#endif \ No newline at end of file diff --git a/embedded/lib/NetworkManager/NetworkManagerStructs.h b/embedded/lib/NetworkManager/NetworkManagerStructs.h new file mode 100644 index 0000000..0cdd1ef --- /dev/null +++ b/embedded/lib/NetworkManager/NetworkManagerStructs.h @@ -0,0 +1,36 @@ +#ifndef NETWORKMANAGERSTRUCTS_H +#define NETWORKMANAGERSTRUCTS_H + +#include + +/* +used in: + FILE + ----------------------------------------------------- + NetworkManager.h + NetworkManager.cpp + MQTTTasks.h + MQTTTasks.cpp + main.cpp +used for: + saving Messages to use them later on (asynchronous Communication) +*/ + +struct myJSONStr { + String topic = ""; + String hostname = ""; + String request = ""; + int level = -5; + double vehicleParams[5]; +} ; + +enum SBLevel // describes Smart Box level states, -5 is default if not set! +{ + dead = -1, + full = 0, + empty = 1 +}; + + + +#endif \ No newline at end of file diff --git a/embedded/lib/SmartBoxList/SmartBoxList.cpp b/embedded/lib/SmartBoxList/SmartBoxList.cpp new file mode 100644 index 0000000..bcf09ed --- /dev/null +++ b/embedded/lib/SmartBoxList/SmartBoxList.cpp @@ -0,0 +1,55 @@ +#include + +SmartBoxList::SmartBoxList() +{ + iterator = 0; +} + +bool SmartBoxList::push(String &hostname) +{ + if (iterator < MAX_SMARTBOXES_TOLISTENTO) + { + iterator++; + hostnames[iterator] = hostname; + return true; + } + else + return false; +} + +bool SmartBoxList::pop(String &hostname) +{ + if (isInList(hostname)) + { + int index; + for (int i = 0; i < MAX_SMARTBOXES_TOLISTENTO; i++) // find index of searched hostname + { + if (hostnames[i] == hostname) + index = i; + } + for (int i = index; i < MAX_SMARTBOXES_TOLISTENTO - 1; i++) // copy all strings + { + hostnames[i] = hostnames[i + 1]; + } + hostnames[MAX_SMARTBOXES_TOLISTENTO - 1] = NULL; + iterator--; + } + else + return false; +} + +bool SmartBoxList::isInList(String &hostname) +{ + bool returns = false; + for (int i = 0; i < MAX_SMARTBOXES_TOLISTENTO; i++) + { + if (hostnames[i] == hostname) + returns = true; + } + return returns; +} + +int SmartBoxList::getSize() +{ + return iterator; +} diff --git a/embedded/lib/SmartBoxList/SmartBoxList.h b/embedded/lib/SmartBoxList/SmartBoxList.h new file mode 100644 index 0000000..0501ced --- /dev/null +++ b/embedded/lib/SmartBoxList/SmartBoxList.h @@ -0,0 +1,21 @@ +#ifndef SMARTBOXLIST_H +#define SMARTBOXLIST_H + +#include +#include + +class SmartBoxList // stores valuable SmartBoxes +{ +public: + SmartBoxList(); + bool push(String &hostname); // pushes String to list if list not full + bool pop(String &hostname); // pops String from list + bool isInList(String &hostname); // checks if String is in list + int getSize(); // gets size of list + +private: + String hostnames[MAX_SMARTBOXES_TOLISTENTO]; + int iterator; +}; + +#endif diff --git a/embedded/lib/SmartBoxList/SmartBoxList.h.orig b/embedded/lib/SmartBoxList/SmartBoxList.h.orig new file mode 100644 index 0000000..6670d37 --- /dev/null +++ b/embedded/lib/SmartBoxList/SmartBoxList.h.orig @@ -0,0 +1,24 @@ +#ifndef SMARTBOXLIST_H +#define SMARTBOXLIST_H + +#include +<<<<<<< HEAD +======= +#include +>>>>>>> 475470f89eaaf070c0bbb251460f021db20224e1 + +class SmartBoxList // stores valuable SmartBoxes +{ +public: + SmartBoxList(); + bool push(String &hostname); // pushes String to list if list not full + bool pop(String &hostname); // pops String from list + bool isInList(String &hostname); // checks if String is in list + int getSize(); // gets size of list + +private: + String hostnames[MAX_SMARTBOXES_TOLISTENTO]; + int iterator; +}; + +#endif \ No newline at end of file diff --git a/embedded/lib/SmartBoxList/SmartBoxListConfiguration.h b/embedded/lib/SmartBoxList/SmartBoxListConfiguration.h new file mode 100644 index 0000000..4a30fe6 --- /dev/null +++ b/embedded/lib/SmartBoxList/SmartBoxListConfiguration.h @@ -0,0 +1,10 @@ +#ifndef SMARTBOXLISTCONFIGURATION_H +#define SMARTBOXLISTCONFIGURATION_H + + +#define MAX_SMARTBOXES_TOLISTENTO 20 + + + + +#endif \ No newline at end of file diff --git a/embedded/lib/Sonar/Sonar.cpp b/embedded/lib/Sonar/Sonar.cpp index 91ed323..f051b00 100755 --- a/embedded/lib/Sonar/Sonar.cpp +++ b/embedded/lib/Sonar/Sonar.cpp @@ -10,55 +10,83 @@ #include "Arduino.h" #include "Sonar.h" -Sonar::Sonar(int servoPin, int triggerPin, int echoPin, int maxDistance, int min_Error, int max_Error, int min_TurnAngle, int max_TurnAngle) { - if (DEBUGGER == true) Serial.print("Initializing sonar..."); +Sonar::Sonar(int servoPin, int triggerPin, int echoPin, int maxDistance, int min_Error, int max_Error, int min_TurnAngle, int max_TurnAngle) +{ + if (DEBUGGER == true) + Serial.print("Initializing sonar..."); sonar = new NewPing(triggerPin, echoPin, maxDistance); minError = min_Error; maxError = max_Error; minTurnAngle = min_TurnAngle; maxTurnAngle = max_TurnAngle; _servoPin = servoPin; - if (DEBUGGER == true) Serial.println(" complete!"); + if (DEBUGGER == true) + Serial.println(" complete!"); } -void Sonar::loop(SonarState *state, int directionError, bool servoActive) { - if ((servoActive == true) && (state->isAttached == false)) { +void Sonar::loop(SonarState *state, int directionError, bool servoActive) +{ + if ((servoActive == true) && (state->isAttached == false)) + { sonarServo.attach(_servoPin); state->isAttached = true; } int uSonar = sonar->ping_cm(); - if (DEBUGGER == true) Serial.print("distance: "); if (DEBUGGER == true) Serial.println(uSonar); + if (DEBUGGER == true) + Serial.print("distance: "); + if (DEBUGGER == true) + Serial.println(uSonar); state->obstacleDistance = uSonar; - if (servoActive == true) { - if (DEBUGGER == true) Serial.print("Turn sonar with directionError: "); if (DEBUGGER == true) Serial.println(directionError); - turnSonar(directionError); + if (servoActive == true) + { + if (DEBUGGER == true) + Serial.print("Turn sonar with directionError: "); + if (DEBUGGER == true) + Serial.println(directionError); + turnSonar(directionError); } calculateSonarFactor(state); - if (state->detachServo == true) { + if (state->detachServo == true) + { sonarServo.detach(); state->detachServo = false; state->isAttached = false; } } -void Sonar::turnSonar(int directionError) { +int readSonar() +{ + return sonar->ping_cm(); +} + +void Sonar::turnSonar(int directionError) +{ int turnSonar = map(directionError, minError, maxError, minTurnAngle, maxTurnAngle); - if (DEBUGGER == true) Serial.print("turnanglesonar: "); if (DEBUGGER == true) Serial.println(turnSonar); + if (DEBUGGER == true) + Serial.print("turnanglesonar: "); + if (DEBUGGER == true) + Serial.println(turnSonar); sonarServo.write(turnSonar); - } -void Sonar::calculateSonarFactor(SonarState *state) { - if ((state->obstacleDistance > 10) || (state->obstacleDistance == 0)) { +void Sonar::calculateSonarFactor(SonarState *state) +{ + if ((state->obstacleDistance > 10) || (state->obstacleDistance == 0)) + { state->sonarFactor = 1; - if (DEBUGGER == true) Serial.println("Factor1"); + if (DEBUGGER == true) + Serial.println("Factor1"); } - else if ((state->obstacleDistance <= 10) && (state->obstacleDistance >= 5)) { + else if ((state->obstacleDistance <= 10) && (state->obstacleDistance >= 5)) + { state->sonarFactor = 0.5; - if (DEBUGGER == true) Serial.println("Factor0.5"); + if (DEBUGGER == true) + Serial.println("Factor0.5"); } - else { + else + { state->sonarFactor = 0; - if (DEBUGGER == true) Serial.println("Factor0"); + if (DEBUGGER == true) + Serial.println("Factor0"); } } diff --git a/embedded/lib/Sonar/Sonar.h b/embedded/lib/Sonar/Sonar.h index 5b710c4..54a34b4 100755 --- a/embedded/lib/Sonar/Sonar.h +++ b/embedded/lib/Sonar/Sonar.h @@ -30,6 +30,7 @@ class Sonar : public Component void loop(SonarState *state, int directionError, bool servoActive); void turnSonar(int directionError); void calculateSonarFactor(SonarState *state); + int readSonar(); protected: NewPing *sonar; diff --git a/embedded/lib/Vision/PixyCam_Lib/Pixy.h b/embedded/lib/Vision/PixyCam_Lib/Pixy.h new file mode 100644 index 0000000..9be72ca --- /dev/null +++ b/embedded/lib/Vision/PixyCam_Lib/Pixy.h @@ -0,0 +1,169 @@ +// +// begin license header +// +// This file is part of Pixy CMUcam5 or "Pixy" for short +// +// All Pixy source code is provided under the terms of the +// GNU General Public License v2 (http://www.gnu.org/licenses/gpl-2.0.html). +// Those wishing to use Pixy source code, software and/or +// technologies under different licensing terms should contact us at +// cmucam@cs.cmu.edu. Such licensing terms are available for +// all portions of the Pixy codebase presented here. +// +// end license header +// +// This file is for defining the SPI-related classes. It's called Pixy.h instead +// of Pixy_SPI.h because it's the default/recommended communication method +// with Arduino. This class assumes you are using the ICSP connector to talk to +// Pixy from your Arduino. For more information go to: +// +//http://cmucam.org/projects/cmucam5/wiki/Hooking_up_Pixy_to_a_Microcontroller_(like_an_Arduino) +// + +#ifndef PIXY_H +#define PIXY_H + +#include "TPixy.h" +#include "SPI.h" + + +#define PIXY_SYNC_BYTE 0x5a +#define PIXY_SYNC_BYTE_DATA 0x5b +#define PIXY_BUF_SIZE 16 + +template struct CircularQ +{ + CircularQ() + { + len = 0; + writeIndex = 0; + readIndex = 0; + } + + bool read(BufType *c) + { + if (len) + { + *c = buf[readIndex++]; + len--; + if (readIndex==PIXY_BUF_SIZE) + readIndex = 0; + return true; + } + else + return false; + } + + uint8_t freeLen() + { + return PIXY_BUF_SIZE-len; + } + + bool write(BufType c) + { + if (freeLen()==0) + return false; + + buf[writeIndex++] = c; + len++; + if (writeIndex==PIXY_BUF_SIZE) + writeIndex = 0; + return true; + } + + BufType buf[PIXY_BUF_SIZE]; + uint8_t len; + uint8_t writeIndex; + uint8_t readIndex; +}; + +class LinkSPI +{ + public: + void init() + { + SPI.begin(); + + #ifdef __SAM3X8E__ + // DUE clock divider // + SPI.setClockDivider(84); + #else + // Default clock divider // + SPI.setClockDivider(SPI_CLOCK_DIV16); + #endif + } + + uint16_t getWord() + { + // ordering is different (big endian) because Pixy is sending 16 bits through SPI + // instead of 2 bytes in a 16-bit word as with I2C + uint16_t w; + + if (inQ.read(&w)) + return w; + + return getWordHw(); + } + + uint8_t getByte() + { + return SPI.transfer(0x00); + } + + int8_t send(uint8_t *data, uint8_t len) + { + int i; + + // check to see if we have enough space in our circular queue + if (outQ.freeLen() outQ; + CircularQ inQ; +}; + + +typedef TPixy Pixy; + +#endif diff --git a/embedded/lib/Vision/PixyCam_Lib/PixyI2C.h b/embedded/lib/Vision/PixyCam_Lib/PixyI2C.h new file mode 100644 index 0000000..6fb85f0 --- /dev/null +++ b/embedded/lib/Vision/PixyCam_Lib/PixyI2C.h @@ -0,0 +1,78 @@ +// +// begin license header +// +// This file is part of Pixy CMUcam5 or "Pixy" for short +// +// All Pixy source code is provided under the terms of the +// GNU General Public License v2 (http://www.gnu.org/licenses/gpl-2.0.html). +// Those wishing to use Pixy source code, software and/or +// technologies under different licensing terms should contact us at +// cmucam@cs.cmu.edu. Such licensing terms are available for +// all portions of the Pixy codebase presented here. +// +// end license header +// +// This file is for defining the link class for I2C communications. +// +// Note, the PixyI2C class takes an optional argument, which is the I2C address +// of the Pixy you want to talk to. The default address is 0x54 (used when no +// argument is used.) So, for example, if you wished to talk to Pixy at I2C +// address 0x55, declare like this: +// +// PixyI2C pixy(0x55); +// + +#ifndef _PIXYI2C_H +#define _PIXYI2C_H + +#include "TPixy.h" +#include "Wire.h" + +#define PIXY_I2C_DEFAULT_ADDR 0x54 + +class LinkI2C +{ +public: + void init() + { + Wire.begin(); + } + void setArg(uint16_t arg) + { + if (arg==PIXY_DEFAULT_ARGVAL) + addr = PIXY_I2C_DEFAULT_ADDR; + else + addr = arg; + } + uint16_t getWord() + { + uint16_t w; + uint8_t c; + Wire.requestFrom((int)addr, 2); + c = Wire.read(); + w = Wire.read(); + w <<= 8; + w |= c; + return w; + } + uint8_t getByte() + { + Wire.requestFrom((int)addr, 1); + return Wire.read(); + } + + int8_t send(uint8_t *data, uint8_t len) + { + Wire.beginTransmission(addr); + Wire.write(data, len); + Wire.endTransmission(); + return len; + } + +private: + uint8_t addr; +}; + +typedef TPixy PixyI2C; + +#endif diff --git a/embedded/lib/Vision/PixyCam_Lib/PixySPI_SS.h b/embedded/lib/Vision/PixyCam_Lib/PixySPI_SS.h new file mode 100644 index 0000000..850400d --- /dev/null +++ b/embedded/lib/Vision/PixyCam_Lib/PixySPI_SS.h @@ -0,0 +1,127 @@ +// +// begin license header +// +// This file is part of Pixy CMUcam5 or "Pixy" for short +// +// All Pixy source code is provided under the terms of the +// GNU General Public License v2 (http://www.gnu.org/licenses/gpl-2.0.html). +// Those wishing to use Pixy source code, software and/or +// technologies under different licensing terms should contact us at +// cmucam@cs.cmu.edu. Such licensing terms are available for +// all portions of the Pixy codebase presented here. +// +// end license header +// +// This file is for defining the link class for SPI with Slave Select. The +// default communication for Arduino is through the ICSP connector, which uses +// SPI without a slave select. The LinkSPI_SS allows you to use a slave select +// so you can share the SPI port with other devices, or use multiple Pixys. +// +// Note, the PixySPI_SS class takes an optional argument, which is the pin +// number of the slave select signal you wish to use. The default pin is the +// SS pin (used when no argument is used.) So, for example, if you wished to +// use pin 14 for slave select, declare like this: +// +// PixySPI_SS pixy(14); +// + +#ifndef PIXYSPI_SS_H +#define PIXYSPI_SS_H + +#include "TPixy.h" +#include "SPI.h" + + +#define PIXY_SYNC_BYTE 0x5a +#define PIXY_SYNC_BYTE_DATA 0x5b +#define PIXY_OUTBUF_SIZE 6 + +class LinkSPI_SS +{ + public: + void init() + { + outLen = 0; + SPI.begin(); + + #ifdef __SAM3X8E__ + // DUE clock divider // + SPI.setClockDivider(84); + #else + // Default clock divider // + SPI.setClockDivider(SPI_CLOCK_DIV16); + #endif + } + + uint16_t getWord() + { + // ordering is different because Pixy is sending 16 bits through SPI + // instead of 2 bytes in a 16-bit word as with I2C + uint16_t w; + uint8_t c, cout = 0; + + // assert slave select + digitalWrite(ssPin, LOW); + + if (outLen) + { + w = SPI.transfer(PIXY_SYNC_BYTE_DATA); + cout = outBuf[outIndex++]; + if (outIndex==outLen) + outLen = 0; + } + else + w = SPI.transfer(0); + + w <<= 8; + c = SPI.transfer(cout); + w |= c; + + // negate slave select + digitalWrite(ssPin, HIGH); + return w; + } + + uint8_t getByte() // this shouldn't be called normally + // It should only be called if we get out of sync, but with slave select + // we should stay in sync + { + uint8_t c; + // assert slave select + digitalWrite(ssPin, LOW); + c = SPI.transfer(0x00); + // negate slave select + digitalWrite(ssPin, HIGH); + + return c; + } + + int8_t send(uint8_t *data, uint8_t len) + { + if (len>PIXY_OUTBUF_SIZE || outLen!=0) + return -1; + memcpy(outBuf, data, len); + outLen = len; + outIndex = 0; + return len; + } + + void setArg(uint16_t arg) + { + if (arg==PIXY_DEFAULT_ARGVAL) + ssPin = SS; // default slave select pin + else + ssPin = arg; + } + + private: + uint8_t outBuf[PIXY_OUTBUF_SIZE]; + uint8_t outLen; + uint8_t outIndex; + uint16_t ssPin; +}; + + +typedef TPixy PixySPI_SS; + +#endif diff --git a/embedded/lib/Vision/PixyCam_Lib/PixyUART.h b/embedded/lib/Vision/PixyCam_Lib/PixyUART.h new file mode 100644 index 0000000..8a4f324 --- /dev/null +++ b/embedded/lib/Vision/PixyCam_Lib/PixyUART.h @@ -0,0 +1,74 @@ +// +// begin license header +// +// This file is part of Pixy CMUcam5 or "Pixy" for short +// +// All Pixy source code is provided under the terms of the +// GNU General Public License v2 (http://www.gnu.org/licenses/gpl-2.0.html). +// Those wishing to use Pixy source code, software and/or +// technologies under different licensing terms should contact us at +// cmucam@cs.cmu.edu. Such licensing terms are available for +// all portions of the Pixy codebase presented here. +// +// end license header +// +// This file is for defining the link class for UART communications. +// + +#ifndef _PIXYUART_H +#define _PIXYUART_H + +#include "TPixy.h" +#include "Arduino.h" + +class LinkUART +{ +public: + void init() + { + Serial1.begin(19200); + } + void setArg(uint16_t arg) + { + } + uint16_t getWord() + { + int16_t u, v; + + while(1) + { + u = Serial1.read(); + if (u>=0) + break; + } + while(1) + { + v = Serial1.read(); + if (v>=0) + break; + } + v <<= 8; + v |= u&0xff; + return v; + } + uint8_t getByte() + { + int16_t u; + + while(1) + { + u = Serial1.read(); + if (u>=0) + break; + } + return (uint8_t)u; + } + int8_t send(uint8_t *data, uint8_t len) + { + return Serial1.write(data, len); + } +}; + +typedef TPixy PixyUART; + +#endif diff --git a/embedded/lib/Vision/PixyCam_Lib/TPixy.h b/embedded/lib/Vision/PixyCam_Lib/TPixy.h new file mode 100644 index 0000000..9668ab1 --- /dev/null +++ b/embedded/lib/Vision/PixyCam_Lib/TPixy.h @@ -0,0 +1,273 @@ +// +// begin license header +// +// This file is part of Pixy CMUcam5 or "Pixy" for short +// +// All Pixy source code is provided under the terms of the +// GNU General Public License v2 (http://www.gnu.org/licenses/gpl-2.0.html). +// Those wishing to use Pixy source code, software and/or +// technologies under different licensing terms should contact us at +// cmucam@cs.cmu.edu. Such licensing terms are available for +// all portions of the Pixy codebase presented here. +// +// end license header +// +// This file is for defining the Block struct and the Pixy template class. +// (TPixy). TPixy takes a communication link as a template parameter so that +// all communication modes (SPI, I2C and UART) can share the same code. +// + +#ifndef _TPIXY_H +#define _TPIXY_H + +#include "Arduino.h" + +// Communication/misc parameters +#define PIXY_INITIAL_ARRAYSIZE 30 +#define PIXY_MAXIMUM_ARRAYSIZE 130 +#define PIXY_START_WORD 0xaa55 +#define PIXY_START_WORD_CC 0xaa56 +#define PIXY_START_WORDX 0x55aa +#define PIXY_MAX_SIGNATURE 7 +#define PIXY_DEFAULT_ARGVAL 0xffff + +// Pixy x-y position values +#define PIXY_MIN_X 0L +#define PIXY_MAX_X 319L +#define PIXY_MIN_Y 0L +#define PIXY_MAX_Y 199L + +// RC-servo values +#define PIXY_RCS_MIN_POS 0L +#define PIXY_RCS_MAX_POS 1000L +#define PIXY_RCS_CENTER_POS ((PIXY_RCS_MAX_POS-PIXY_RCS_MIN_POS)/2) + + +enum BlockType +{ + NORMAL_BLOCK, + CC_BLOCK +}; + +struct Block +{ + // print block structure! + void print() + { + int i, j; + char buf[128], sig[6], d; + bool flag; + if (signature>PIXY_MAX_SIGNATURE) // color code! (CC) + { + // convert signature number to an octal string + for (i=12, j=0, flag=false; i>=0; i-=3) + { + d = (signature>>i)&0x07; + if (d>0 && !flag) + flag = true; + if (flag) + sig[j++] = d + '0'; + } + sig[j] = '\0'; + sprintf(buf, "CC block! sig: %s (%d decimal) x: %d y: %d width: %d height: %d angle %d\n", sig, signature, x, y, width, height, angle); + } + else // regular block. Note, angle is always zero, so no need to print + sprintf(buf, "sig: %d x: %d y: %d width: %d height: %d\n", signature, x, y, width, height); + Serial.print(buf); + } + uint16_t signature; + uint16_t x; + uint16_t y; + uint16_t width; + uint16_t height; + uint16_t angle; +}; + + + +template class TPixy +{ +public: + TPixy(uint16_t arg=PIXY_DEFAULT_ARGVAL); + ~TPixy(); + + uint16_t getBlocks(uint16_t maxBlocks=1000); + int8_t setServos(uint16_t s0, uint16_t s1); + int8_t setBrightness(uint8_t brightness); + int8_t setLED(uint8_t r, uint8_t g, uint8_t b); + void init(); + + Block *blocks; + +private: + boolean getStart(); + void resize(); + + LinkType link; + boolean skipStart; + BlockType blockType; + uint16_t blockCount; + uint16_t blockArraySize; +}; + + +template TPixy::TPixy(uint16_t arg) +{ + skipStart = false; + blockCount = 0; + blockArraySize = PIXY_INITIAL_ARRAYSIZE; + blocks = (Block *)malloc(sizeof(Block)*blockArraySize); + link.setArg(arg); +} + +template void TPixy::init() +{ + link.init(); +} + +template TPixy::~TPixy() +{ + free(blocks); +} + +template boolean TPixy::getStart() +{ + uint16_t w, lastw; + + lastw = 0xffff; + + while(true) + { + w = link.getWord(); + if (w==0 && lastw==0) + { + delayMicroseconds(10); + return false; + } + else if (w==PIXY_START_WORD && lastw==PIXY_START_WORD) + { + blockType = NORMAL_BLOCK; + return true; + } + else if (w==PIXY_START_WORD_CC && lastw==PIXY_START_WORD) + { + blockType = CC_BLOCK; + return true; + } + else if (w==PIXY_START_WORDX) + { + Serial.println("reorder"); + link.getByte(); // resync + } + lastw = w; + } +} + +template void TPixy::resize() +{ + blockArraySize += PIXY_INITIAL_ARRAYSIZE; + blocks = (Block *)realloc(blocks, sizeof(Block)*blockArraySize); +} + +template uint16_t TPixy::getBlocks(uint16_t maxBlocks) +{ + uint8_t i; + uint16_t w, checksum, sum; + Block *block; + + if (!skipStart) + { + if (getStart()==false) + return 0; + } + else + skipStart = false; + + for(blockCount=0; blockCountblockArraySize) + resize(); + + block = blocks + blockCount; + + for (i=0, sum=0; i=5) // skip + { + block->angle = 0; + break; + } + w = link.getWord(); + sum += w; + *((uint16_t *)block + i) = w; + } + + if (checksum==sum) + blockCount++; + else + Serial.println("cs error"); + + w = link.getWord(); + if (w==PIXY_START_WORD) + blockType = NORMAL_BLOCK; + else if (w==PIXY_START_WORD_CC) + blockType = CC_BLOCK; + else + return blockCount; + } +} + +template int8_t TPixy::setServos(uint16_t s0, uint16_t s1) +{ + uint8_t outBuf[6]; + + outBuf[0] = 0x00; + outBuf[1] = 0xff; + *(uint16_t *)(outBuf + 2) = s0; + *(uint16_t *)(outBuf + 4) = s1; + + return link.send(outBuf, 6); +} + +template int8_t TPixy::setBrightness(uint8_t brightness) +{ + uint8_t outBuf[3]; + + outBuf[0] = 0x00; + outBuf[1] = 0xfe; + outBuf[2] = brightness; + + return link.send(outBuf, 3); +} + +template int8_t TPixy::setLED(uint8_t r, uint8_t g, uint8_t b) +{ + uint8_t outBuf[5]; + + outBuf[0] = 0x00; + outBuf[1] = 0xfd; + outBuf[2] = r; + outBuf[3] = g; + outBuf[4] = b; + + return link.send(outBuf, 5); +} + +#endif diff --git a/embedded/lib/Vision/Vision.h b/embedded/lib/Vision/Vision.h index 3fa000b..0a6622f 100755 --- a/embedded/lib/Vision/Vision.h +++ b/embedded/lib/Vision/Vision.h @@ -13,7 +13,7 @@ #include "Modular.h" #include "Servo.h" #include "Wire.h" -#include "PixyI2C.h" +#include "PixyCam_Lib/PixyI2C.h" #include "Configuration.h" struct VisionState { diff --git a/embedded/platformio.ini b/embedded/platformio.ini index cc78fdb..6c94f10 100755 --- a/embedded/platformio.ini +++ b/embedded/platformio.ini @@ -21,3 +21,11 @@ lib_deps = NewPing ArduinoJson PixyI2C + NetworkManager + SmartBoxList + Sonar + Hoist + Chassis + Vision + MQTTTasks +lib_extra_dirs = /lib/Vision/PixyCam_Lib \ No newline at end of file diff --git a/embedded/src/main.cpp b/embedded/src/main.cpp index ffb4fe8..107f45e 100755 --- a/embedded/src/main.cpp +++ b/embedded/src/main.cpp @@ -1,779 +1,211 @@ -/* - Main programm to run the transport vehicle - Created by Glenn Huber 05.05.2018 -*/ -#include "Arduino.h" -#include "VehicleWebAPI.h" -#include "SPI.h" -#include "Sonar.h" -#include "Chassis.h" -#include "Hoist.h" -#include "Vision.h" - -struct VehicleState { - ChassisState chassis; - SonarState sonar; - VisionState vision; - HoistState hoist; - ApiState api; - float curveFactor = 0; - int turnCount = 1; - int waitCount = 0; - int sonarCount = 0; - unsigned long timeStart = 0; - bool turnSonar = true; - bool waitForTurn = false; - bool visual = false; - String side = ""; -} state; - -Sonar *vehicleSonar; -Vision *vehicleVision; -Hoist *vehicleHoist; -Chassis *vehicleChassis; -VehicleWebAPI *vehicleAPI; - -void strategy() { - if (DEBUGGER == true) Serial.print("Sector begin strategy"); if (DEBUGGER == true) Serial.println(state.api.sector); - if (DEBUGGER == true) Serial.print("VehicleCount: "); if (DEBUGGER == true) Serial.println(state.api.vehicleCount); - if (DEBUGGER == true) Serial.print("JobCount: "); if (DEBUGGER == true) Serial.println(state.api.jobCount); - if (DEBUGGER == true) Serial.print("otherTask: "); if (DEBUGGER == true) Serial.println(state.api.otherTask); - if (DEBUGGER == true) Serial.print("othersector: "); if (DEBUGGER == true) Serial.println(state.api.otherSector); - if (DEBUGGER == true) Serial.print("Container: "); if (DEBUGGER == true) Serial.println(state.api.container); - if (DEBUGGER == true) Serial.print("port: "); if (DEBUGGER == true) Serial.println(state.api.unloadPort); - - //After Task is finished, change the task - if ((state.api.state == "resting") && (state.api.task != "finished")) { - state.api.task = "finished"; - } - //Sending the state if change happend - if (state.api.taskChange == true) { - if (DEBUGGER == true) Serial.println("Sending task"); - vehicleAPI->sendTask(); - if ((state.api.vehicleID == "Idefix") && (state.api.vehicleCount == 2)) { - if (DEBUGGER == true) Serial.println("send obelix task"); - vehicleAPI->sendTaskToObelix(); - } - if ((state.api.vehicleID == "Obelix") && (state.api.vehicleCount == 2)) { - if (DEBUGGER == true) Serial.println("send idefix task"); - vehicleAPI->sendTaskToIdefix(); - } - state.api.taskChange = false; - } - if (state.api.sectorChange == true) { - if (DEBUGGER == true) Serial.println("Sending sector"); - vehicleAPI->sendSector(); - if ((state.api.vehicleID == "Idefix") && (state.api.vehicleCount == 2) && ((state.api.sector == "20") || (state.api.sector == "24") || (state.api.sector == "31") || (state.api.sector == "40"))) { - if (DEBUGGER == true) Serial.println("send obelix sector"); - vehicleAPI->sendSectorToObelix(); - } - if ((state.api.vehicleID == "Obelix") && (state.api.vehicleCount == 2) && ((state.api.sector == "20") || (state.api.sector == "24") || (state.api.sector == "31") || (state.api.sector == "40"))) { - if (DEBUGGER == true) Serial.println("send idefix sector"); - vehicleAPI->sendSectorToIdefix(); - } - state.api.sectorChange = false; - } - if (state.api.stateChange == true) { - if (DEBUGGER == true) Serial.println("Sending state"); - vehicleAPI->sendState(); - state.api.stateChange = false; - } - - //Sectorstrategy the workState is changed, dependend on the position of the other vehicle - if (((state.api.sector == "10") || (state.api.sector == "11")) && (state.api.otherSector == "20")) { - state.api.workState = false; - state.api.workStateChange = true; - } - else if ((state.api.sector == "20") && (state.api.otherSector == "24")) { - state.api.workState = false; - state.api.workStateChange = true; - } - else if ((state.api.sector == "31") && (state.api.otherSector == "31")) { - state.api.workState = false; - state.api.workStateChange = true; - } - //else if ((state.api.sector == "20") && (state.api.otherSector == "22")) { - // state.api.workState = false; - // state.api.workStateChange = true; - //} - //else if ((state.api.sector == "30") && (state.api.otherSector == "32")) { - // state.api.workState = false; - // state.api.workStateChange = true; - //} - //else if ((state.api.sector == "40") && (state.api.otherSector == "20") && (state.api.vehicleID == "Idefix")) { - // state.api.workState = false; - // state.api.workStateChange = true; - //} - else if (((state.api.sector == "10") || (state.api.sector == "11")) && (state.api.otherSector == "24")) { - state.api.workState = true; - state.api.workStateChange = true; - } - else if ((state.api.sector == "20") && (state.api.otherSector == "31")) { - state.api.workState = true; - state.api.workStateChange = true; - } - else if ((state.api.sector == "31") && (state.api.otherSector == "40")) { - state.api.workState = true; - state.api.workStateChange = true; - } - //else if ((state.api.sector == "20") && ((state.api.otherSector == "10") || (state.api.otherSector == "22") || (state.api.otherSector == "32") || (state.api.otherSector == "40"))) { - // state.api.workState = true; - // state.api.workStateChange = true; - //} - //else if ((state.api.sector == "30") && ((state.api.otherSector == "10") || (state.api.otherSector == "20") || (state.api.otherSector == "22") || (state.api.otherSector == "40"))) { - // state.api.workState = true; - // state.api.workStateChange = true; - //} - //else if ((state.api.sector == "40") && ((state.api.otherSector == "10") || (state.api.otherSector == "20") || (state.api.otherSector == "32") || (state.api.otherSector == "40")) && (state.api.vehicleID == "Idefix")) { - // state.api.workState = true; - // state.api.workStateChange = true; - //} - //Jobstrategy - if ((state.api.container == "yellow") && (state.api.unloadPort == "1") && (state.api.task != "RED_TO_PORT1") && (state.api.task != "RED_TO_PORT2") && (state.api.task != "YELLOW_TO_PORT2") && (state.api.task != "YELLOW_TO_PORT1") && (state.api.otherTask != "YELLOW_TO_PORT1")) { - state.api.task = "YELLOW_TO_PORT1"; - state.api.state = "Starting_Task"; - state.api.workState = true; - state.api.workStateChange = true; - state.vision.target = 2; - } - else if ((state.api.container == "red") && (state.api.unloadPort == "1") && (state.api.task != "RED_TO_PORT1") && (state.api.task != "RED_TO_PORT2") && (state.api.task != "YELLOW_TO_PORT2") && (state.api.task != "YELLOW_TO_PORT1") && (state.api.otherTask != "RED_TO_PORT1")) { - state.api.task = "RED_TO_PORT1"; - state.api.state = "Starting_Task"; - state.api.workState = true; - state.api.workStateChange = true; - state.vision.target = 1; - } - else if ((state.api.container == "yellow") && (state.api.unloadPort == "2") && (state.api.task != "RED_TO_PORT1") && (state.api.task != "RED_TO_PORT2") && (state.api.task != "YELLOW_TO_PORT2") && (state.api.task != "YELLOW_TO_PORT1") && (state.api.otherTask != "YELLOW_TO_PORT2")) { - state.api.task = "YELLOW_TO_PORT2"; - state.api.state = "Starting_Task"; - state.api.workState = true; - state.api.workStateChange = true; - state.vision.target = 2; - } - else if ((state.api.container == "red") && (state.api.unloadPort == "2") && (state.api.task != "RED_TO_PORT1") && (state.api.task != "RED_TO_PORT2") && (state.api.task != "YELLOW_TO_PORT2") && (state.api.task != "YELLOW_TO_PORT1") && (state.api.otherTask != "RED_TO_PORT2")) { - state.api.task = "RED_TO_PORT2"; - state.api.state = "Starting_Task"; - state.api.workState = true; - state.api.workStateChange = true; - state.vision.target = 1; - } - else { - if (DEBUGGER == true) Serial.println("Container change in else"); - state.api.container = ""; - state.api.containerChange = true; - } - - //Drive strategy without an active workstate, there is no sectorhandling - if (state.api.workState == true) { - - //Navigation strategy - if (DEBUGGER == true) Serial.print("IR-status: "); if (DEBUGGER == true) Serial.println(state.chassis.IR); - if (DEBUGGER == true) Serial.print("curveFactor before pid: "); - if (DEBUGGER == true) Serial.println(state.curveFactor); - state.curveFactor = state.chassis.PidValue; - - //Sector 10 movement - if ((state.api.sector == "10") && (state.api.task != "nothing") && (state.api.task != "finished")) { - if (DEBUGGER == true) Serial.println("Sector 10"); - if (state.chassis.direction != "right") { - delay(1000); - } - state.chassis.direction = "right"; - vehicleChassis->motorControl(state.sonar.sonarFactor); - if (state.chassis.actionDone == true) { - if (DEBUGGER == true) Serial.print("Sector 10 vehicleID"); if (DEBUGGER == true) Serial.println(state.api.vehicleID); - if (state.api.vehicleID == "Obelix") { - state.api.sector = "11"; - if (DEBUGGER == true) Serial.println("Sector 10 order to Obelix"); - state.chassis.direction = "straight"; - } - else if (state.api.vehicleID == "Idefix") { - state.api.sector = "12"; - if (DEBUGGER == true) Serial.println("Sector 10 order to Idefix"); - state.chassis.direction = "stop"; - } - if (DEBUGGER == true) Serial.println("end of scector10"); - state.chassis.actionDone = false; - if (DEBUGGER == true) Serial.print("Sector 10 next sector: "); if (DEBUGGER == true) Serial.println(state.api.sector); - } - } - if (DEBUGGER == true) Serial.print("Between 10 and 11 sector: "); if (DEBUGGER == true) Serial.println(state.api.sector); - //Sector 11 movement - if (state.api.sector == "11") { - if (DEBUGGER == true) Serial.println("Sector 11"); - vehicleChassis->motorControl(state.sonar.sonarFactor); - if (state.chassis.actionDone == true) { - state.api.sector = "12"; - state.chassis.direction = "stop"; - if (DEBUGGER == true) Serial.println("end of sector11"); - state.chassis.actionDone = false; - } - } - - //Sector 12 movement - if (state.api.sector == "12") { - if (DEBUGGER == true) Serial.println("Sector 12"); - vehicleChassis->motorControl(state.sonar.sonarFactor); - if (state.chassis.actionDone == true) { - if (DEBUGGER == true) Serial.println("End of 12"); - state.api.sector = "20"; - state.chassis.actionDone = false; - vehicleChassis->stop(); - state.chassis.direction = "straight"; - state.api.state = "Searching_box"; - state.visual = true; - state.turnSonar = false; - state.sonar.detachServo = true; - } - } - - //Sector 20 movement - if (state.api.sector == "20") { - if (DEBUGGER == true) Serial.println("start of sector20"); - - if (((state.chassis.sensor[0] + state.chassis.sensor[1] + state.chassis.sensor[2] + state.chassis.sensor[3] + state.chassis.sensor[4]) == 0) || ((state.chassis.sensor[0] + state.chassis.sensor[1] + state.chassis.sensor[2] + state.chassis.sensor[3] + state.chassis.sensor[4]) == 5)) { - if ((state.waitForTurn == false) && (state.waitCount == 0)) { - if (DEBUGGER == true) Serial.println("changing wait for turn"); - state.waitForTurn = true; - } - } - if (DEBUGGER == true) Serial.print("Targetdetected: "); if (DEBUGGER == true) Serial.println(state.vision.targetDetected); - if ((state.vision.targetDetected == true) && (state.turnCount < 4)) { - //First move dependend on which side the box is on - if (DEBUGGER == true) Serial.println("before first turn"); - - //Is the Box on the right side of the vehicle turn right - if (state.vision.servoAngle < 90) { - if (DEBUGGER == true) Serial.print("waitForturn: "); if (DEBUGGER == true) Serial.println(state.waitForTurn); - if (DEBUGGER == true) Serial.print("TurnCount: "); if (DEBUGGER == true) Serial.println(state.turnCount); - if ((state.waitForTurn == true) && (state.turnCount ==1)) { - - if (state.chassis.actionDone == true) { - state.turnCount++; - if (DEBUGGER == true) Serial.println("right turn accomplished"); - state.chassis.actionDone = false; - } - - if (state.waitCount == 0) { - if (DEBUGGER == true) Serial.println("starting timer"); - state.timeStart = millis(); - state.waitCount++; - } - - if (state.turnCount == 1) { - vehicleChassis->turnRightLimited(2800, state.timeStart); - state.api.state = "Picking_up_right_box"; - state.side = "right"; - if (DEBUGGER == true) Serial.println("turnRightLimited"); - state.visual = false; - state.turnSonar = true; - //vehicleVision->turnVision(90); - //if (DEBUGGER == true) Serial.println("after turnVision"); - } - } - - //Second move is per default a drive straight for adjustment - if (DEBUGGER == true) Serial.println("before second turn"); - if (state.turnCount == 2) { - if (state.chassis.actionDone == true) { - state.turnCount++; - if (DEBUGGER == true) Serial.println("drive straight accomplished"); - state.chassis.actionDone = false; - } - - if (DEBUGGER == true) Serial.println("turncount2"); - if (state.waitCount == 1) { - state.timeStart = millis(); - if (DEBUGGER == true) Serial.println("starting second timer"); - state.waitCount++; - } - - if (state.turnCount == 2) { - vehicleChassis->driveStraightLimited(2800, state.timeStart); - if (DEBUGGER == true) Serial.println("drivestraight"); - } - } - //Third move is a counterturn dependend on the first move - if (DEBUGGER == true) Serial.println("before third turn"); - if (state.turnCount == 3) { - if (DEBUGGER == true) Serial.println("turncount3"); - if (state.chassis.actionDone == true) { - state.turnCount++; - if (DEBUGGER == true) Serial.println("right turn accomplished"); - state.chassis.actionDone = false; - state.visual = true; - state.turnSonar = false; - state.sonar.detachServo = true; - } - - if (state.waitCount == 2) { - state.timeStart = millis(); - if (DEBUGGER == true) Serial.println("starting third timer"); - state.waitCount++; - } - - if (state.turnCount == 3) { - vehicleChassis->turnLeftLimited(2600, state.timeStart); - if (DEBUGGER == true) Serial.println("turnLeftLimited"); - state.waitForTurn = false; - } - } - } - //Is the Box on the left side of the vehicle turn left - if (state.vision.servoAngle > 90) { - if (DEBUGGER == true) Serial.print("waitForturn: "); if (DEBUGGER == true) Serial.println(state.waitForTurn); - if (DEBUGGER == true) Serial.print("TurnCount: "); if (DEBUGGER == true) Serial.println(state.turnCount); - if ((state.waitForTurn == true) && (state.turnCount == 1)) { - - if (state.chassis.actionDone == true) { - state.turnCount++; - if (DEBUGGER == true) Serial.println("left turn accomplished"); - state.chassis.actionDone = false; - } - - if (state.waitCount == 0) { - if (DEBUGGER == true) Serial.println("starting timer"); - state.timeStart = millis(); - state.waitCount++; - } - - if (state.turnCount == 1) { - vehicleChassis->turnLeftLimited(2600, state.timeStart); - state.api.state = "Picking_up_left_box"; - state.side = "left"; - if (DEBUGGER == true) Serial.println("turnleftlimited"); - state.visual = false; - state.turnSonar = true; - if (DEBUGGER == true) Serial.println("after turnVision"); - } - } - - //Second move is per default a drive straight for adjustment - if (DEBUGGER == true) Serial.println("before second turn"); - if (state.turnCount == 2) { - if (state.chassis.actionDone == true) { - state.turnCount++; - if (DEBUGGER == true) Serial.println("drive straight accomplished"); - state.chassis.actionDone = false; - } - - if (DEBUGGER == true) Serial.println("turncount2"); - if (state.waitCount == 1) { - state.timeStart = millis(); - if (DEBUGGER == true) Serial.println("starting second timer"); - state.waitCount++; - } - - if (state.turnCount == 2) { - vehicleChassis->driveStraightLimited(2400, state.timeStart); - if (DEBUGGER == true) Serial.println("drivestraight"); - } - } - //Third move is a counterturn dependend on the first move - if (DEBUGGER == true) Serial.println("before third turn"); - if (state.turnCount == 3) { - if (DEBUGGER == true) Serial.println("turncount3"); - if (state.chassis.actionDone == true) { - state.turnCount++; - if (DEBUGGER == true) Serial.println("right turn accomplished"); - state.chassis.actionDone = false; - state.visual = true; - state.turnSonar = false; - state.sonar.detachServo = true; - state.vision.targetDetected = false; - } - - if (state.waitCount == 2) { - state.timeStart = millis(); - if (DEBUGGER == true) Serial.println("starting third timer"); - state.waitCount++; - } - - if (state.turnCount == 3) { - vehicleChassis->turnRightLimited(3200, state.timeStart); - if (DEBUGGER == true) Serial.println("turnright"); - state.waitForTurn = false; - } - } - } - } - //If the target is lost or never detected, the system has to stop and search for a box again - else if ((state.vision.targetDetected == false) && (state.turnCount < 4)) { - state.visual = true; - state.turnSonar = false; - state.sonar.detachServo = true; - vehicleChassis->stop(); - if (DEBUGGER == true) Serial.println("Stopping when No target"); - } - - //Before the box the vehicle first has to refind the line - if (DEBUGGER == true) Serial.println("before cameranavigation"); - if (state.turnCount == 4) { - if (DEBUGGER == true) Serial.print("Turn4 with servoAngle: "); - if (DEBUGGER == true) Serial.println(state.vision.servoAngle); - if (DEBUGGER == true) Serial.print("Visual: "); if (DEBUGGER == true) Serial.println(state.visual); - if (((state.vision.targetDetected == true) || (state.visual == false)) && (state.hoist.loaded == false)) { - if ((state.vision.servoAngle > 92) && (state.visual == true)) { - vehicleChassis->turnLeft(); - if (DEBUGGER == true) Serial.println("after turnleft"); - } - if ((state.vision.servoAngle < 88) && (state.visual == true)) { - vehicleChassis->turnRight(); - if (DEBUGGER == true) Serial.println("after turnright"); - } - if (((state.vision.servoAngle <= 92) && (state.vision.servoAngle >= 88)) && (state.visual == true)) { - vehicleChassis->driveStraight(); - if (DEBUGGER == true) Serial.println("after drivestraight"); - } - if (DEBUGGER == true) Serial.print(state.chassis.sensor[0]); if (DEBUGGER == true) Serial.print(state.chassis.sensor[1]); if (DEBUGGER == true) Serial.print(state.chassis.sensor[2]); if (DEBUGGER == true) Serial.print(state.chassis.sensor[3]); if (DEBUGGER == true) Serial.print(state.chassis.sensor[4]); - if (((state.chassis.sensor[0] + state.chassis.sensor[1] + state.chassis.sensor[2] + state.chassis.sensor[3] + state.chassis.sensor[4]) != 0) && (state.visual == true)) { - if (DEBUGGER == true) Serial.println("detected line"); - //state.chassis.IR = true; - state.visual = false; - state.turnSonar = true; - state.api.sector = "21"; - } - } - else if (state.visual == true) { - vehicleChassis->stop(); - if (DEBUGGER == true) Serial.println("stopping when no target available"); - } - } - - - } - - //Sector 21 movement - if (state.api.sector == "21") { - if (DEBUGGER == true) Serial.println("following line again"); - vehicleChassis->motorControl(state.sonar.sonarFactor); - if (state.chassis.actionDone == true) { - state.api.sector = "22"; - state.chassis.direction = "stop"; - if (DEBUGGER == true) Serial.println("end of sector21"); - state.chassis.actionDone = false; - } - } - - //Sector 22 movement - if (state.api.sector == "22") { - vehicleChassis->motorControl(state.sonar.sonarFactor); - if (state.chassis.actionDone == true) { - state.api.sector = "23"; - if (DEBUGGER == true) Serial.println("end of sector22"); - state.chassis.actionDone = false; - } - } - - //Sector 23 movement - if (state.api.sector == "23") { - state.vision.targetDetected = false; - if (DEBUGGER == true) Serial.println("Arrived at loadingpoint"); - if (DEBUGGER == true) Serial.print("ActionDone 23: "); if (DEBUGGER == true) Serial.println(state.chassis.actionDone); - if (state.hoist.loaded == false) { - vehicleHoist->load(); - vehicleChassis->stop(); - state.chassis.direction = "straight"; - state.api.state = "Loading_box"; - state.chassis.IR = false; - if (DEBUGGER == true) Serial.print("ActionDone 23 loading: "); if (DEBUGGER == true) Serial.println(state.chassis.actionDone); - } - //After loading process the system has to turn around - if (state.hoist.loaded == true) { - state.hoist.detachServo = true; - state.api.state = "Bringing_box_to_port"; - if (DEBUGGER == true) Serial.println("Loading complete"); - if (state.chassis.actionDone == false) { - vehicleChassis->turnAround(2); - } - if (DEBUGGER == true) Serial.print("ActionDone: "); if (DEBUGGER == true) Serial.println(state.chassis.actionDone); - if (state.chassis.actionDone == true) { - state.api.sector = "24"; - state.chassis.direction = "right"; - state.chassis.actionDone = false; - if (DEBUGGER == true) Serial.println("sector23 finished"); - } - } - } - - //Start of sector 24 where the vehicle drives the container to the targeted port - if (state.api.sector == "24") { - if (DEBUGGER == true) Serial.println("Sector24"); - vehicleChassis->motorControl(state.sonar.sonarFactor); - if (state.chassis.actionDone == true) { - state.api.sector = "30"; - if (state.side == "right") { - state.chassis.direction = "left"; - if (DEBUGGER == true) Serial.println("direction from box at beginning was right"); - } - if (state.side == "left") { - state.chassis.direction = "straight"; - if (DEBUGGER == true) Serial.println("direction from box at beginning was left"); - } - if (DEBUGGER == true) Serial.println("Sector24 actiondone"); - state.chassis.actionDone = false; - } - } - - //Start of sector 30 where the vehicle still drives the container to the targeted port - if (state.api.sector == "30") { - vehicleChassis->motorControl(state.sonar.sonarFactor); - if (DEBUGGER == true) Serial.println("inside sector 30"); - if (state.chassis.actionDone == true) { - - state.api.sector = "31"; - if (state.api.unloadPort == "1") { - state.chassis.direction = "straight"; - } - else if (state.api.unloadPort == "2") { - state.chassis.direction = "left"; - } - state.chassis.actionDone = false; - } - } - - //Sector 31 movement - if (state.api.sector == "31") { - vehicleChassis->motorControl(state.sonar.sonarFactor); - if (DEBUGGER == true) Serial.println("Inside sector 31"); - if (state.chassis.actionDone == true) { - - if (state.api.unloadPort == "1") { - state.chassis.direction = "stop"; - state.api.sector = "33"; - } - if (state.api.unloadPort == "2") { - state.chassis.direction = "right"; - state.api.sector = "32"; - } - state.chassis.actionDone = false; - } - } - - //Sector 32 movement - if (state.api.sector == "32") { - vehicleChassis->motorControl(state.sonar.sonarFactor); - if (DEBUGGER == true) Serial.println("inside sector 32"); - if (state.chassis.actionDone == true) { - state.api.sector = "33"; - if (DEBUGGER == true) Serial.println("Finnishing sector 32"); - state.chassis.direction = "stop"; - state.chassis.actionDone = false; - } - } - - //Sector 33 movement - if (state.api.sector == "33") { - vehicleChassis->motorControl(state.sonar.sonarFactor); - if (state.chassis.actionDone == true) { - state.api.sector = "34"; - if (DEBUGGER == true) Serial.println("Finnishing sector 33"); - state.chassis.direction = "straight"; - state.chassis.actionDone = false; - } - } - - //Sector 34 movement - if (state.api.sector == "34") { - vehicleChassis->stop(); - vehicleHoist->unload(); - state.chassis.IR = false; - if (DEBUGGER == true) Serial.println("unload"); - if (state.hoist.loaded == false) { - state.hoist.detachServo = true; - state.api.sector = "35"; - state.api.state = "Box_delivered"; - state.timeStart = millis(); - - if (DEBUGGER == true) Serial.println("starting fourth timer"); - } - } - - //Sector 35 movement - if (state.api.sector == "35") { - if (state.chassis.actionDone == false) { - vehicleChassis->driveBackLimited(2000, state.timeStart); - state.chassis.IR = false; - } - if (state.chassis.actionDone == true) { - state.api.sector = "36"; - if (DEBUGGER == true) Serial.println("Driveback home"); - state.api.state = "driving_back_home"; - state.chassis.actionDone = false; - } - - } - - //Sector 36 movement - if (state.api.sector == "36") { - if (DEBUGGER == true) Serial.print("Sector 36 Actiondone: "); if (DEBUGGER == true) Serial.println(state.chassis.actionDone); - if (state.chassis.actionDone == false) { - vehicleChassis->turnAround(2); - if (DEBUGGER == true) Serial.println("Sector 36 after turnaround"); - } - if (state.chassis.actionDone == true) { - state.api.sector = "37"; - state.chassis.direction = "right"; - state.chassis.actionDone = false; - } - } - - //Sector 37 movement - if (state.api.sector == "37") { - if (DEBUGGER == true) Serial.println("Sector 37"); - vehicleChassis->motorControl(state.sonar.sonarFactor); - if (state.chassis.actionDone == true) { - if (state.api.unloadPort == "1") { - state.api.sector = "38"; - state.chassis.direction = "straight"; - } - else if (state.api.unloadPort == "2") { - state.api.sector = "40"; - if (state.api.vehicleID == "Idefix") { - state.chassis.direction = "left"; - } - else if (state.api.vehicleID == "Obelix") { - state.chassis.direction = "straight"; - } - state.chassis.actionDone = false; - } - state.chassis.actionDone = false; - } - } - - //Sector 38 movement - if (state.api.sector == "38") { - if (DEBUGGER == true) Serial.println("Sector 38"); - vehicleChassis->motorControl(state.sonar.sonarFactor); - if (state.chassis.actionDone == true) { - state.api.sector = "40"; - if (state.api.vehicleID == "Idefix") { - state.chassis.direction = "left"; - } - else if (state.api.vehicleID == "Obelix") { - state.chassis.direction = "straight"; - } - state.chassis.actionDone = false; - } - } - - //Sector 40 movement - if (state.api.sector == "40") { - vehicleChassis->motorControl(state.sonar.sonarFactor); - if (DEBUGGER == true) Serial.println("Sector 40"); - if (state.chassis.actionDone == true) { - if (state.api.vehicleID == "Obelix") { - if (DEBUGGER == true) Serial.println("Sector 40 Obelix finished"); - state.chassis.direction = "stop"; - state.api.sector = "42"; - } - else if (state.api.vehicleID == "Idefix") { - if (DEBUGGER == true) Serial.println("Sector 40 Idefix finished"); - state.chassis.direction = "right"; - state.api.sector = "41"; - } - state.chassis.actionDone = false; - } - } - - //Sector 41 movement - if (state.api.sector == "41") { - vehicleChassis->motorControl(state.sonar.sonarFactor); - if (DEBUGGER == true) Serial.print("Sector 41 Actiondone: "); if (DEBUGGER == true) Serial.println(state.chassis.actionDone); - if (state.chassis.actionDone == true) { - state.chassis.direction = "stop"; - if (DEBUGGER == true) Serial.println("Sector 41 finished"); - state.api.sector = "42"; - state.chassis.actionDone = false; - } - } - - //Sector 42 movement - if (state.api.sector == "42") { - vehicleChassis->motorControl(state.sonar.sonarFactor); - if (DEBUGGER == true) Serial.print("Sector 42 Actiondone: "); if (DEBUGGER == true) Serial.println(state.chassis.actionDone); - if (state.chassis.actionDone == true) { - state.chassis.direction = "stop"; - if (DEBUGGER == true) Serial.println("Sector 42 finished"); - vehicleChassis->stop(); - state.api.sector = "43"; - state.chassis.actionDone = false; - } - } - - //Sector 43 movement reset the variables once the task is finished - if (state.api.sector == "43") { - if (DEBUGGER == true) Serial.print("Sector 43 Actiondone: "); if (DEBUGGER == true) Serial.println(state.chassis.actionDone); - if (state.chassis.actionDone == false) { - vehicleChassis->turnAround(2); - if (DEBUGGER == true) Serial.println("Sector 43 after turnaround"); - } - if (state.chassis.actionDone == true) { - if (DEBUGGER == true) Serial.println("Sector 43 finished"); - state.api.sector = "10"; - state.api.state = "resting"; - state.api.task = "finished"; - state.api.workState = false; - state.api.workStateChange = true; - state.chassis.actionDone = false; - state.waitCount = 0; - state.turnCount = 1; - state.waitForTurn = false; - state.vision.targetDetected = false; - state.api.jobCount++; - state.vision.reset = true; - state.api.container = ""; - state.api.unloadPort= ""; - state.api.containerChange = true; - state.api.unloadPortChange = true; - } - - } - } - else { +// Smart Box main file + +#include +#include +//#include "SPI.h" +// own files: +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// ===================================== Global Variables ===================================== +myJSONStr *JSarra; // used in NetworkManager.h, used for saving incoming Messages, FIFO order, see also MAX_JSON_MESSAGES_SAVED +NetworkManager *mNetwP = 0; // used for using NetworkManager access outside setup() +Sonar *vehicleSonar; // used for Sonar access +Vision *vehicleVision; // used for Vision access +Hoist *vehicleHoist; // used for Hoist access +Chassis *vehicleChassis; // used for Chassis access +VehicleWebAPI *vehicleAPI; // used for Sonar access +bool hasAnswered = false; // variable used to see if Vehicle have answered +bool publishParams = false; // if SmartBox requests Vehicle to answer +bool publishAck = false; +myJSONStr *tmp_mess; // pointer to array of messages, used for iteration of messages +MQTTTasks *TaskMain; // filled in NetworkManager.cpp, used for saving incoming Messages, FIFO order +SmartBoxList SB_hostnames; + +// -.-.-.-.-.-.-.-.-.-.-.-.-.- used for Statuses -.-.-.-.-.-.-.-.-.-.-.-.-.- +enum status_main // stores main status for Program run (main.cpp) +{ + status_noTask = 0, + status_hasRequest = 1, + status_handleRequest = 2, + status_transporting = 3, + status_transported = 4, + stauts_requestHandeled = 5 +}; +int mcount = 0; // needed for number of messages received, lower num +int mcount2 = 0; +status_main stat = status_main::status_noTask; +bool toNextStatus = true; // true if changing state, false if staying in state, it's enshuring that certain code will only run once +int messageCounter = 0; +int numOfTasksToDo = 0; +myJSONStr currentTask; +void (*myFuncToCall)() = nullptr; // func to call in main-loop, for finite state machine + +// ===================================== my helper Functions ===================================== + +void getVehicleInfo(){} // print Smart Box Information +{ + LOG2("Vehicle state: " + String(stat)); + LOG2("Variable has answered: " + String(hasAnswered)); + LOG2("Variable mcount: " + String(mcount)); + LOG2("Variable mcount2: " + String(mcount2)); + LOG2("Variable toNextStatus: " + String(toNextStatus)); + LOG2("Variable messageCounter: " + String(messageCounter)); + LOG2("Variable numOfTasksToDo: " + String(numOfTasksToDo)); + LOG2("-------- current Task --------"); + LOG2("current Task (hostname): " + String(currentTask.hostname)); + LOG2("current Task (topic): " + String(currentTask.topic)); + LOG2("current Task (request): " + String(currentTask.request)); + LOG2("current Task (level): " + String(currentTask.level)); +}; + +// ===================================== my Functions ===================================== + +void loopNoTask() +{ + if (toNextStatus) + { + LOG1("entering new state: loopNoTask"); + mcount = TaskMain->returnCurrentIterator(); + mcount2 = TaskMain->returnCurrentIterator(); + toNextStatus = false; + } + mNetwP->loop(); + mcount2 = TaskMain->returnCurrentIterator(); + if (mcount2 > mcount) + { + toNextStatus = true; + LOG2("Messages arrived"); + stat = status_main::status_hasRequest; + myFuncToCall = loopTask; + } + else if (mcount2 == mcount) + { + mNetwP->loop(); + LOG3("nothing to do"); + } + else + LOG1("something is wrong... "); +}; + +void loopTask() // loops through all unhandeled tasks +{ + LOG1("entering new state: loopTask"); + mNetwP->loop(); + mcount2 = TaskMain->returnCurrentIterator(); + tmp_mess = TaskMain->getBetween(mcount, mcount2); + numOfTasksToDo = (sizeof(tmp_mess) / sizeof(tmp_mess[0])); + if (tmp_mess == nullptr) + { + LOG2("no messages"); + stat = status_main::status_noTask; // jump back to noTask status + myFuncToCall = loopNoTask; + } + else + { + LOG3("num of tasks: " + String(numOfTasksToDo)); + currentTask = tmp_mess[0]; // do first tasks + mcount++; + stat = status_main::status_handleRequest; + LOG3("going to do next task"); + String toparr[] = TaskMain->returnMQTTtopics(currentTask); // array indexes: 0: SmartBox, 1: SmartBox_ID, 2: level/decision + if ((toparr[0] == "SmartBox") && (toparr[2] == "level")) // if SmartBox/+/level + { + publishParams = true; + SB_hostnames.push(toparr[1]); // save hostname + } + else if ((toparr[0] == "SmartBox") && SB_hostnames.isInList(toparr[1]) && (toparr[2] == "decision")) // if SmartBox/+/decision + { + SB_hostnames.pop(toparr[1]); + if (SB_hostnames.getSize() == 0) + publishParams = false; + mNetwP->publishMessage("Vehicle/" + mNetwP->getHostName() + "/ack", "{hostname:" + toparr[1] + "}"); // acknoledge message received and coming for transport + stat = status_main::status_transporting; + myFuncToCall = transportBox1; + } + else + { + LOG3("topic: [" + currentTask.topic + "] unknown, and therefore not treated here\t\t hostname: " + currentTask.hostname + "\t Request: " + currentTask.request); + } + } +}; + +void transportBox1() // used to transport the Smart Box, 1 is for Setup from Luciano Bettinaglio, can be editted for other Setups +{ + if (vehicleSonar->readSonar() > DIST_TO_BOX) // very simple case that vehicle can drive forward in one line to Box + { + vehicleChassis->driveStraight(); + } + else + { vehicleChassis->stop(); - } + vehicleHoist->unload(); + } + mNetwP->loop(); + if (TaskMain->hasUrgentMessage()) // here emergency stopping functionality can be added + myJSONStr nextTaskTodo = doLastUrgentMessage(); + + // if transported: + mNetwP->publishMessage("Vehicle/" + mNetwP->getHostName() + "/ack", "{request:" + toparr[1] + "}"); // publish acknoledgement transported to Vehicle/...ID.../ack + myFuncToCall = transportBox2; +}; + +void transportBox2() +{ + vehicleHoist->load(); + vehicleChassis->driveBack(); + stat = status_main::status_hasRequest; + myFuncToCall = loopTask; } -void setup() { - //Initialize serial and wait for port to open: - if (DEBUGGER == true) Serial.begin(9600); - //while (!if (DEBUGGER == true) Serial) { - //; // wait foserial port to connect. Needed for native USB port only - //} - if (DEBUGGER == true) Serial.println("Vehicle: booting..."); - vehicleChassis = new Chassis(SPEED, K_P, K_I, K_D, RIGHT_MOTOR, LEFT_MOTOR, PIN_SENSOR_0, PIN_SENSOR_1, PIN_SENSOR_2, PIN_SENSOR_3, PIN_SENSOR_4); - state.chassis.speed = SPEED; - delay(1000); +// ===================================== Arduino Functions ===================================== +void setup() // for initialisation +{ + if (log_level > 0) + { + Serial.begin(12000); //Initialize serial + while (!Serial) + ; // wait for serial port to connect + } + mNetwP = new NetworkManager(); vehicleSonar = new Sonar(SONAR_SERVO_PIN, SONAR_TRIGGER_PIN, SONAR_ECHO_PIN, SONAR_MAX_DISTANCE, MIN_ERROR, MAX_ERROR, MIN_TURN_ANGLE, MAX_TURN_ANGLE); - delay(1000); vehicleVision = new Vision(VISION_START_ANGLE, VISION_SERVO_PIN, VISION_DELAY_FACTOR, VISION_TOLERANCE_LEFT, VISION_TOLERANCE_RIGHT); - delay(1000); vehicleHoist = new Hoist(HOIST_SERVO_PIN, HOIST_SERVO_DELAY, HOIST_POISITION_MAX, HOIST_POSITION_MIN); - delay(1000); - vehicleAPI = new VehicleWebAPI(&state.api); - delay(1000); - if (DEBUGGER == true) Serial.println("Booting complete!"); -} - + vehicleChassis = new Chassis(SPEED, K_P, K_I, K_D, RIGHT_MOTOR, LEFT_MOTOR, PIN_SENSOR_0, PIN_SENSOR_1, PIN_SENSOR_2, PIN_SENSOR_3, PIN_SENSOR_4); + JSarra = mNetwP->JSarrP; + mNetwP->subscribe("SmartBox/+/level"); + mNetwP->subscribe("SmartBox/+/decision"); + TaskMain = mNetwP->NetManTask_classPointer; + myFuncToCall = loopNoTask; + if (true) // for debugging purpose + { + pinMode(13, OUTPUT); // debug LED + } +} -void loop() { - vehicleAPI->loop(&state.api); - if (state.api.workState == true) { - state.sonarCount++; - if ((state.api.sector == "20") && (state.visual == true)) { - vehicleVision->loop(&state.vision); - } - vehicleChassis->loop(&state.chassis); - if (state.sonarCount > 30) { - state.sonarCount = 0; - vehicleSonar->loop(&state.sonar, state.chassis.directionError, state.turnSonar); - } - vehicleHoist->loop(&state.hoist); +void loop() // one loop per one cycle (SB full -> transported -> returned empty) +{ + if (true) // degug cycle + { + digitalWrite(13, LOW); + delay(1000); + digitalWrite(13, HIGH); + delay(1000); } - strategy(); - if (DEBUGGER == true) Serial.println("-----------------------------------------"); -} \ No newline at end of file + + myFuncToCall(); + + mNetwP->loop(); + if (publishParams) + mNetwP->publishMessage("Vehicle/" + mNetwP->getHostName() + "/params", "{params:[100,100" + String(numOfTasksToDo) + "] }"); // publish params, see documentation +} diff --git a/embedded/src/main_old.cpp b/embedded/src/main_old.cpp new file mode 100644 index 0000000..b4d0e3e --- /dev/null +++ b/embedded/src/main_old.cpp @@ -0,0 +1,783 @@ +/* + Main programm to run the transport vehicle + Created by Glenn Huber 05.05.2018 +*/ + +/* +#include "Arduino.h" +#include "VehicleWebAPI.h" +#include "SPI.h" +#include "Sonar.h" +#include "Chassis.h" +#include "Hoist.h" +#include "Vision.h" + +struct VehicleState { + ChassisState chassis; + SonarState sonar; + VisionState vision; + HoistState hoist; + ApiState api; + float curveFactor = 0; + int turnCount = 1; + int waitCount = 0; + int sonarCount = 0; + unsigned long timeStart = 0; + bool turnSonar = true; + bool waitForTurn = false; + bool visual = false; + String side = ""; +} state; + +Sonar *vehicleSonar; +Vision *vehicleVision; +Hoist *vehicleHoist; +Chassis *vehicleChassis; +VehicleWebAPI *vehicleAPI; + +void strategy() { + if (DEBUGGER == true) Serial.print("Sector begin strategy"); if (DEBUGGER == true) Serial.println(state.api.sector); + if (DEBUGGER == true) Serial.print("VehicleCount: "); if (DEBUGGER == true) Serial.println(state.api.vehicleCount); + if (DEBUGGER == true) Serial.print("JobCount: "); if (DEBUGGER == true) Serial.println(state.api.jobCount); + if (DEBUGGER == true) Serial.print("otherTask: "); if (DEBUGGER == true) Serial.println(state.api.otherTask); + if (DEBUGGER == true) Serial.print("othersector: "); if (DEBUGGER == true) Serial.println(state.api.otherSector); + if (DEBUGGER == true) Serial.print("Container: "); if (DEBUGGER == true) Serial.println(state.api.container); + if (DEBUGGER == true) Serial.print("port: "); if (DEBUGGER == true) Serial.println(state.api.unloadPort); + + //After Task is finished, change the task + if ((state.api.state == "resting") && (state.api.task != "finished")) { + state.api.task = "finished"; + } + //Sending the state if change happend + if (state.api.taskChange == true) { + if (DEBUGGER == true) Serial.println("Sending task"); + vehicleAPI->sendTask(); + if ((state.api.vehicleID == "Idefix") && (state.api.vehicleCount == 2)) { + if (DEBUGGER == true) Serial.println("send obelix task"); + vehicleAPI->sendTaskToObelix(); + } + if ((state.api.vehicleID == "Obelix") && (state.api.vehicleCount == 2)) { + if (DEBUGGER == true) Serial.println("send idefix task"); + vehicleAPI->sendTaskToIdefix(); + } + state.api.taskChange = false; + } + if (state.api.sectorChange == true) { + if (DEBUGGER == true) Serial.println("Sending sector"); + vehicleAPI->sendSector(); + if ((state.api.vehicleID == "Idefix") && (state.api.vehicleCount == 2) && ((state.api.sector == "20") || (state.api.sector == "24") || (state.api.sector == "31") || (state.api.sector == "40"))) { + if (DEBUGGER == true) Serial.println("send obelix sector"); + vehicleAPI->sendSectorToObelix(); + } + if ((state.api.vehicleID == "Obelix") && (state.api.vehicleCount == 2) && ((state.api.sector == "20") || (state.api.sector == "24") || (state.api.sector == "31") || (state.api.sector == "40"))) { + if (DEBUGGER == true) Serial.println("send idefix sector"); + vehicleAPI->sendSectorToIdefix(); + } + state.api.sectorChange = false; + } + if (state.api.stateChange == true) { + if (DEBUGGER == true) Serial.println("Sending state"); + vehicleAPI->sendState(); + state.api.stateChange = false; + } + + //Sectorstrategy the workState is changed, dependend on the position of the other vehicle + if (((state.api.sector == "10") || (state.api.sector == "11")) && (state.api.otherSector == "20")) { + state.api.workState = false; + state.api.workStateChange = true; + } + else if ((state.api.sector == "20") && (state.api.otherSector == "24")) { + state.api.workState = false; + state.api.workStateChange = true; + } + else if ((state.api.sector == "31") && (state.api.otherSector == "31")) { + state.api.workState = false; + state.api.workStateChange = true; + } + //else if ((state.api.sector == "20") && (state.api.otherSector == "22")) { + // state.api.workState = false; + // state.api.workStateChange = true; + //} + //else if ((state.api.sector == "30") && (state.api.otherSector == "32")) { + // state.api.workState = false; + // state.api.workStateChange = true; + //} + //else if ((state.api.sector == "40") && (state.api.otherSector == "20") && (state.api.vehicleID == "Idefix")) { + // state.api.workState = false; + // state.api.workStateChange = true; + //} + else if (((state.api.sector == "10") || (state.api.sector == "11")) && (state.api.otherSector == "24")) { + state.api.workState = true; + state.api.workStateChange = true; + } + else if ((state.api.sector == "20") && (state.api.otherSector == "31")) { + state.api.workState = true; + state.api.workStateChange = true; + } + else if ((state.api.sector == "31") && (state.api.otherSector == "40")) { + state.api.workState = true; + state.api.workStateChange = true; + } + //else if ((state.api.sector == "20") && ((state.api.otherSector == "10") || (state.api.otherSector == "22") || (state.api.otherSector == "32") || (state.api.otherSector == "40"))) { + // state.api.workState = true; + // state.api.workStateChange = true; + //} + //else if ((state.api.sector == "30") && ((state.api.otherSector == "10") || (state.api.otherSector == "20") || (state.api.otherSector == "22") || (state.api.otherSector == "40"))) { + // state.api.workState = true; + // state.api.workStateChange = true; + //} + //else if ((state.api.sector == "40") && ((state.api.otherSector == "10") || (state.api.otherSector == "20") || (state.api.otherSector == "32") || (state.api.otherSector == "40")) && (state.api.vehicleID == "Idefix")) { + // state.api.workState = true; + // state.api.workStateChange = true; + //} + //Jobstrategy + if ((state.api.container == "yellow") && (state.api.unloadPort == "1") && (state.api.task != "RED_TO_PORT1") && (state.api.task != "RED_TO_PORT2") && (state.api.task != "YELLOW_TO_PORT2") && (state.api.task != "YELLOW_TO_PORT1") && (state.api.otherTask != "YELLOW_TO_PORT1")) { + state.api.task = "YELLOW_TO_PORT1"; + state.api.state = "Starting_Task"; + state.api.workState = true; + state.api.workStateChange = true; + state.vision.target = 2; + } + else if ((state.api.container == "red") && (state.api.unloadPort == "1") && (state.api.task != "RED_TO_PORT1") && (state.api.task != "RED_TO_PORT2") && (state.api.task != "YELLOW_TO_PORT2") && (state.api.task != "YELLOW_TO_PORT1") && (state.api.otherTask != "RED_TO_PORT1")) { + state.api.task = "RED_TO_PORT1"; + state.api.state = "Starting_Task"; + state.api.workState = true; + state.api.workStateChange = true; + state.vision.target = 1; + } + else if ((state.api.container == "yellow") && (state.api.unloadPort == "2") && (state.api.task != "RED_TO_PORT1") && (state.api.task != "RED_TO_PORT2") && (state.api.task != "YELLOW_TO_PORT2") && (state.api.task != "YELLOW_TO_PORT1") && (state.api.otherTask != "YELLOW_TO_PORT2")) { + state.api.task = "YELLOW_TO_PORT2"; + state.api.state = "Starting_Task"; + state.api.workState = true; + state.api.workStateChange = true; + state.vision.target = 2; + } + else if ((state.api.container == "red") && (state.api.unloadPort == "2") && (state.api.task != "RED_TO_PORT1") && (state.api.task != "RED_TO_PORT2") && (state.api.task != "YELLOW_TO_PORT2") && (state.api.task != "YELLOW_TO_PORT1") && (state.api.otherTask != "RED_TO_PORT2")) { + state.api.task = "RED_TO_PORT2"; + state.api.state = "Starting_Task"; + state.api.workState = true; + state.api.workStateChange = true; + state.vision.target = 1; + } + else { + if (DEBUGGER == true) Serial.println("Container change in else"); + state.api.container = ""; + state.api.containerChange = true; + } + + //Drive strategy without an active workstate, there is no sectorhandling + if (state.api.workState == true) { + + //Navigation strategy + if (DEBUGGER == true) Serial.print("IR-status: "); if (DEBUGGER == true) Serial.println(state.chassis.IR); + if (DEBUGGER == true) Serial.print("curveFactor before pid: "); + if (DEBUGGER == true) Serial.println(state.curveFactor); + state.curveFactor = state.chassis.PidValue; + + //Sector 10 movement + if ((state.api.sector == "10") && (state.api.task != "nothing") && (state.api.task != "finished")) { + if (DEBUGGER == true) Serial.println("Sector 10"); + if (state.chassis.direction != "right") { + delay(1000); + } + state.chassis.direction = "right"; + vehicleChassis->motorControl(state.sonar.sonarFactor); + if (state.chassis.actionDone == true) { + if (DEBUGGER == true) Serial.print("Sector 10 vehicleID"); if (DEBUGGER == true) Serial.println(state.api.vehicleID); + if (state.api.vehicleID == "Obelix") { + state.api.sector = "11"; + if (DEBUGGER == true) Serial.println("Sector 10 order to Obelix"); + state.chassis.direction = "straight"; + } + else if (state.api.vehicleID == "Idefix") { + state.api.sector = "12"; + if (DEBUGGER == true) Serial.println("Sector 10 order to Idefix"); + state.chassis.direction = "stop"; + } + if (DEBUGGER == true) Serial.println("end of scector10"); + state.chassis.actionDone = false; + if (DEBUGGER == true) Serial.print("Sector 10 next sector: "); if (DEBUGGER == true) Serial.println(state.api.sector); + } + } + if (DEBUGGER == true) Serial.print("Between 10 and 11 sector: "); if (DEBUGGER == true) Serial.println(state.api.sector); + //Sector 11 movement + if (state.api.sector == "11") { + if (DEBUGGER == true) Serial.println("Sector 11"); + vehicleChassis->motorControl(state.sonar.sonarFactor); + if (state.chassis.actionDone == true) { + state.api.sector = "12"; + state.chassis.direction = "stop"; + if (DEBUGGER == true) Serial.println("end of sector11"); + state.chassis.actionDone = false; + } + } + + //Sector 12 movement + if (state.api.sector == "12") { + if (DEBUGGER == true) Serial.println("Sector 12"); + vehicleChassis->motorControl(state.sonar.sonarFactor); + if (state.chassis.actionDone == true) { + if (DEBUGGER == true) Serial.println("End of 12"); + state.api.sector = "20"; + state.chassis.actionDone = false; + vehicleChassis->stop(); + state.chassis.direction = "straight"; + state.api.state = "Searching_box"; + state.visual = true; + state.turnSonar = false; + state.sonar.detachServo = true; + } + } + + //Sector 20 movement + if (state.api.sector == "20") { + if (DEBUGGER == true) Serial.println("start of sector20"); + + if (((state.chassis.sensor[0] + state.chassis.sensor[1] + state.chassis.sensor[2] + state.chassis.sensor[3] + state.chassis.sensor[4]) == 0) || ((state.chassis.sensor[0] + state.chassis.sensor[1] + state.chassis.sensor[2] + state.chassis.sensor[3] + state.chassis.sensor[4]) == 5)) { + if ((state.waitForTurn == false) && (state.waitCount == 0)) { + if (DEBUGGER == true) Serial.println("changing wait for turn"); + state.waitForTurn = true; + } + } + if (DEBUGGER == true) Serial.print("Targetdetected: "); if (DEBUGGER == true) Serial.println(state.vision.targetDetected); + if ((state.vision.targetDetected == true) && (state.turnCount < 4)) { + //First move dependend on which side the box is on + if (DEBUGGER == true) Serial.println("before first turn"); + + //Is the Box on the right side of the vehicle turn right + if (state.vision.servoAngle < 90) { + if (DEBUGGER == true) Serial.print("waitForturn: "); if (DEBUGGER == true) Serial.println(state.waitForTurn); + if (DEBUGGER == true) Serial.print("TurnCount: "); if (DEBUGGER == true) Serial.println(state.turnCount); + if ((state.waitForTurn == true) && (state.turnCount ==1)) { + + if (state.chassis.actionDone == true) { + state.turnCount++; + if (DEBUGGER == true) Serial.println("right turn accomplished"); + state.chassis.actionDone = false; + } + + if (state.waitCount == 0) { + if (DEBUGGER == true) Serial.println("starting timer"); + state.timeStart = millis(); + state.waitCount++; + } + + if (state.turnCount == 1) { + vehicleChassis->turnRightLimited(2800, state.timeStart); + state.api.state = "Picking_up_right_box"; + state.side = "right"; + if (DEBUGGER == true) Serial.println("turnRightLimited"); + state.visual = false; + state.turnSonar = true; + //vehicleVision->turnVision(90); + //if (DEBUGGER == true) Serial.println("after turnVision"); + } + } + + //Second move is per default a drive straight for adjustment + if (DEBUGGER == true) Serial.println("before second turn"); + if (state.turnCount == 2) { + if (state.chassis.actionDone == true) { + state.turnCount++; + if (DEBUGGER == true) Serial.println("drive straight accomplished"); + state.chassis.actionDone = false; + } + + if (DEBUGGER == true) Serial.println("turncount2"); + if (state.waitCount == 1) { + state.timeStart = millis(); + if (DEBUGGER == true) Serial.println("starting second timer"); + state.waitCount++; + } + + if (state.turnCount == 2) { + vehicleChassis->driveStraightLimited(2800, state.timeStart); + if (DEBUGGER == true) Serial.println("drivestraight"); + } + } + //Third move is a counterturn dependend on the first move + if (DEBUGGER == true) Serial.println("before third turn"); + if (state.turnCount == 3) { + if (DEBUGGER == true) Serial.println("turncount3"); + if (state.chassis.actionDone == true) { + state.turnCount++; + if (DEBUGGER == true) Serial.println("right turn accomplished"); + state.chassis.actionDone = false; + state.visual = true; + state.turnSonar = false; + state.sonar.detachServo = true; + } + + if (state.waitCount == 2) { + state.timeStart = millis(); + if (DEBUGGER == true) Serial.println("starting third timer"); + state.waitCount++; + } + + if (state.turnCount == 3) { + vehicleChassis->turnLeftLimited(2600, state.timeStart); + if (DEBUGGER == true) Serial.println("turnLeftLimited"); + state.waitForTurn = false; + } + } + } + //Is the Box on the left side of the vehicle turn left + if (state.vision.servoAngle > 90) { + if (DEBUGGER == true) Serial.print("waitForturn: "); if (DEBUGGER == true) Serial.println(state.waitForTurn); + if (DEBUGGER == true) Serial.print("TurnCount: "); if (DEBUGGER == true) Serial.println(state.turnCount); + if ((state.waitForTurn == true) && (state.turnCount == 1)) { + + if (state.chassis.actionDone == true) { + state.turnCount++; + if (DEBUGGER == true) Serial.println("left turn accomplished"); + state.chassis.actionDone = false; + } + + if (state.waitCount == 0) { + if (DEBUGGER == true) Serial.println("starting timer"); + state.timeStart = millis(); + state.waitCount++; + } + + if (state.turnCount == 1) { + vehicleChassis->turnLeftLimited(2600, state.timeStart); + state.api.state = "Picking_up_left_box"; + state.side = "left"; + if (DEBUGGER == true) Serial.println("turnleftlimited"); + state.visual = false; + state.turnSonar = true; + if (DEBUGGER == true) Serial.println("after turnVision"); + } + } + + //Second move is per default a drive straight for adjustment + if (DEBUGGER == true) Serial.println("before second turn"); + if (state.turnCount == 2) { + if (state.chassis.actionDone == true) { + state.turnCount++; + if (DEBUGGER == true) Serial.println("drive straight accomplished"); + state.chassis.actionDone = false; + } + + if (DEBUGGER == true) Serial.println("turncount2"); + if (state.waitCount == 1) { + state.timeStart = millis(); + if (DEBUGGER == true) Serial.println("starting second timer"); + state.waitCount++; + } + + if (state.turnCount == 2) { + vehicleChassis->driveStraightLimited(2400, state.timeStart); + if (DEBUGGER == true) Serial.println("drivestraight"); + } + } + //Third move is a counterturn dependend on the first move + if (DEBUGGER == true) Serial.println("before third turn"); + if (state.turnCount == 3) { + if (DEBUGGER == true) Serial.println("turncount3"); + if (state.chassis.actionDone == true) { + state.turnCount++; + if (DEBUGGER == true) Serial.println("right turn accomplished"); + state.chassis.actionDone = false; + state.visual = true; + state.turnSonar = false; + state.sonar.detachServo = true; + state.vision.targetDetected = false; + } + + if (state.waitCount == 2) { + state.timeStart = millis(); + if (DEBUGGER == true) Serial.println("starting third timer"); + state.waitCount++; + } + + if (state.turnCount == 3) { + vehicleChassis->turnRightLimited(3200, state.timeStart); + if (DEBUGGER == true) Serial.println("turnright"); + state.waitForTurn = false; + } + } + } + } + //If the target is lost or never detected, the system has to stop and search for a box again + else if ((state.vision.targetDetected == false) && (state.turnCount < 4)) { + state.visual = true; + state.turnSonar = false; + state.sonar.detachServo = true; + vehicleChassis->stop(); + if (DEBUGGER == true) Serial.println("Stopping when No target"); + } + + //Before the box the vehicle first has to refind the line + if (DEBUGGER == true) Serial.println("before cameranavigation"); + if (state.turnCount == 4) { + if (DEBUGGER == true) Serial.print("Turn4 with servoAngle: "); + if (DEBUGGER == true) Serial.println(state.vision.servoAngle); + if (DEBUGGER == true) Serial.print("Visual: "); if (DEBUGGER == true) Serial.println(state.visual); + if (((state.vision.targetDetected == true) || (state.visual == false)) && (state.hoist.loaded == false)) { + if ((state.vision.servoAngle > 92) && (state.visual == true)) { + vehicleChassis->turnLeft(); + if (DEBUGGER == true) Serial.println("after turnleft"); + } + if ((state.vision.servoAngle < 88) && (state.visual == true)) { + vehicleChassis->turnRight(); + if (DEBUGGER == true) Serial.println("after turnright"); + } + if (((state.vision.servoAngle <= 92) && (state.vision.servoAngle >= 88)) && (state.visual == true)) { + vehicleChassis->driveStraight(); + if (DEBUGGER == true) Serial.println("after drivestraight"); + } + if (DEBUGGER == true) Serial.print(state.chassis.sensor[0]); if (DEBUGGER == true) Serial.print(state.chassis.sensor[1]); if (DEBUGGER == true) Serial.print(state.chassis.sensor[2]); if (DEBUGGER == true) Serial.print(state.chassis.sensor[3]); if (DEBUGGER == true) Serial.print(state.chassis.sensor[4]); + if (((state.chassis.sensor[0] + state.chassis.sensor[1] + state.chassis.sensor[2] + state.chassis.sensor[3] + state.chassis.sensor[4]) != 0) && (state.visual == true)) { + if (DEBUGGER == true) Serial.println("detected line"); + //state.chassis.IR = true; + state.visual = false; + state.turnSonar = true; + state.api.sector = "21"; + } + } + else if (state.visual == true) { + vehicleChassis->stop(); + if (DEBUGGER == true) Serial.println("stopping when no target available"); + } + } + + + } + + //Sector 21 movement + if (state.api.sector == "21") { + if (DEBUGGER == true) Serial.println("following line again"); + vehicleChassis->motorControl(state.sonar.sonarFactor); + if (state.chassis.actionDone == true) { + state.api.sector = "22"; + state.chassis.direction = "stop"; + if (DEBUGGER == true) Serial.println("end of sector21"); + state.chassis.actionDone = false; + } + } + + //Sector 22 movement + if (state.api.sector == "22") { + vehicleChassis->motorControl(state.sonar.sonarFactor); + if (state.chassis.actionDone == true) { + state.api.sector = "23"; + if (DEBUGGER == true) Serial.println("end of sector22"); + state.chassis.actionDone = false; + } + } + + //Sector 23 movement + if (state.api.sector == "23") { + state.vision.targetDetected = false; + if (DEBUGGER == true) Serial.println("Arrived at loadingpoint"); + if (DEBUGGER == true) Serial.print("ActionDone 23: "); if (DEBUGGER == true) Serial.println(state.chassis.actionDone); + if (state.hoist.loaded == false) { + vehicleHoist->load(); + vehicleChassis->stop(); + state.chassis.direction = "straight"; + state.api.state = "Loading_box"; + state.chassis.IR = false; + if (DEBUGGER == true) Serial.print("ActionDone 23 loading: "); if (DEBUGGER == true) Serial.println(state.chassis.actionDone); + } + //After loading process the system has to turn around + if (state.hoist.loaded == true) { + state.hoist.detachServo = true; + state.api.state = "Bringing_box_to_port"; + if (DEBUGGER == true) Serial.println("Loading complete"); + if (state.chassis.actionDone == false) { + vehicleChassis->turnAround(2); + } + if (DEBUGGER == true) Serial.print("ActionDone: "); if (DEBUGGER == true) Serial.println(state.chassis.actionDone); + if (state.chassis.actionDone == true) { + state.api.sector = "24"; + state.chassis.direction = "right"; + state.chassis.actionDone = false; + if (DEBUGGER == true) Serial.println("sector23 finished"); + } + } + } + + //Start of sector 24 where the vehicle drives the container to the targeted port + if (state.api.sector == "24") { + if (DEBUGGER == true) Serial.println("Sector24"); + vehicleChassis->motorControl(state.sonar.sonarFactor); + if (state.chassis.actionDone == true) { + state.api.sector = "30"; + if (state.side == "right") { + state.chassis.direction = "left"; + if (DEBUGGER == true) Serial.println("direction from box at beginning was right"); + } + if (state.side == "left") { + state.chassis.direction = "straight"; + if (DEBUGGER == true) Serial.println("direction from box at beginning was left"); + } + if (DEBUGGER == true) Serial.println("Sector24 actiondone"); + state.chassis.actionDone = false; + } + } + + //Start of sector 30 where the vehicle still drives the container to the targeted port + if (state.api.sector == "30") { + vehicleChassis->motorControl(state.sonar.sonarFactor); + if (DEBUGGER == true) Serial.println("inside sector 30"); + if (state.chassis.actionDone == true) { + + state.api.sector = "31"; + if (state.api.unloadPort == "1") { + state.chassis.direction = "straight"; + } + else if (state.api.unloadPort == "2") { + state.chassis.direction = "left"; + } + state.chassis.actionDone = false; + } + } + + //Sector 31 movement + if (state.api.sector == "31") { + vehicleChassis->motorControl(state.sonar.sonarFactor); + if (DEBUGGER == true) Serial.println("Inside sector 31"); + if (state.chassis.actionDone == true) { + + if (state.api.unloadPort == "1") { + state.chassis.direction = "stop"; + state.api.sector = "33"; + } + if (state.api.unloadPort == "2") { + state.chassis.direction = "right"; + state.api.sector = "32"; + } + state.chassis.actionDone = false; + } + } + + //Sector 32 movement + if (state.api.sector == "32") { + vehicleChassis->motorControl(state.sonar.sonarFactor); + if (DEBUGGER == true) Serial.println("inside sector 32"); + if (state.chassis.actionDone == true) { + state.api.sector = "33"; + if (DEBUGGER == true) Serial.println("Finnishing sector 32"); + state.chassis.direction = "stop"; + state.chassis.actionDone = false; + } + } + + //Sector 33 movement + if (state.api.sector == "33") { + vehicleChassis->motorControl(state.sonar.sonarFactor); + if (state.chassis.actionDone == true) { + state.api.sector = "34"; + if (DEBUGGER == true) Serial.println("Finnishing sector 33"); + state.chassis.direction = "straight"; + state.chassis.actionDone = false; + } + } + + //Sector 34 movement + if (state.api.sector == "34") { + vehicleChassis->stop(); + vehicleHoist->unload(); + state.chassis.IR = false; + if (DEBUGGER == true) Serial.println("unload"); + if (state.hoist.loaded == false) { + state.hoist.detachServo = true; + state.api.sector = "35"; + state.api.state = "Box_delivered"; + state.timeStart = millis(); + + if (DEBUGGER == true) Serial.println("starting fourth timer"); + } + } + + //Sector 35 movement + if (state.api.sector == "35") { + if (state.chassis.actionDone == false) { + vehicleChassis->driveBackLimited(2000, state.timeStart); + state.chassis.IR = false; + } + if (state.chassis.actionDone == true) { + state.api.sector = "36"; + if (DEBUGGER == true) Serial.println("Driveback home"); + state.api.state = "driving_back_home"; + state.chassis.actionDone = false; + } + + } + + //Sector 36 movement + if (state.api.sector == "36") { + if (DEBUGGER == true) Serial.print("Sector 36 Actiondone: "); if (DEBUGGER == true) Serial.println(state.chassis.actionDone); + if (state.chassis.actionDone == false) { + vehicleChassis->turnAround(2); + if (DEBUGGER == true) Serial.println("Sector 36 after turnaround"); + } + if (state.chassis.actionDone == true) { + state.api.sector = "37"; + state.chassis.direction = "right"; + state.chassis.actionDone = false; + } + } + + //Sector 37 movement + if (state.api.sector == "37") { + if (DEBUGGER == true) Serial.println("Sector 37"); + vehicleChassis->motorControl(state.sonar.sonarFactor); + if (state.chassis.actionDone == true) { + if (state.api.unloadPort == "1") { + state.api.sector = "38"; + state.chassis.direction = "straight"; + } + else if (state.api.unloadPort == "2") { + state.api.sector = "40"; + if (state.api.vehicleID == "Idefix") { + state.chassis.direction = "left"; + } + else if (state.api.vehicleID == "Obelix") { + state.chassis.direction = "straight"; + } + state.chassis.actionDone = false; + } + state.chassis.actionDone = false; + } + } + + //Sector 38 movement + if (state.api.sector == "38") { + if (DEBUGGER == true) Serial.println("Sector 38"); + vehicleChassis->motorControl(state.sonar.sonarFactor); + if (state.chassis.actionDone == true) { + state.api.sector = "40"; + if (state.api.vehicleID == "Idefix") { + state.chassis.direction = "left"; + } + else if (state.api.vehicleID == "Obelix") { + state.chassis.direction = "straight"; + } + state.chassis.actionDone = false; + } + } + + //Sector 40 movement + if (state.api.sector == "40") { + vehicleChassis->motorControl(state.sonar.sonarFactor); + if (DEBUGGER == true) Serial.println("Sector 40"); + if (state.chassis.actionDone == true) { + if (state.api.vehicleID == "Obelix") { + if (DEBUGGER == true) Serial.println("Sector 40 Obelix finished"); + state.chassis.direction = "stop"; + state.api.sector = "42"; + } + else if (state.api.vehicleID == "Idefix") { + if (DEBUGGER == true) Serial.println("Sector 40 Idefix finished"); + state.chassis.direction = "right"; + state.api.sector = "41"; + } + state.chassis.actionDone = false; + } + } + + //Sector 41 movement + if (state.api.sector == "41") { + vehicleChassis->motorControl(state.sonar.sonarFactor); + if (DEBUGGER == true) Serial.print("Sector 41 Actiondone: "); if (DEBUGGER == true) Serial.println(state.chassis.actionDone); + if (state.chassis.actionDone == true) { + state.chassis.direction = "stop"; + if (DEBUGGER == true) Serial.println("Sector 41 finished"); + state.api.sector = "42"; + state.chassis.actionDone = false; + } + } + + //Sector 42 movement + if (state.api.sector == "42") { + vehicleChassis->motorControl(state.sonar.sonarFactor); + if (DEBUGGER == true) Serial.print("Sector 42 Actiondone: "); if (DEBUGGER == true) Serial.println(state.chassis.actionDone); + if (state.chassis.actionDone == true) { + state.chassis.direction = "stop"; + if (DEBUGGER == true) Serial.println("Sector 42 finished"); + vehicleChassis->stop(); + state.api.sector = "43"; + state.chassis.actionDone = false; + } + } + + //Sector 43 movement reset the variables once the task is finished + if (state.api.sector == "43") { + if (DEBUGGER == true) Serial.print("Sector 43 Actiondone: "); if (DEBUGGER == true) Serial.println(state.chassis.actionDone); + if (state.chassis.actionDone == false) { + vehicleChassis->turnAround(2); + if (DEBUGGER == true) Serial.println("Sector 43 after turnaround"); + } + if (state.chassis.actionDone == true) { + if (DEBUGGER == true) Serial.println("Sector 43 finished"); + state.api.sector = "10"; + state.api.state = "resting"; + state.api.task = "finished"; + state.api.workState = false; + state.api.workStateChange = true; + state.chassis.actionDone = false; + state.waitCount = 0; + state.turnCount = 1; + state.waitForTurn = false; + state.vision.targetDetected = false; + state.api.jobCount++; + state.vision.reset = true; + state.api.container = ""; + state.api.unloadPort= ""; + state.api.containerChange = true; + state.api.unloadPortChange = true; + } + + } + } + else { + vehicleChassis->stop(); + } +} + + +void setup() { + //Initialize serial and wait for port to open: + if (DEBUGGER == true) Serial.begin(9600); + //while (!if (DEBUGGER == true) Serial) { + //; // wait foserial port to connect. Needed for native USB port only + //} + if (DEBUGGER == true) Serial.println("Vehicle: booting..."); + vehicleChassis = new Chassis(SPEED, K_P, K_I, K_D, RIGHT_MOTOR, LEFT_MOTOR, PIN_SENSOR_0, PIN_SENSOR_1, PIN_SENSOR_2, PIN_SENSOR_3, PIN_SENSOR_4); + state.chassis.speed = SPEED; + delay(1000); + vehicleSonar = new Sonar(SONAR_SERVO_PIN, SONAR_TRIGGER_PIN, SONAR_ECHO_PIN, SONAR_MAX_DISTANCE, MIN_ERROR, MAX_ERROR, MIN_TURN_ANGLE, MAX_TURN_ANGLE); + delay(1000); + vehicleVision = new Vision(VISION_START_ANGLE, VISION_SERVO_PIN, VISION_DELAY_FACTOR, VISION_TOLERANCE_LEFT, VISION_TOLERANCE_RIGHT); + delay(1000); + vehicleHoist = new Hoist(HOIST_SERVO_PIN, HOIST_SERVO_DELAY, HOIST_POISITION_MAX, HOIST_POSITION_MIN); + delay(1000); + vehicleAPI = new VehicleWebAPI(&state.api); + delay(1000); + if (DEBUGGER == true) Serial.println("Booting complete!"); +} + + + + +void loop() { + vehicleAPI->loop(&state.api); + if (state.api.workState == true) { + state.sonarCount++; + if ((state.api.sector == "20") && (state.visual == true)) { + vehicleVision->loop(&state.vision); + } + vehicleChassis->loop(&state.chassis); + if (state.sonarCount > 30) { + state.sonarCount = 0; + vehicleSonar->loop(&state.sonar, state.chassis.directionError, state.turnSonar); + } + vehicleHoist->loop(&state.hoist); + } + strategy(); + if (DEBUGGER == true) Serial.println("-----------------------------------------"); +} + +*/ \ No newline at end of file