Skip to content

Commit

Permalink
Merge pull request #89 from ad101-lab/ad-tank-pure-pursuit
Browse files Browse the repository at this point in the history 
Tank drive pure pursuit
  • Loading branch information
@alexDickhans
alexDickhans authored Jan 13, 2022
2 parents 06381d2 + 6982a10 commit f0da4b2
Show file tree
Hide file tree
Showing 50 changed files with 1,612 additions and 875 deletions.
3 changes: 1 addition & 2 deletions .gitignore
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,6 @@
# Executables
*.bin
*.elf
pathTest

# PROS
bin/
Expand All @@ -14,4 +13,4 @@ compile_commands.json
temp.log
temp.errors
*.ini
.d/
.d/
195 changes: 103 additions & 92 deletions .vscode/settings.json
View file
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"
}
}
Binary file modified firmware/libpros.a
View file
Binary file not shown.
4 changes: 2 additions & 2 deletions include/api.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,8 +41,8 @@

#define PROS_VERSION_MAJOR 3
#define PROS_VERSION_MINOR 5
#define PROS_VERSION_PATCH 3
#define PROS_VERSION_STRING "3.5.3"
#define PROS_VERSION_PATCH 4
#define PROS_VERSION_STRING "3.5.4"

#define PROS_ERR (INT32_MAX)
#define PROS_ERR_F (INFINITY)
Expand Down
159 changes: 159 additions & 0 deletions include/autoPaths.hpp
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,159 @@
#pragma once

#include "utils/pointUtil.hpp"
#include "utils/path.hpp"
#include "utils/splinePath.hpp"
#include "motionControl/purePursuit.hpp"
#include "utils/purePursuitProfile.hpp"
#include "utils/purePursuitProfileManager.hpp"

namespace Pronounce {

// Test path
int testPathIndex;

// Right steal right
int rightHomeToGoalNeutralIndex;
int rightNeutralToMidNeutralIndex;
int midNeutralToRightAllianceIndex;
int midNeutralToMidHomeZoneIndex;
int rightNeutralToRightHomeIndex;

// Right awp right
int farRightHomeZoneToRightAllianceIndex;
int rightAllianceToRightHomeZoneIndex;

// Left steal left
int leftAllianceToLeftNeutralIndex;
int leftNeutralToMidNeutralIndex;
int midNeutralToLeftHomeZoneIndex;

// Skills
int rightNeutralToFarPlatformIndex;
int farPlatformToNearPlatformIndex;
int nearPlatformViaLeftNeutralToFarPlatformIndex;
int nearPlatformToMidIndex;

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);

// Test path
Path testPath = Path();

testPath.addPoint(0, 0);
testPath.addPoint(24, 0);
testPath.addPoint(24, 24);
testPath.addPoint(24, 48);
testPath.addPoint(-24, 48);
testPath.addPoint(0, 24);

testPathIndex = purePursuit->addPath(testPath);

Path rightNeutralToRightHomeZone;

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

rightNeutralToRightHomeIndex = purePursuit->addPath(rightNeutralToRightHomeZone);

// Right Steal Right
Path rightHomeToGoalNeutral;

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

rightHomeToGoalNeutralIndex = purePursuit->addPath(rightHomeToGoalNeutral);

SplinePath rightNeutralToMidNeutral;

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

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

Path midNeutralToRightAlliance;

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

midNeutralToRightAllianceIndex = purePursuit->addPath(midNeutralToRightAlliance);

Path midNeutralToMidHomeZone;

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

midNeutralToMidHomeZoneIndex = purePursuit->addPath(midNeutralToMidHomeZone);

Path farRightHomeZoneToRightAlliance;

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

farRightHomeZoneToRightAllianceIndex = purePursuit->addPath(farRightHomeZoneToRightAlliance);

Path rightAllianceToRightHomeZone;

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

rightAllianceToRightHomeZoneIndex = purePursuit->addPath(rightAllianceToRightHomeZone);

Path leftAllianceToLeftNeutral;

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

leftAllianceToLeftNeutralIndex = purePursuit->addPath(leftAllianceToLeftNeutral);

Path leftNeutralToMidNeutral;

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

leftNeutralToMidNeutralIndex = purePursuit->addPath(leftNeutralToMidNeutral);

// mid neutral to mid home zone

Path rightNeutralToFarPlatform;

rightNeutralToFarPlatform.addPoint(105.7, 61);
rightNeutralToFarPlatform.addPoint(75, 76.5);
rightNeutralToFarPlatform.addPoint(75, 100);
rightNeutralToFarPlatform.addPoint(60.3, 115);

rightNeutralToFarPlatformIndex = purePursuit->addPath(rightNeutralToFarPlatform);

Path farPlatformToNearPlatform;

farPlatformToNearPlatform.addPoint(70.3, 107);
farPlatformToNearPlatform.addPoint(60, 70.3);
farPlatformToNearPlatform.addPoint(58.6, 64.1);
farPlatformToNearPlatform.addPoint(58.6, 45);
farPlatformToNearPlatform.addPoint(70.3, 30.7);

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);

Path nearPlatformToMid;

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

nearPlatformToMidIndex = purePursuit->addPath(nearPlatformToMid);

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

} // Namespace Prononce
Loading

0 comments on commit f0da4b2

Please sign in to comment.