Skip to content

Instantly share code, notes, and snippets.

@victoryforphil

victoryforphil/Details

Last active March 19, 2018 21:31
Show Gist options
  • Save victoryforphil/503d12623d115a45f7c9a641038168a7 to your computer and use it in GitHub Desktop.
Save victoryforphil/503d12623d115a45f7c9a641038168a7 to your computer and use it in GitHub Desktop.
Download ZIP
Pathfinder #1
Raw
Details
Encoder Left: no reversal
Encoder Right: no reversal
Profile:
#pragma once
#pragma once
namespace DriveProfile {
const double WHEELBASE_WIDTH = 0.6;
const double WHEEL_CIRCUMFERENCE = 18.49;
const double MAX_VELOCITY = 5;
const double ENCODER_TICKS_PER_REV = 40;
const double GEAR_RATIO = 0.0934;
const double TICKS_PER_REV = ENCODER_TICKS_PER_REV * GEAR_RATIO;
}
Action:
Spins smoothly in a circle.
Raw
Logs Sample
ed: 0 Turn: -0.000100098 Dif: 0.0100098
Pathfinder: L: 0.150858 R: 0.150858 Gyro: -0.0100098 Desired: 0 Turn: -0.000100098 Dif: 0.0100098
Pathfinder: L: 0.18549 R: 5.11616 Gyro: -0.0100098 Desired: 0 Turn: -0.000100098 Dif: 0.0100098
Pathfinder: L: 10.0856 R: 19.9469 Gyro: -0.0100098 Desired: 0 Turn: -0.000100098 Dif: 0.0100098
Pathfinder: L: 34.7807 R: 59.434 Gyro: -0.0100098 Desired: 0 Turn: -0.000100098 Dif: 0.0100098
Pathfinder: L: 89.0616 R: 89.0616 Gyro: 0.01 Desired: 0 Turn: 0.0001 Dif: -0.01
Pathfinder: L: 143.344 R: 118.691 Gyro: 0.59 Desired: 0 Turn: 0.0059 Dif: -0.59
CS: USB Camera 0: Connecting to USB camera on /dev/video0
Pathfinder: L: 197.629 R: 143.391 Gyro: 1.27 Desired: 0 Turn: 0.0127 Dif: -1.27
Pathfinder: L: 256.845 R: 173.024 Gyro: 2.13 Desired: 0 Turn: 0.0213 Dif: -2.13
Pathfinder: L: 306.203 R: 207.589 Gyro: 4.37 Desired: 0 Turn: 0.0437 Dif: -4.37
Pathfinder: L: 360.492 R: 242.156 Gyro: 5.76 Desired: 0 Turn: 0.0576 Dif: -5.76
Pathfinder: L: 429.576 R: 276.725 Gyro: 8.69 Desired: 0 Turn: 0.0869 Dif: -8.69
Pathfinder: L: 562.76 R: 375.394 Gyro: 12.06 Desired: 0 Turn: 0.1206 Dif: -12.06
Pathfinder: L: 607.193 R: 409.967 Gyro: 15.8 Desired: 0 Turn: 0.158 Dif: -15.8
Pathfinder: L: 686.143 R: 454.402 Gyro: 17.76 Desired: 0 Turn: 0.1776 Dif: -17.76
Pathfinder: L: 755.234 R: 498.839 Gyro: 19.8 Desired: 0 Turn: 0.198 Dif: -19.8
Pathfinder: L: 819.396 R: 548.209 Gyro: 21.93 Desired: 0 Turn: 0.2193 Dif: -21.93
Pathfinder: L: 883.559 R: 592.65 Gyro: 26.31 Desired: 0 Turn: 0.2631 Dif: -26.31
Pathfinder: L: 962.516 R: 642.023 Gyro: 28.59 Desired: 0 Turn: 0.2859 Dif: -28.59
Pathfinder: L: 1031.61 R: 696.328 Gyro: 33.25 Desired: 0 Turn: 0.3325 Dif: -33.25
Pathfinder: L: 1174.67 R: 790.081 Gyro: 38.02 Desired: 0 Turn: 0.3802 Dif: -38.02
Pathfinder: L: 1258.57 R: 844.39 Gyro: 42.98 Desired: 0 Turn: 0.4298 Dif: -42.98
Pathfinder: L: 1322.74 R: 888.84 Gyro: 45.5 Desired: 0 Turn: 0.455 Dif: -45.5
Pathfinder: L: 1396.77 R: 943.152 Gyro: 48.04 Desired: 0 Turn: 0.4804 Dif: -48.04
Pathfinder: L: 1475.74 R: 997.467 Gyro: 53.23 Desired: 0 Turn: 0.5323 Dif: -53.23
Pathfinder: L: 1559.64 R: 1056.71 Gyro: 55.83 Desired: 0 Turn: 0.5583 Dif: -55.83
Pathfinder: L: 1623.82 R: 1101.17 Gyro: 58.51 Desired: 0 Turn: 0.5851 Dif: -58.51
Pathfinder: L: 1702.79 R: 1160.42 Gyro: 63.88 Desired: 0 Turn: 0.6388 Dif: -63.88
Pathfinder: L: 1835.99 R: 1268.97 Gyro: 69.3 Desired: 0 Turn: 0.693 Dif: -69.3
Pathfinder: L: 1929.75 R: 1338.07 Gyro: 74.81 Desired: 0 Turn: 0.7481 Dif: -74.81
Pathfinder: L: 2003.79 R: 1397.32 Gyro: 77.64 Desired: 0 Turn: 0.7764 Dif: -77.64
Pathfinder: L: 2082.75 R: 1456.56 Gyro: 80.45 Desired: 0 Turn: 0.8045 Dif: -80.45
Pathfinder: L: 2156.78 R: 1520.73 Gyro: 83.22 Desired: 0 Turn: 0.8322 Dif: -83.22
Pathfinder: L: 2235.74 R: 1584.9 Gyro: 88.98 Desired: 0 Turn: 0.8898 Dif: -88.98
Pathfinder: L: 2309.77 R: 1644.13 Gyro: 91.83 Desired: 0 Turn: 0.9183 Dif: -91.83
Pathfinder: L: 2393.66 R: 1708.3 Gyro: 94.68 Desired: 0 Turn: 0.9468 Dif: -94.68
Pathfinder: L: 2472.61 R: 1767.53 Gyro: 100.52 Desired: 0 Turn: 1.0052 Dif: -100.52
Pathfinder: L: 2551.56 R: 1831.69 Gyro: 103.43 Desired: 0 Turn: 1.0343 Dif: -103.43
Pathfinder: L: 2640.38 R: 1900.78 Gyro: 109.31 Desired: 0 Turn: 1.0931 Dif: -109.31
Pathfinder: L: 2719.33 R: 1960 Gyro: 112.24 Desired: 0 Turn: 1.1224 Dif: -112.24
Pathfinder: L: 2798.28 R: 2014.3 Gyro: 115.17 Desired: 0 Turn: 1.1517 Dif: -115.17
Pathfinder: L: 2872.3 R: 2073.53 Gyro: 121.13 Desired: 0 Turn: 1.2113 Dif: -121.13
Pathfinder: L: 2961.11 R: 2137.69 Gyro: 124.03 Desired: 0 Turn: 1.2403 Dif: -124.03
Pathfinder: L: 3040.06 R: 2196.92 Gyro: 127 Desired: 0 Turn: 1.27 Dif: -127
Pathfinder: L: 3163.39 R: 2290.66 Gyro: 132.95 Desired: 0 Turn: 1.3295 Dif: -132.95
Pathfinder: L: 3212.75 R: 2325.23 Gyro: 135.91 Desired: 0 Turn: 1.3591 Dif: -135.91
Pathfinder: L: 3296.64 R: 2389.39 Gyro: 138.93 Desired: 0 Turn: 1.3893 Dif: -138.93
Pathfinder: L: 3380.52 R: 2443.69 Gyro: 144.92 Desired: 0 Turn: 1.4492 Dif: -144.92
Pathfinder: L: 3464.4 R: 2507.85 Gyro: 147.95 Desired: 0 Turn: 1.4795 Dif: -147.95
Pathfinder: L: 3553.21 R: 2572.01 Gyro: 150.93 Desired: 0 Turn: 1.5093 Dif: -150.93
Pathfinder: L: 3632.16 R: 2631.24 Gyro: 156.95 Desired: 0 Turn: 1.5695 Dif: -156.95
Pathfinder: L: 3716.04 R: 2690.46 Gyro: 159.93 Desired: 0 Turn: 1.5993 Dif: -159.93
Pathfinder: L: 3804.85 R: 2749.69 Gyro: 162.88 Desired: 0 Turn: 1.6288 Dif: -162.88
Pathfinder: L: 3888.74 R: 2808.92 Gyro: 168.92 Desired: 0 Turn: 1.6892 Dif: -168.92
Pathfinder: L: 3972.62 R: 2873.08 Gyro: 171.93 Desired: 0 Turn: 1.7193 Dif: -171.93
Pathfinder: L: 4066.36 R: 2932.31 Gyro: 174.89 Desired: 0 Turn: 1.7489 Dif: -174.89
Pathfinder: L: 4155.17 R: 2996.46 Gyro: -179.1 Desired: 0 Turn: -1.791 Dif: 179.1
Pathfinder: L: 4239.05 R: 3055.69 Gyro: -176.09 Desired: 0 Turn: -1.7609 Dif: 176.09
Pathfinder: L: 4327.86 R: 3114.92 Gyro: -173.06 Desired: 0 Turn: -1.7306 Dif: 173.06
Pathfinder: L: 4421.61 R: 3179.08 Gyro: -166.98 Desired: 0 Turn: -1.6698 Dif: 166.98
Pathfinder: L: 4500.56 R: 3233.38 Gyro: -163.96 Desired: 0 Turn: -1.6396 Dif: 163.96
Pathfinder: L: 4584.44 R: 3292.6 Gyro: -160.94 Desired: 0 Turn: -1.6094 Dif: 160.94
Pathfinder: L: 4678.18 R: 3346.9 Gyro: -155.03 Desired: 0 Turn: -1.5503 Dif: 155.03
Pathfinder: L: 4757.13 R: 3411.06 Gyro: -151.96 Desired: 0 Turn: -1.5196 Dif: 151.96
Pathfinder: L: 4845.94 R: 3470.29 Gyro: -148.99 Desired: 0 Turn: -1.4899 Dif: 148.99
Pathfinder: L: 4944.62 R: 3529.52 Gyro: -143.13 Desired: 0 Turn: -1.4313 Dif: 143.13
Pathfinder: L: 5023.57 R: 3578.88 Gyro: -140.1 Desired: 0 Turn: -1.401 Dif: 140.1
Pathfinder: L: 5107.45 R: 3647.97 Gyro: -137.1 Desired: 0 Turn: -1.371 Dif: 137.1
Pathfinder: L: 5196.26 R: 3717.06 Gyro: -131.28 Desired: 0 Turn: -1.3128 Dif: 131.28
Pathfinder: L: 5280.14 R: 3776.29 Gyro: -128.35 Desired: 0 Turn: -1.2835 Dif: 128.35
Pathfinder: L: 5359.09 R: 3830.59 Gyro: -125.43 Desired: 0 Turn: -1.2543 Dif: 125.43
Pathfinder: L: 5447.91 R: 3894.75 Gyro: -119.57 Desired: 0 Turn: -1.1957 Dif: 119.57
Pathfinder: L: 5526.86 R: 3963.84 Gyro: -116.59 Desired: 0 Turn: -1.1659 Dif: 116.59
Pathfinder: L: 5615.67 R: 4032.92 Gyro: -113.7 Desired: 0 Turn: -1.137 Dif: 113.7
Pathfinder: L: 5689.69 R: 4087.22 Gyro: -107.88 Desired: 0 Turn: -1.0788 Dif: 107.88
Pathfinder: L: 5773.57 R: 4151.38 Gyro: -104.96 Desired: 0 Turn: -1.0496 Dif: 104.96
Pathfinder: L: 5857.45 R: 4210.61 Gyro: -101.98 Desired: 0 Turn: -1.0198 Dif: 101.98
Pathfinder: L: 5941.33 R: 4279.7 Gyro: -96.04 Desired: 0 Turn: -0.9604 Dif: 96.04
Pathfinder: L: 6020.28 R: 4353.72 Gyro: -93.1 Desired: 0 Turn: -0.931 Dif: 93.1
Pathfinder: L: 6114.03 R: 4432.67 Gyro: -90.17 Desired: 0 Turn: -0.9017 Dif: 90.17
Pathfinder: L: 6183.12 R: 4486.97 Gyro: -84.32 Desired: 0 Turn: -0.8432 Dif: 84.32
Pathfinder: L: 6276.86 R: 4546.19 Gyro: -81.36 Desired: 0 Turn: -0.8136 Dif: 81.36
Pathfinder: L: 6360.74 R: 4600.49 Gyro: -78.41 Desired: 0 Turn: -0.7841 Dif: 78.41
Pathfinder: L: 6434.76 R: 4674.51 Gyro: -72.43 Desired: 0 Turn: -0.7243 Dif: 72.43
Raw
PathfinderCommand.cpp
#include "TestPathfinder.h"
#include "Robot.h"
#include "RoboMap.h"
#include <stdlib.h>
using namespace std;
TestPathfinder::TestPathfinder()
{
Requires(Robot::drivetrain.get());
SetTimeout(30);
}
void TestPathfinder::Initialize()
{
RoboMap::navX->Reset();
int POINT_LENGTH = 3;
Waypoint *points = new Waypoint[POINT_LENGTH];
Waypoint p1 = { 1, 0 , d2r(0) }; // Waypoint @ x=-4, y=-1, exit angle=45 degrees
Waypoint p2 = { 3,0,d2r(0) }; // Waypoint @ x=-1, y= 2, exit angle= 0 radians
Waypoint p3 = { 10,0, 0 }; // Waypoint @ x= 2, y= 4, exit angle= 0 radians
points[0] = p1;
points[1] = p2;
points[2] = p3;
TrajectoryCandidate candidate;
pathfinder_prepare(points, POINT_LENGTH, FIT_HERMITE_CUBIC, PATHFINDER_SAMPLES_HIGH, 0.015, DriveProfile::MAX_VELOCITY, 10.0, 60.0, &candidate);
length = candidate.length;
Segment *trajectory = new Segment[length];
std::cout << "Generating... " << std::endl;
pathfinder_generate(&candidate, trajectory);
std::cout << "Mofiying... " << std::endl;
leftTrajectory.reset(new Segment[length]);
rightTrajectory.reset(new Segment[length]);
pathfinder_modify_tank(trajectory, length, leftTrajectory.get(), rightTrajectory.get(), DriveProfile::WHEELBASE_WIDTH);
leftFollower.last_error = 0;
leftFollower.segment = 0;
leftFollower.finished = 0;
rightFollower.last_error = 0;
rightFollower.segment = 0;
rightFollower.finished = 0;
configL = { Robot::drivetrain->GetEncoderLDistance(), DriveProfile::TICKS_PER_REV, DriveProfile::WHEEL_CIRCUMFERENCE ,0.8,0.0,0.0,1.0 / DriveProfile::MAX_VELOCITY, 0 };
configR = { Robot::drivetrain->GetEncoderRDistance(), DriveProfile::TICKS_PER_REV, DriveProfile::WHEEL_CIRCUMFERENCE, 0.8,0.0,0.0,1.0 / DriveProfile::MAX_VELOCITY, 0 };
delete[] points;
delete[] trajectory;
}
void TestPathfinder::Execute()
{
double l = pathfinder_follow_encoder(configL, &leftFollower, leftTrajectory.get(), length, Robot::drivetrain->GetEncoderLDistance());
double r = pathfinder_follow_encoder(configR, &rightFollower, rightTrajectory.get(), length, Robot::drivetrain->GetEncoderRDistance());
double gyro = RoboMap::navX->GetFusedHeading();
if (gyro > 180) {
gyro -= 360;
}
double desired = r2d(rightFollower.heading);
double dif = desired - gyro;
double turn = 0.8 * (-1.0 / 80.0) * dif ;
//Robot::drivetrain->SetTankDrive(l + turn, r - turn);
RoboMap::sgroupDriveL->Set(l + turn);
RoboMap::sgroupDriveR->Set( r - turn);
std::cout << "Pathfinder: " << "L: " << l << " R: " << r << " Gyro: " << gyro << " Desired: " << desired << " Turn: " << turn << " Dif: " << dif << std::endl;
//Robot::drivetrain->SetTankDrive(0.5,0.5);
}
bool TestPathfinder::IsFinished()
{
return leftFollower.finished && rightFollower.finished;
}
void TestPathfinder::End()
{
printf("Info: End auto driving.\n");
Robot::drivetrain->SetTankDrive(0.0f, 0.0f);
}
void TestPathfinder::Interrupted()
{
printf("Info: End auto driving.\n");
Robot::drivetrain->SetTankDrive(0.0f, 0.0f);
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment