diff --git a/choreo.chor b/choreo.chor index 0e793b9..ba65a13 100644 --- a/choreo.chor +++ b/choreo.chor @@ -2294,7 +2294,7 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [], "eventMarkers": [], - "isTrajectoryStale": false + "isTrajectoryStale": true }, "s3 to n3": { "waypoints": [ @@ -3512,7 +3512,7 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [], "eventMarkers": [], - "isTrajectoryStale": false + "isTrajectoryStale": true }, "n3 to n2": { "waypoints": [ @@ -11819,7 +11819,7 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [], "eventMarkers": [], - "isTrajectoryStale": false + "isTrajectoryStale": true }, "shoot1 to n1": { "waypoints": [ diff --git a/src/main/deploy/choreo/s1 to n1 to f1.1.traj b/src/main/deploy/choreo/s1 to n1 to f1.1.traj new file mode 100644 index 0000000..a046530 --- /dev/null +++ b/src/main/deploy/choreo/s1 to n1 to f1.1.traj @@ -0,0 +1,554 @@ +{ + "samples": [ + { + "x": 0.778, + "y": 6.588, + "heading": 1.04, + "angularVelocity": 2.4164541391435912e-25, + "velocityX": -3.728502797018986e-25, + "velocityY": -5.3441801096923694e-24, + "timestamp": 0 + }, + { + "x": 0.7947611606983267, + "y": 6.589869661617816, + "heading": 1.0126099593801905, + "angularVelocity": -0.6445019986538463, + "velocityX": 0.39439888825929614, + "velocityY": 0.04399411692063376, + "timestamp": 0.04249799174714492 + }, + { + "x": 0.8283954618787034, + "y": 6.594193132903254, + "heading": 0.959188754564873, + "angularVelocity": -1.2570289234645176, + "velocityX": 0.7914327194681035, + "velocityY": 0.1017335433439317, + "timestamp": 0.08499598349428984 + }, + { + "x": 0.8788880987750964, + "y": 6.601560724788077, + "heading": 0.8801356862394247, + "angularVelocity": -1.8601600940533651, + "velocityX": 1.188118186779663, + "velocityY": 0.17336329510951268, + "timestamp": 0.12749397524143477 + }, + { + "x": 0.9461886049889704, + "y": 6.612265643307154, + "heading": 0.7749905730434724, + "angularVelocity": -2.474119573027027, + "velocityX": 1.583616153306687, + "velocityY": 0.2518923384137919, + "timestamp": 0.16999196698857968 + }, + { + "x": 1.0304270885444502, + "y": 6.626229203926427, + "heading": 0.6439353204551028, + "angularVelocity": -3.0837987208460036, + "velocityX": 1.9821756297728585, + "velocityY": 0.3285698934282453, + "timestamp": 0.2124899587357246 + }, + { + "x": 1.1320860354691415, + "y": 6.643158774136244, + "heading": 0.4900230011574287, + "angularVelocity": -3.621637469681476, + "velocityX": 2.3920882551237894, + "velocityY": 0.3983616522527101, + "timestamp": 0.25498795048286954 + }, + { + "x": 1.2520343262104483, + "y": 6.662850781234026, + "heading": 0.3226848255956792, + "angularVelocity": -3.9375548980615434, + "velocityX": 2.822446092393631, + "velocityY": 0.4633632387841555, + "timestamp": 0.29748594223001446 + }, + { + "x": 1.390388797776305, + "y": 6.685146841329519, + "heading": 0.1672743636277293, + "angularVelocity": -3.6568895512195723, + "velocityX": 3.2555531656422927, + "velocityY": 0.5246379694400047, + "timestamp": 0.33998393397715937 + }, + { + "x": 1.5423325796247145, + "y": 6.707428540952282, + "heading": 0.06260023349795245, + "angularVelocity": -2.463037094848347, + "velocityX": 3.575316752670252, + "velocityY": 0.524300059996487, + "timestamp": 0.3824819257243043 + }, + { + "x": 1.7081616227866216, + "y": 6.73044020207964, + "heading": 0.007072259544556311, + "angularVelocity": -1.3066023044989319, + "velocityX": 3.902044222431897, + "velocityY": 0.541476436446092, + "timestamp": 0.4249799174714492 + }, + { + "x": 1.8878304247243443, + "y": 6.755318397124457, + "heading": 0.000003201252181953625, + "angularVelocity": -0.16633864335128887, + "velocityX": 4.227700993654471, + "velocityY": 0.5853969569394615, + "timestamp": 0.4674779092185941 + }, + { + "x": 2.068948056192747, + "y": 6.784047994730074, + "heading": 0.00000284555867364818, + "angularVelocity": -0.000008369654510306158, + "velocityX": 4.26179271119488, + "velocityY": 0.6760224759925306, + "timestamp": 0.5099759009657391 + }, + { + "x": 2.2500656605056086, + "y": 6.8127777635333056, + "heading": 0.000002489867520757382, + "angularVelocity": -0.000008369599086166081, + "velocityX": 4.261792072210799, + "velocityY": 0.6760265043621432, + "timestamp": 0.552473892712884 + }, + { + "x": 2.431183264817259, + "y": 6.841507532344333, + "heading": 0.000002134176523746188, + "angularVelocity": -0.000008369595418237388, + "velocityX": 4.261792072182275, + "velocityY": 0.6760265045455476, + "timestamp": 0.594971884460029 + }, + { + "x": 2.6123008691289322, + "y": 6.870237301155361, + "heading": 0.000001778485682500457, + "angularVelocity": -0.000008369591752994459, + "velocityX": 4.261792072182845, + "velocityY": 0.6760265045455279, + "timestamp": 0.637469876207174 + }, + { + "x": 2.7934184734406315, + "y": 6.898967069966389, + "heading": 0.0000014227949971132448, + "angularVelocity": -0.000008369588085561907, + "velocityX": 4.261792072183418, + "velocityY": 0.6760265045454995, + "timestamp": 0.679967867954319 + }, + { + "x": 2.9745360777523553, + "y": 6.927696838777412, + "heading": 0.0000010671044675807755, + "angularVelocity": -0.000008369584418218188, + "velocityX": 4.26179207218399, + "velocityY": 0.6760265045454718, + "timestamp": 0.7224658597014639 + }, + { + "x": 3.1556536820641043, + "y": 6.956426607588424, + "heading": 7.114140938199114e-7, + "angularVelocity": -0.000008369580752830746, + "velocityX": 4.261792072184615, + "velocityY": 0.6760265045451095, + "timestamp": 0.7649638514486089 + }, + { + "x": 3.3367712864253214, + "y": 6.98515637608772, + "heading": 3.557238745499121e-7, + "angularVelocity": -0.0000083695771175798, + "velocityX": 4.261792073348637, + "velocityY": 0.6760264972104505, + "timestamp": 0.8074618431957539 + }, + { + "x": 3.517889976501465, + "y": 7.013879299163818, + "heading": 7.289295258743566e-23, + "angularVelocity": -0.000008370369043939825, + "velocityX": 4.261817620789355, + "velocityY": 0.6758654208177892, + "timestamp": 0.8499598349428988 + }, + { + "x": 3.6335989147911016, + "y": 7.02524370198304, + "heading": -9.507035478047604e-17, + "angularVelocity": -3.5284408548154295e-15, + "velocityX": 4.294414666406014, + "velocityY": 0.4217777715626412, + "timestamp": 0.8769038883885978 + }, + { + "x": 3.749308510749384, + "y": 7.036601406625281, + "heading": -7.562942251478163e-17, + "angularVelocity": 7.215269597853555e-16, + "velocityX": 4.2944390750809225, + "velocityY": 0.42152917582224764, + "timestamp": 0.9038479418342967 + }, + { + "x": 3.865018106821736, + "y": 7.0479591101054355, + "heading": -7.373318282211032e-17, + "angularVelocity": 7.037428120346869e-17, + "velocityX": 4.2944390793146745, + "velocityY": 0.42152913268978187, + "timestamp": 0.9307919952799957 + }, + { + "x": 3.980727702894107, + "y": 7.059316813585294, + "heading": -9.049943694768188e-17, + "angularVelocity": -6.222644064316587e-16, + "velocityX": 4.294439079315409, + "velocityY": 0.4215291326822982, + "timestamp": 0.9577360487256946 + }, + { + "x": 4.096437298966481, + "y": 7.07067451706525, + "heading": -1.105309312631036e-16, + "angularVelocity": -7.43450336990658e-16, + "velocityX": 4.294439079315409, + "velocityY": 0.42152913268229675, + "timestamp": 0.9846801021713936 + }, + { + "x": 4.212146895038837, + "y": 7.082032220545177, + "heading": -9.454628169624526e-17, + "angularVelocity": 5.932506888918781e-16, + "velocityX": 4.294439079315409, + "velocityY": 0.42152913268229675, + "timestamp": 1.0116241556170924 + }, + { + "x": 4.327856491111232, + "y": 7.0933899240249945, + "heading": -5.941610596663945e-17, + "angularVelocity": 1.3038166102866517e-15, + "velocityX": 4.294439079315409, + "velocityY": 0.42152913268229675, + "timestamp": 1.0385682090627912 + }, + { + "x": 4.443566087183581, + "y": 7.104747627505038, + "heading": -9.618061504048743e-17, + "angularVelocity": -1.3644784533478365e-15, + "velocityX": 4.294439079315409, + "velocityY": 0.42152913268229675, + "timestamp": 1.06551226250849 + }, + { + "x": 4.559275683255968, + "y": 7.116105330984801, + "heading": -8.385704748278896e-17, + "angularVelocity": 4.573735085500748e-16, + "velocityX": 4.294439079315409, + "velocityY": 0.4215291326822968, + "timestamp": 1.092456315954189 + }, + { + "x": 4.674985279328332, + "y": 7.127463034464815, + "heading": -6.920759503666312e-17, + "angularVelocity": 5.436962622191256e-16, + "velocityX": 4.294439079315409, + "velocityY": 0.4215291326822968, + "timestamp": 1.1194003693998877 + }, + { + "x": 4.790694875400705, + "y": 7.138820737944737, + "heading": -8.936627630308976e-17, + "angularVelocity": -7.48170745939213e-16, + "velocityX": 4.294439079315409, + "velocityY": 0.4215291326822968, + "timestamp": 1.1463444228455866 + }, + { + "x": 4.906404471473082, + "y": 7.150178441424623, + "heading": -9.003105304160849e-17, + "angularVelocity": -2.467513058215487e-17, + "velocityX": 4.294439079315409, + "velocityY": 0.4215291326822968, + "timestamp": 1.1732884762912854 + }, + { + "x": 5.022114067545444, + "y": 7.1615361449045185, + "heading": -1.155538288286611e-16, + "angularVelocity": -9.472534315289015e-16, + "velocityX": 4.294439079315409, + "velocityY": 0.4215291326822968, + "timestamp": 1.2002325297369842 + }, + { + "x": 5.137823663617817, + "y": 7.172893848384477, + "heading": -1.1180541028972435e-16, + "angularVelocity": 1.391159369467602e-16, + "velocityX": 4.294439079315409, + "velocityY": 0.4215291326822968, + "timestamp": 1.227176583182683 + }, + { + "x": 5.253533259690204, + "y": 7.1842515518643335, + "heading": -8.573613819184158e-17, + "angularVelocity": 9.675307708154093e-16, + "velocityX": 4.294439079315409, + "velocityY": 0.4215291326822968, + "timestamp": 1.254120636628382 + }, + { + "x": 5.369242855762564, + "y": 7.19560925534427, + "heading": -8.492399373464744e-17, + "angularVelocity": 3.0139234995482524e-17, + "velocityX": 4.294439079315409, + "velocityY": 0.4215291326822968, + "timestamp": 1.2810646900740807 + }, + { + "x": 5.484952451834932, + "y": 7.206966958824167, + "heading": -4.985418876125364e-17, + "angularVelocity": 1.3015760136246041e-15, + "velocityX": 4.294439079315409, + "velocityY": 0.42152913268229664, + "timestamp": 1.3080087435197796 + }, + { + "x": 5.600662047907313, + "y": 7.218324662304093, + "heading": -3.4124684986808435e-17, + "angularVelocity": 5.837812230996926e-16, + "velocityX": 4.294439079315497, + "velocityY": 0.42152913268141085, + "timestamp": 1.3349527969654784 + }, + { + "x": 5.716371643993196, + "y": 7.229682365646357, + "heading": 5.459432577417325e-18, + "angularVelocity": 1.469119943120619e-15, + "velocityX": 4.294439079816645, + "velocityY": 0.42152912757582495, + "timestamp": 1.3618968504111773 + }, + { + "x": 5.832081317901611, + "y": 7.241039276123047, + "heading": -7.306327534388352e-23, + "angularVelocity": -2.0262641440836581e-16, + "velocityX": 4.294441968117965, + "velocityY": 0.421499701206165, + "timestamp": 1.388840903856876 + }, + { + "x": 5.949210569571271, + "y": 7.248848645193732, + "heading": -1.6370541704070278e-17, + "angularVelocity": -6.017570023841364e-16, + "velocityX": 4.305518462573149, + "velocityY": 0.2870622174699031, + "timestamp": 1.416045351518136 + }, + { + "x": 6.06633987423792, + "y": 7.256657219345569, + "heading": -2.973277501870474e-17, + "angularVelocity": -4.911781969028432e-16, + "velocityX": 4.305520410673374, + "velocityY": 0.2870329972902524, + "timestamp": 1.4432497991793958 + }, + { + "x": 6.183469178913503, + "y": 7.2644657933634065, + "heading": -4.477219350756502e-17, + "angularVelocity": -5.528293355364244e-16, + "velocityX": 4.30552041100175, + "velocityY": 0.28703299236458263, + "timestamp": 1.4704542468406556 + }, + { + "x": 6.300598483589087, + "y": 7.272274367381222, + "heading": -1.3222644280129107e-17, + "angularVelocity": 1.1597203290039311e-15, + "velocityX": 4.3055204110018055, + "velocityY": 0.28703299236375224, + "timestamp": 1.4976586945019155 + }, + { + "x": 6.4177277882646715, + "y": 7.280082941399036, + "heading": 5.959330887391521e-18, + "angularVelocity": 7.051044436196019e-16, + "velocityX": 4.3055204110018055, + "velocityY": 0.2870329923637522, + "timestamp": 1.5248631421631753 + }, + { + "x": 6.534857092940256, + "y": 7.2878915154168515, + "heading": -2.5355594009497396e-17, + "angularVelocity": -1.1510957150154529e-15, + "velocityX": 4.3055204110018055, + "velocityY": 0.2870329923637522, + "timestamp": 1.5520675898244352 + }, + { + "x": 6.651986397615841, + "y": 7.295700089434666, + "heading": -3.2387330748087324e-18, + "angularVelocity": 8.129870150179335e-16, + "velocityX": 4.3055204110018055, + "velocityY": 0.2870329923637521, + "timestamp": 1.579272037485695 + }, + { + "x": 6.769115702291424, + "y": 7.303508663452481, + "heading": 8.113604173662996e-18, + "angularVelocity": 4.172971697303915e-16, + "velocityX": 4.3055204110018055, + "velocityY": 0.2870329923637521, + "timestamp": 1.6064764851469548 + }, + { + "x": 6.886245006967009, + "y": 7.311317237470296, + "heading": 3.429930712535077e-17, + "angularVelocity": 9.625523385879208e-16, + "velocityX": 4.3055204110018055, + "velocityY": 0.2870329923637522, + "timestamp": 1.6336809328082147 + }, + { + "x": 7.0033743116425935, + "y": 7.319125811488111, + "heading": 4.2753403512269446e-17, + "angularVelocity": 3.107616172486459e-16, + "velocityX": 4.3055204110018055, + "velocityY": 0.2870329923637522, + "timestamp": 1.6608853804694745 + }, + { + "x": 7.120503616318178, + "y": 7.3269343855059255, + "heading": 6.192803998817309e-17, + "angularVelocity": 7.048346828907741e-16, + "velocityX": 4.3055204110018055, + "velocityY": 0.2870329923637522, + "timestamp": 1.6880898281307344 + }, + { + "x": 7.237632920993763, + "y": 7.334742959523741, + "heading": 9.8074903668434e-17, + "angularVelocity": 1.3287116097697159e-15, + "velocityX": 4.3055204110018055, + "velocityY": 0.2870329923637522, + "timestamp": 1.7152942757919942 + }, + { + "x": 7.354762225669346, + "y": 7.342551533541555, + "heading": 7.082460621437034e-17, + "angularVelocity": -1.0016853136946598e-15, + "velocityX": 4.3055204110018055, + "velocityY": 0.2870329923637522, + "timestamp": 1.742498723453254 + }, + { + "x": 7.471891530344931, + "y": 7.3503601075593705, + "heading": 1.038286534728566e-16, + "angularVelocity": 1.2131857788121341e-15, + "velocityX": 4.3055204110018055, + "velocityY": 0.2870329923637522, + "timestamp": 1.769703171114514 + }, + { + "x": 7.5890208350205155, + "y": 7.358168681577185, + "heading": 8.061737472885344e-17, + "angularVelocity": -8.532162559759179e-16, + "velocityX": 4.3055204110018055, + "velocityY": 0.2870329923637522, + "timestamp": 1.7969076187757738 + }, + { + "x": 7.7061501396961, + "y": 7.365977255595, + "heading": 4.8704248802202976e-17, + "angularVelocity": -1.173084804347112e-15, + "velocityX": 4.3055204110018055, + "velocityY": 0.2870329923637522, + "timestamp": 1.8241120664370336 + }, + { + "x": 7.823279444371685, + "y": 7.373785829612815, + "heading": 7.660156345624939e-17, + "angularVelocity": 1.0254689514738244e-15, + "velocityX": 4.3055204110018055, + "velocityY": 0.2870329923637522, + "timestamp": 1.8513165140982935 + }, + { + "x": 7.940408749047268, + "y": 7.38159440363063, + "heading": 5.704626230613074e-17, + "angularVelocity": -7.188272898996364e-16, + "velocityX": 4.3055204110018055, + "velocityY": 0.2870329923637522, + "timestamp": 1.8785209617595533 + }, + { + "x": 8.057538053722853, + "y": 7.3894029776484444, + "heading": 4.040721833497174e-17, + "angularVelocity": -6.116294810979149e-16, + "velocityX": 4.3055204110018055, + "velocityY": 0.2870329923637522, + "timestamp": 1.9057254094208131 + }, + { + "x": 8.174667358398438, + "y": 7.39721155166626, + "heading": 1.7645472792716848e-24, + "angularVelocity": -1.4853164934283371e-15, + "velocityX": 4.3055204110018055, + "velocityY": 0.2870329923637522, + "timestamp": 1.932929857082073 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/java/team3647/frc2024/constants/AutoConstants.java b/src/main/java/team3647/frc2024/constants/AutoConstants.java index bcf08a7..88c0a70 100644 --- a/src/main/java/team3647/frc2024/constants/AutoConstants.java +++ b/src/main/java/team3647/frc2024/constants/AutoConstants.java @@ -53,7 +53,7 @@ public class AutoConstants { public static final PIDController kRotController = new PIDController(xRotControllerP, xRotControllerI, xRotControllerD); - public static final double kDrivetrainXShootingThreshold = 4.85; + public static final double kDrivetrainXShootingThreshold = 5; static { kRotController.enableContinuousInput(-Math.PI, Math.PI); diff --git a/src/main/java/team3647/frc2024/constants/PivotConstants.java b/src/main/java/team3647/frc2024/constants/PivotConstants.java index 2f236a6..f67896e 100644 --- a/src/main/java/team3647/frc2024/constants/PivotConstants.java +++ b/src/main/java/team3647/frc2024/constants/PivotConstants.java @@ -37,7 +37,7 @@ public class PivotConstants { public static final double kMaxDegree = 76.0; public static final double kMaxDegreeUnderStage = 30; - private static final double masterKP = 3; + private static final double masterKP = 23; private static final double masterKI = 0; private static final double masterKD = 0; @@ -90,20 +90,26 @@ public class PivotConstants { // kMasterSpeakerMap.put(20.0, 26.5); kMasterSpeakerMap.put(0.0, 60.0); kMasterSpeakerMap.put(1.5, 55.0); + kMasterSpeakerMap.put(1.75, 52.0); kMasterSpeakerMap.put(2.0, 48.0); - kMasterSpeakerMap.put(2.5, 42.8); - kMasterSpeakerMap.put(3.0, 37.8); - kMasterSpeakerMap.put(3.5, 34.3); - kMasterSpeakerMap.put(4.0, 31.8); - kMasterSpeakerMap.put(4.5, 30.5); - kMasterSpeakerMap.put(5.0, 27.8); - kMasterSpeakerMap.put(5.5, 26.0); - kMasterSpeakerMap.put(6.0, 24.2); - kMasterSpeakerMap.put(6.5, 24.0); - kMasterSpeakerMap.put(7.0, 23.1); - kMasterSpeakerMap.put(7.5, 22.1); - kMasterSpeakerMap.put(8.0, 21.5); - kMasterSpeakerMap.put(20.0, 21.5); + kMasterSpeakerMap.put(2.25, 44.0); + kMasterSpeakerMap.put(2.5, 41.0); + kMasterSpeakerMap.put(2.75, 39.0); + kMasterSpeakerMap.put(3.0, 36.0); + kMasterSpeakerMap.put(3.25, 34.8); + kMasterSpeakerMap.put(3.5, 33.0); + kMasterSpeakerMap.put(3.75, 31.5); + kMasterSpeakerMap.put(4.0, 30.0); + kMasterSpeakerMap.put(4.25, 28.3); + kMasterSpeakerMap.put(4.5, 27.5); + kMasterSpeakerMap.put(5.0, 26.0); + kMasterSpeakerMap.put(5.5, 24.5); + kMasterSpeakerMap.put(6.0, 22.8); + kMasterSpeakerMap.put(6.5, 22.1); + kMasterSpeakerMap.put(7.0, 21.6); + kMasterSpeakerMap.put(7.5, 21.3); + kMasterSpeakerMap.put(8.0, 21.3); + kMasterSpeakerMap.put(20.0, 21.3); kMasterAmpMap.put(0.0, 60.0); kMasterAmpMap.put(100.0, 60.0); Slot0Configs kMasterSlot0 = new Slot0Configs(); @@ -118,7 +124,7 @@ public class PivotConstants { kMasterSlot0.kP = masterKP; kMasterSlot0.kI = masterKI; kMasterSlot0.kD = masterKD; - kMasterSlot0.kS = 0.21; + kMasterSlot0.kS = 0.5; kMasterSlot0.kA = 0; kMasterSlot0.kV = 0; kMasterCurrent.StatorCurrentLimitEnable = true; diff --git a/src/main/java/team3647/frc2024/constants/ShooterConstants.java b/src/main/java/team3647/frc2024/constants/ShooterConstants.java index 1a1bdd0..bd7a625 100644 --- a/src/main/java/team3647/frc2024/constants/ShooterConstants.java +++ b/src/main/java/team3647/frc2024/constants/ShooterConstants.java @@ -27,13 +27,13 @@ public class ShooterConstants { kWheelRotationMeters / GlobalConstants.kFalconTicksPerRotation * kGearboxReduction; // tune ff - public static final double kS = 9.8785; // 17.729; // 8.7167; - public static final double kV = 0.40849; // 0.28947; // 0.24226; - public static final double kA = 1.3338; // 0.88966; // 0.60231; + public static final double kS = 13.132; // 17.729; // 8.7167; + public static final double kV = 0.23336; // 0.28947; // 0.24226; + public static final double kA = 1.2516; // 0.88966; // 0.60231; public static final SimpleMotorFeedforward ff = new SimpleMotorFeedforward(kS, kV, kA); - public static final double masterKP = 1.0657; + public static final double masterKP = 17.378; public static final double masterKI = 0; public static final double masterKD = 0;