Skip to content

Commit

Permalink
Merge pull request #96 from ad101-lab/ad-tank-simulator
Browse files Browse the repository at this point in the history
Tank drive pure pursuit simulator
  • Loading branch information
alexDickhans authored Jan 13, 2022
2 parents 9a370fe + 5fb0f85 commit 6982a10
Show file tree
Hide file tree
Showing 30 changed files with 744 additions and 315 deletions.
195 changes: 103 additions & 92 deletions .vscode/settings.json
Original file line number Diff line number Diff line change
@@ -1,95 +1,106 @@
{
"files.associations": {
"ios": "cpp",
"type_traits": "cpp",
"cmath": "cpp",
"valarray": "cpp",
"hash_map": "cpp",
"deque": "cpp",
"forward_list": "cpp",
"list": "cpp",
"string": "cpp",
"unordered_map": "cpp",
"unordered_set": "cpp",
"vector": "cpp",
"system_error": "cpp",
"__hash_table": "cpp",
"__split_buffer": "cpp",
"__tree": "cpp",
"filesystem": "cpp",
"map": "cpp",
"pros": "json",
"set": "cpp",
"array": "cpp",
"atomic": "cpp",
"bit": "cpp",
"*.tcc": "cpp",
"bitset": "cpp",
"cctype": "cpp",
"compare": "cpp",
"cstdarg": "cpp",
"cstddef": "cpp",
"cstdint": "cpp",
"cstdio": "cpp",
"cstdlib": "cpp",
"cstring": "cpp",
"ctime": "cpp",
"cwchar": "cpp",
"cwctype": "cpp",
"exception": "cpp",
"algorithm": "cpp",
"functional": "cpp",
"iterator": "cpp",
"memory": "cpp",
"memory_resource": "cpp",
"random": "cpp",
"string_view": "cpp",
"tuple": "cpp",
"utility": "cpp",
"fstream": "cpp",
"initializer_list": "cpp",
"iosfwd": "cpp",
"iostream": "cpp",
"istream": "cpp",
"limits": "cpp",
"new": "cpp",
"ostream": "cpp",
"sstream": "cpp",
"stdexcept": "cpp",
"streambuf": "cpp",
"cinttypes": "cpp",
"typeindex": "cpp",
"typeinfo": "cpp",
"chrono": "cpp",
"clocale": "cpp",
"codecvt": "cpp",
"concepts": "cpp",
"condition_variable": "cpp",
"numeric": "cpp",
"optional": "cpp",
"ratio": "cpp",
"iomanip": "cpp",
"mutex": "cpp",
"numbers": "cpp",
"semaphore": "cpp",
"stop_token": "cpp",
"thread": "cpp",
"variant": "cpp",
"__bit_reference": "cpp",
"__bits": "cpp",
"__config": "cpp",
"__debug": "cpp",
"__errc": "cpp",
"__functional_base": "cpp",
"__locale": "cpp",
"__mutex_base": "cpp",
"__node_handle": "cpp",
"__nullptr": "cpp",
"__string": "cpp",
"__threading_support": "cpp",
"__tuple": "cpp",
"locale": "cpp",
"queue": "cpp",
"stack": "cpp"
}
"ios": "cpp",
"type_traits": "cpp",
"cmath": "cpp",
"valarray": "cpp",
"hash_map": "cpp",
"deque": "cpp",
"forward_list": "cpp",
"list": "cpp",
"string": "cpp",
"unordered_map": "cpp",
"unordered_set": "cpp",
"vector": "cpp",
"system_error": "cpp",
"__hash_table": "cpp",
"__split_buffer": "cpp",
"__tree": "cpp",
"filesystem": "cpp",
"map": "cpp",
"pros": "json",
"set": "cpp",
"array": "cpp",
"atomic": "cpp",
"bit": "cpp",
"*.tcc": "cpp",
"bitset": "cpp",
"cctype": "cpp",
"compare": "cpp",
"cstdarg": "cpp",
"cstddef": "cpp",
"cstdint": "cpp",
"cstdio": "cpp",
"cstdlib": "cpp",
"cstring": "cpp",
"ctime": "cpp",
"cwchar": "cpp",
"cwctype": "cpp",
"exception": "cpp",
"algorithm": "cpp",
"functional": "cpp",
"iterator": "cpp",
"memory": "cpp",
"memory_resource": "cpp",
"random": "cpp",
"string_view": "cpp",
"tuple": "cpp",
"utility": "cpp",
"fstream": "cpp",
"initializer_list": "cpp",
"iosfwd": "cpp",
"iostream": "cpp",
"istream": "cpp",
"limits": "cpp",
"new": "cpp",
"ostream": "cpp",
"sstream": "cpp",
"stdexcept": "cpp",
"streambuf": "cpp",
"cinttypes": "cpp",
"typeindex": "cpp",
"typeinfo": "cpp",
"chrono": "cpp",
"clocale": "cpp",
"codecvt": "cpp",
"concepts": "cpp",
"condition_variable": "cpp",
"numeric": "cpp",
"optional": "cpp",
"ratio": "cpp",
"iomanip": "cpp",
"mutex": "cpp",
"numbers": "cpp",
"semaphore": "cpp",
"stop_token": "cpp",
"thread": "cpp",
"variant": "cpp",
"__bit_reference": "cpp",
"__bits": "cpp",
"__config": "cpp",
"__debug": "cpp",
"__errc": "cpp",
"__functional_base": "cpp",
"__locale": "cpp",
"__mutex_base": "cpp",
"__node_handle": "cpp",
"__nullptr": "cpp",
"__string": "cpp",
"__threading_support": "cpp",
"__tuple": "cpp",
"locale": "cpp",
"queue": "cpp",
"stack": "cpp",
"any": "cpp",
"cfenv": "cpp",
"charconv": "cpp",
"complex": "cpp",
"csetjmp": "cpp",
"csignal": "cpp",
"cuchar": "cpp",
"regex": "cpp",
"future": "cpp",
"scoped_allocator": "cpp",
"shared_mutex": "cpp"
}
}
36 changes: 19 additions & 17 deletions include/autoPaths.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -34,10 +34,10 @@ namespace Pronounce {
int nearPlatformViaLeftNeutralToFarPlatformIndex;
int nearPlatformToMidIndex;

void autoPaths(PurePursuit purePursuit) {
void autoPaths(PurePursuit* purePursuit) {
// Default pure pursuit profile
PurePursuitProfile defaultProfile(new PID(20, 0.0, 2.0), new PID(60.0, 0.0, 5.0), 10.0);
purePursuit.getPurePursuitProfileManager().setDefaultProfile(defaultProfile);
purePursuit->getPurePursuitProfileManager().setDefaultProfile(defaultProfile);

// Test path
Path testPath = Path();
Expand All @@ -49,73 +49,73 @@ namespace Pronounce {
testPath.addPoint(-24, 48);
testPath.addPoint(0, 24);

testPathIndex = purePursuit.addPath(testPath);
testPathIndex = purePursuit->addPath(testPath);

Path rightNeutralToRightHomeZone;

rightNeutralToRightHomeZone.addPoint(105.7, 60);
rightNeutralToRightHomeZone.addPoint(105.7, 16);

rightNeutralToRightHomeIndex = purePursuit.addPath(rightNeutralToRightHomeZone);
rightNeutralToRightHomeIndex = purePursuit->addPath(rightNeutralToRightHomeZone);

// Right Steal Right
Path rightHomeToGoalNeutral;

rightHomeToGoalNeutral.addPoint(105.7, 16);
rightHomeToGoalNeutral.addPoint(105.7, 61);

rightHomeToGoalNeutralIndex = purePursuit.addPath(rightHomeToGoalNeutral);
rightHomeToGoalNeutralIndex = purePursuit->addPath(rightHomeToGoalNeutral);

Path rightNeutralToMidNeutral;
SplinePath rightNeutralToMidNeutral;

rightNeutralToMidNeutral.addPoint(105.7, 62);
rightNeutralToMidNeutral.addPoint(75.3, 40);
rightNeutralToMidNeutral.addPoint(60.3, 65);

rightNeutralToMidNeutralIndex = purePursuit.addPath(rightNeutralToMidNeutral);
rightNeutralToMidNeutralIndex = purePursuit->addPath(rightNeutralToMidNeutral.getPath(0.1));

Path midNeutralToRightAlliance;

midNeutralToRightAlliance.addPoint(70.3, 65);
midNeutralToRightAlliance.addPoint(120.1, 28);

midNeutralToRightAllianceIndex = purePursuit.addPath(midNeutralToRightAlliance);
midNeutralToRightAllianceIndex = purePursuit->addPath(midNeutralToRightAlliance);

Path midNeutralToMidHomeZone;

midNeutralToMidHomeZone.addPoint(70.3, 70.3);
midNeutralToMidHomeZone.addPoint(70.3, 36);

midNeutralToMidHomeZoneIndex = purePursuit.addPath(midNeutralToMidHomeZone);
midNeutralToMidHomeZoneIndex = purePursuit->addPath(midNeutralToMidHomeZone);

Path farRightHomeZoneToRightAlliance;

farRightHomeZoneToRightAlliance.addPoint(127.9, 16);
farRightHomeZoneToRightAlliance.addPoint(127.9, 24);

farRightHomeZoneToRightAllianceIndex = purePursuit.addPath(farRightHomeZoneToRightAlliance);
farRightHomeZoneToRightAllianceIndex = purePursuit->addPath(farRightHomeZoneToRightAlliance);

Path rightAllianceToRightHomeZone;

rightAllianceToRightHomeZone.addPoint(127.9, 24);
rightAllianceToRightHomeZone.addPoint(105.7, 16);

rightAllianceToRightHomeZoneIndex = purePursuit.addPath(rightAllianceToRightHomeZone);
rightAllianceToRightHomeZoneIndex = purePursuit->addPath(rightAllianceToRightHomeZone);

Path leftAllianceToLeftNeutral;

leftAllianceToLeftNeutral.addPoint(29, 11.4);
leftAllianceToLeftNeutral.addPoint(32, 67);

leftAllianceToLeftNeutralIndex = purePursuit.addPath(leftAllianceToLeftNeutral);
leftAllianceToLeftNeutralIndex = purePursuit->addPath(leftAllianceToLeftNeutral);

Path leftNeutralToMidNeutral;

leftNeutralToMidNeutral.addPoint(32, 67);
leftNeutralToMidNeutral.addPoint(65.3, 40);
leftNeutralToMidNeutral.addPoint(70.3, 65);

leftNeutralToMidNeutralIndex = purePursuit.addPath(leftNeutralToMidNeutral);
leftNeutralToMidNeutralIndex = purePursuit->addPath(leftNeutralToMidNeutral);

// mid neutral to mid home zone

Expand All @@ -126,7 +126,7 @@ namespace Pronounce {
rightNeutralToFarPlatform.addPoint(75, 100);
rightNeutralToFarPlatform.addPoint(60.3, 115);

rightNeutralToFarPlatformIndex = purePursuit.addPath(rightNeutralToFarPlatform);
rightNeutralToFarPlatformIndex = purePursuit->addPath(rightNeutralToFarPlatform);

Path farPlatformToNearPlatform;

Expand All @@ -136,22 +136,24 @@ namespace Pronounce {
farPlatformToNearPlatform.addPoint(58.6, 45);
farPlatformToNearPlatform.addPoint(70.3, 30.7);

farPlatformToNearPlatformIndex = purePursuit.addPath(farPlatformToNearPlatform);
farPlatformToNearPlatformIndex = purePursuit->addPath(farPlatformToNearPlatform);

Path nearPlatformViaLeftNeutralToFarPlatform;

nearPlatformViaLeftNeutralToFarPlatform.addPoint(70.3, 30.7);
nearPlatformViaLeftNeutralToFarPlatform.addPoint(35, 61);
nearPlatformViaLeftNeutralToFarPlatform.addPoint(70.3, 115);

nearPlatformViaLeftNeutralToFarPlatformIndex = purePursuit.addPath(nearPlatformViaLeftNeutralToFarPlatform);
nearPlatformViaLeftNeutralToFarPlatformIndex = purePursuit->addPath(nearPlatformViaLeftNeutralToFarPlatform);

Path nearPlatformToMid;

nearPlatformToMid.addPoint(70.3, 115);
nearPlatformToMid.addPoint(70.3, 70.3);

nearPlatformToMidIndex = purePursuit.addPath(nearPlatformToMid);
nearPlatformToMidIndex = purePursuit->addPath(nearPlatformToMid);

printf("Array size: %d\n", purePursuit->getPaths().size());
}

} // Namespace Prononce
17 changes: 17 additions & 0 deletions include/chassis/abstractDrivetrain.hpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,17 @@
#pragma once

namespace Pronounce {
/**
* @brief Abstract class to be used in both the pure pursuit simulator and on the robot.
*
*/
class AbstractDrivetrain {
private:

public:
AbstractDrivetrain();

~AbstractDrivetrain();
};

} // namespace Pronounce
44 changes: 44 additions & 0 deletions include/chassis/abstractTankDrivetrain.hpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,44 @@
#pragma once

#include "abstractDrivetrain.hpp"
#include "utils/utils.hpp"

namespace Pronounce {
class AbstractTankDrivetrain : public AbstractDrivetrain {
private:
double trackWidth;
public:
AbstractTankDrivetrain();
AbstractTankDrivetrain(double trackWidth);

double getTrackWidth() {
return trackWidth;
}

void setTrackWidth(double trackWidth) {
this->trackWidth = trackWidth;
}

void driveCurvature(double speed, double curvature) {
double leftSpeed = speed * (2.0 - curvature * trackWidth) / 2.0;
double rightSpeed = speed * (2.0 + curvature * trackWidth) / 2.0;

double maxSpeed = max(abs(leftSpeed), abs(rightSpeed));

if (maxSpeed > abs(speed)) {
double multiplier = abs(speed) / maxSpeed;
leftSpeed *= multiplier;
rightSpeed *= multiplier;
}

this->tankSteerVelocity(leftSpeed, rightSpeed);
}

virtual void skidSteerVelocity(double speed, double turn) {}

virtual void tankSteerVelocity(double leftSpeed, double rightSpeed) {}

~AbstractTankDrivetrain();
};
} // namespace Pronounce

Loading

0 comments on commit 6982a10

Please sign in to comment.