Skip to content

Commit

Permalink
Arm
Browse files Browse the repository at this point in the history 
commit ca1fd41
Author: Brandon <[email protected]>
Date:   Sat Mar 9 22:24:08 2024 -0800

    unflipped the numbers for #27

commit f94e42d
Author: Brandon <[email protected]>
Date:   Sat Mar 9 22:19:36 2024 -0800

    correct numbers for #27?

commit 813432d
Author: ProfessorAtomicManiac <[email protected]>
Date:   Sat Mar 9 17:38:36 2024 -0800

    fixed null exception

commit 892ec7c
Author: ProfessorAtomicManiac <[email protected]>
Date:   Sat Mar 9 15:17:09 2024 -0800

    fixed compilation error

commit f1a867a
Author: FriedLongJohns <[email protected]>
Date:   Fri Mar 8 22:36:44 2024 -0800

    Update comments (also clarify stuff for #24 )

commit 2fd9430
Author: FriedLongJohns <[email protected]>
Date:   Fri Mar 8 20:12:12 2024 -0800

    Reordering of logic for #24

    Only do work if we need to move.
    Also remove redundant deadzone checking

commit 1348108
Merge: 1d1355a 9efab59
Author: FriedLongJohns <[email protected]>
Date:   Fri Mar 8 19:42:54 2024 -0800

    Merge branch 'sofie-arm' of https://github.com/DeepBlueRobotics/RobotCode2024 into sofie-arm

commit 1d1355a
Author: FriedLongJohns <[email protected]>
Date:   Fri Mar 8 19:42:44 2024 -0800

    fix #27 by dynamic ff calculation

commit 9efab59
Author: Sofie Budman <[email protected]>
Date:   Fri Mar 8 19:23:12 2024 -0800

    ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD

commit a534422
Author: Juliaaaahhhh <[email protected]>
Date:   Fri Mar 8 17:08:15 2024 -0800

    added comment for fix for issue #22

commit 0266251
Author: Brandon <[email protected]>
Date:   Thu Mar 7 16:47:39 2024 -0800

    work for #27

commit dfe00e4
Author: Brandon <[email protected]>
Date:   Thu Mar 7 16:33:07 2024 -0800

    Max FF vel and accel #27

commit 3622b0d
Author: Brandon <[email protected]>
Date:   Wed Mar 6 21:22:48 2024 -0800

    fixed comment

commit dbf70ba
Author: Brandon <[email protected]>
Date:   Wed Mar 6 14:57:35 2024 -0800

    comments

commit 43d7231
Author: Brandon <[email protected]>
Date:   Wed Mar 6 14:52:40 2024 -0800

    Moved where lastTime was set #24

commit 863565c
Author: Brandon <[email protected]>
Date:   Wed Mar 6 19:52:29 2024 +0000

    if speeds is 0 in armTeleop, returns, fix to #24?

commit d14d5ab
Author: Brandon <[email protected]>
Date:   Tue Mar 5 22:09:04 2024 -0800

    inverted motors

commit 6b6abd8
Author: FriedLongJohns <[email protected]>
Date:   Tue Mar 5 17:23:05 2024 -0800

    drive arm to arm goal

    fixes #20

commit 0bfc3eb
Author: FriedLongJohns <[email protected]>
Date:   Tue Mar 5 17:15:56 2024 -0800

    Fix for #22

commit ae3681f
Merge: 5abd6d5 d28e634
Author: Juliaaaahhhh <[email protected]>
Date:   Tue Mar 5 17:12:54 2024 -0800

    Merge branch 'sofie-arm' of https://github.com/DeepBlueRobotics/RobotCode2024 into sofie-arm

commit 5abd6d5
Merge: f4e16c1 91e7922
Author: Juliaaaahhhh <[email protected]>
Date:   Tue Mar 5 17:12:44 2024 -0800

    Merge branch 'sofie-arm' of https://github.com/DeepBlueRobotics/RobotCode2024 into sofie-arm

commit d28e634
Merge: 81c71d3 91e7922
Author: DriverStationComputer <[email protected]>
Date:   Tue Mar 5 17:12:39 2024 -0800

    Merge branch 'sofie-arm' of https://github.com/DeepBlueRobotics/RobotCode2024 into sofie-arm

commit 81c71d3
Author: DriverStationComputer <[email protected]>
Date:   Tue Mar 5 17:12:36 2024 -0800

    fixed sysID measures

commit 91e7922
Author: FriedLongJohns <[email protected]>
Date:   Tue Mar 5 17:09:47 2024 -0800

    add deadzone for arm defaultcommand

    to fix issue #24

commit f4e16c1
Author: Juliaaaahhhh <[email protected]>
Date:   Tue Mar 5 17:09:45 2024 -0800

    fixed merge conflicts?

commit 571b732
Author: FriedLongJohns <[email protected]>
Date:   Tue Mar 5 17:02:57 2024 -0800

    Revert "fix for #24"

    This reverts commit ec3ea78.

commit 519029c
Author: Sofie Budman <[email protected]>
Date:   Tue Mar 5 16:56:53 2024 -0800

    fixed feedforward stuff

commit 81ae651
Merge: a594629 ec3ea78
Author: Juliaaaahhhh <[email protected]>
Date:   Tue Mar 5 16:55:37 2024 -0800

    Merge branch 'sofie-arm' of https://github.com/DeepBlueRobotics/RobotCode2024 into sofie-arm

commit a594629
Author: Juliaaaahhhh <[email protected]>
Date:   Tue Mar 5 16:45:19 2024 -0800

    changed ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD placeholder based on robotcode2023

commit ec3ea78
Author: Brandon <[email protected]>
Date:   Tue Mar 5 16:39:56 2024 -0800

    fix for #24

commit 53166b4
Merge: d16d9af 01493e1
Author: DriverStationComputer <[email protected]>
Date:   Tue Mar 5 16:39:15 2024 -0800

    Merge branch 'sofie-arm' of https://github.com/DeepBlueRobotics/RobotCode2024 into sofie-arm

commit 01493e1
Merge: 43ce540 8805765
Author: FriedLongJohns <[email protected]>
Date:   Tue Mar 5 16:39:17 2024 -0800

    Merge branch 'sofie-arm' of https://github.com/DeepBlueRobotics/RobotCode2024 into sofie-arm

commit 43ce540
Author: FriedLongJohns <[email protected]>
Date:   Tue Mar 5 16:39:12 2024 -0800

    tell VScode not to err on encoder zero offset

commit 8805765
Author: Sofie Budman <[email protected]>
Date:   Tue Mar 5 16:39:07 2024 -0800

    fixed setzerofoses

commit d16d9af
Merge: e33037d 7a22712
Author: DriverStationComputer <[email protected]>
Date:   Tue Mar 5 16:39:04 2024 -0800

    Merge branch 'sofie-arm' of https://github.com/DeepBlueRobotics/RobotCode2024 into sofie-arm

commit e33037d
Author: DriverStationComputer <[email protected]>
Date:   Tue Mar 5 16:39:00 2024 -0800

    fixed POSTOLRAD

commit 7a22712
Author: Sofie Budman <[email protected]>
Date:   Tue Mar 5 16:36:27 2024 -0800

    fixed izone

commit d2a8859
Author: Sofie Budman <[email protected]>
Date:   Tue Mar 5 16:35:25 2024 -0800

    math for drivearm

commit 35374d4
Author: FriedLongJohns <[email protected]>
Date:   Tue Mar 5 16:28:37 2024 -0800

    use current zero offset

commit eb1cfc9
Author: Juliaaaahhhh <[email protected]>
Date:   Tue Mar 5 16:26:50 2024 -0800

    issue #25 - setting pos. and vel. of goal state

commit b373a0b
Author: DriverStationComputer <[email protected]>
Date:   Tue Mar 5 16:22:05 2024 -0800

    sysID Fixed

commit 721d37f
Author: Brandon <[email protected]>
Date:   Tue Mar 5 16:18:44 2024 -0800

    fixed semicolon stuff

commit 2f33075
Author: FriedLongJohns <[email protected]>
Date:   Tue Mar 5 16:17:33 2024 -0800

    rename and use sensible izone number

commit 54b28c8
Author: Sofie Budman <[email protected]>
Date:   Tue Mar 5 16:16:39 2024 -0800

    updated max vel

commit 0b5d292
Merge: 43acf02 ddf7573
Author: Brandon <[email protected]>
Date:   Tue Mar 5 16:15:50 2024 -0800

    Merge branch 'sofie-arm' of https://github.com/DeepBlueRobotics/RobotCode2024 into sofie-arm

commit 43acf02
Author: Brandon <[email protected]>
Date:   Tue Mar 5 16:15:40 2024 -0800

    added the inverted for #17

commit ddf7573
Author: FriedLongJohns <[email protected]>
Date:   Tue Mar 5 16:13:53 2024 -0800

    correct Izone

commit 5a37fa4
Author: Brandon <[email protected]>
Date:   Tue Mar 5 15:43:31 2024 -0800

    removed armFollowerEncoder fix to #23

commit 556ee37
Author: Brandon <[email protected]>
Date:   Tue Mar 5 15:30:28 2024 -0800

    sysid nondefalt config fix to #13?

commit 0dd661b
Author: stwiggy <[email protected]>
Date:   Tue Mar 5 14:47:34 2024 -0800

    #18

commit d4af6c7
Author: stwiggy <[email protected]>
Date:   Tue Mar 5 14:46:35 2024 -0800

    issue #20

commit 820310f
Author: stwiggy <[email protected]>
Date:   Tue Mar 5 13:48:30 2024 -0800

    issue #14 changed

commit 864bd30
Author: aaronC34 <[email protected]>
Date:   Mon Mar 4 23:45:20 2024 -0800

    finalized pos tol rad and vel tol rad

commit 071ebe6
Author: aaronC34 <[email protected]>
Date:   Mon Mar 4 23:38:51 2024 -0800

    fixed formatting and added number to pos tolerance(NOT FINALIZED NUMBER)

commit 1919ce2
Author: Brandon <[email protected]>
Date:   Mon Mar 4 21:45:03 2024 -0800

    removed margin of error

commit 0157dcd
Author: Brandon <[email protected]>
Date:   Mon Mar 4 20:18:33 2024 -0800

    turns off pid if arm overshoots

commit 9d00521
Author: Brandon <[email protected]>
Date:   Mon Mar 4 18:52:52 2024 -0800

    edge cases to driveArm()

commit f678c1d
Author: Brandon <[email protected]>
Date:   Mon Mar 4 18:50:28 2024 -0800

    Added comments

commit 96f6cb5
Author: Brandon <[email protected]>
Date:   Mon Mar 4 18:48:26 2024 -0800

    disables pid once it is wihtin tolerance

commit ca60b94
Author: Brandon <[email protected]>
Date:   Mon Mar 4 18:22:19 2024 -0800

    ran alt shift f for everything

commit dd1ac71
Author: Brandon <[email protected]>
Date:   Mon Mar 4 23:17:53 2024 +0000

    rad for lower upper angle

commit 33520ca
Author: Brandon <[email protected]>
Date:   Mon Mar 4 23:16:34 2024 +0000

    More units

commit 6ce18d3
Author: Brandon <[email protected]>
Date:   Mon Mar 4 23:15:09 2024 +0000

    Removed seperate setPosition

commit d557415
Author: Brandon <[email protected]>
Date:   Mon Mar 4 23:12:55 2024 +0000

    More Units

commit 5ea8b8e
Author: Brandon <[email protected]>
Date:   Mon Mar 4 23:08:58 2024 +0000

    RAD

commit 9b1d4fb
Author: Brandon <[email protected]>
Date:   Mon Mar 4 23:02:09 2024 +0000

    changed goal state to private

commit 5b8830b
Merge: 15c4f0b 405f752
Author: Sofie Budman <[email protected]>
Date:   Sun Mar 3 15:16:57 2024 -0800

    Merge branch 'sofie-arm' of https://github.com/deepbluerobotics/RobotCode2024 into sofie-arm

commit 15c4f0b
Author: Sofie Budman <[email protected]>
Date:   Sun Mar 3 15:16:10 2024 -0800

    ports!

commit 405f752
Author: Team 199 Driver Station Computer <[email protected]>
Date:   Sun Mar 3 13:56:01 2024 -0800

    set left and right motor ports

commit 080ede3
Author: Sofie Budman <[email protected]>
Date:   Sun Mar 3 13:04:28 2024 -0800

    made it look nice :)

commit 69a70a9
Author: Sofie Budman <[email protected]>
Date:   Sun Mar 3 12:30:15 2024 -0800

    got rid of follower pid

commit b1b5d42
Author: stwiggy <[email protected]>
Date:   Sun Mar 3 11:25:08 2024 -0800

    armteleop clamping

commit ff6a115
Author: stwiggy <[email protected]>
Date:   Sun Mar 3 11:02:57 2024 -0800

    deleted follower motor's bounds

commit a0e46b3
Merge: 32bf0ca 5a215e8
Author: stwiggy <[email protected]>
Date:   Sun Mar 3 10:50:30 2024 -0800

    Merge branch 'sofie-arm' of https://github.com/DeepBlueRobotics/RobotCode2024 into sofie-arm

commit 32bf0ca
Author: stwiggy <[email protected]>
Date:   Sun Mar 3 10:48:28 2024 -0800

    buttons, constants, and arm bounds

commit 5a215e8
Author: Brandon <[email protected]>
Date:   Sun Mar 3 10:41:24 2024 -0800

    maybe fixed the pid wrapping

commit 6b29cd6
Author: Brandon <[email protected]>
Date:   Sun Mar 3 10:37:08 2024 -0800

    undo bad stuff I did

commit 8775659
Author: Brandon <[email protected]>
Date:   Sun Mar 3 10:33:32 2024 -0800

    Fixed angle mistake made by me

commit d14677e
Author: Brandon <[email protected]>
Date:   Sun Mar 3 10:30:57 2024 -0800

    absolute encoder wrapping, im not sure it this works tho

commit 2ac4b07
Author: Brandon <[email protected]>
Date:   Sun Mar 3 10:14:17 2024 -0800

    fixed incorrect usage of getCurrentArmState() in driveArm()

commit 2fb1a2f
Author: Sofie Budman <[email protected]>
Date:   Sun Mar 3 09:58:34 2024 -0800

    organized constants

commit 173787b
Author: Sofie Budman <[email protected]>
Date:   Sun Mar 3 00:41:10 2024 -0800

    no more paramaters for drive arm!

commit f3b27d0
Author: stwiggy <[email protected]>
Date:   Sun Mar 3 00:27:45 2024 -0800

    resetGoal finished?

commit 555e0cc
Author: stwiggy <[email protected]>
Date:   Sun Mar 3 00:23:16 2024 -0800

    specified arm pid

commit 48c4e7a
Merge: 9a262b9 999bb89
Author: Brandon <[email protected]>
Date:   Sat Mar 2 23:39:32 2024 -0800

    Merge branch 'sofie-arm' of https://github.com/DeepBlueRobotics/RobotCode2024 into sofie-arm

commit 9a262b9
Author: Brandon <[email protected]>
Date:   Sat Mar 2 23:39:28 2024 -0800

    Fixed resetGoal

commit 999bb89
Author: Sofie Budman <[email protected]>
Date:   Sat Mar 2 23:38:30 2024 -0800

    setarrmtarget in teleop

commit baf30a2
Author: Brandon <[email protected]>
Date:   Sat Mar 2 23:36:16 2024 -0800

    setArmTarget changes

commit 3e3f011
Merge: 706a79d 71886b3
Author: stwiggy <[email protected]>
Date:   Sat Mar 2 23:34:50 2024 -0800

    Merge branch 'sofie-arm' of https://github.com/DeepBlueRobotics/RobotCode2024 into sofie-arm

commit 706a79d
Merge: 464435e 531f1ab
Author: stwiggy <[email protected]>
Date:   Sat Mar 2 23:34:46 2024 -0800

    changed variables for upper/lower angle limits

commit 71886b3
Merge: 058095a 531f1ab
Author: Brandon <[email protected]>
Date:   Sat Mar 2 23:34:15 2024 -0800

    Merge branch 'sofie-arm' of https://github.com/DeepBlueRobotics/RobotCode2024 into sofie-arm

commit 058095a
Author: Brandon <[email protected]>
Date:   Sat Mar 2 23:34:12 2024 -0800

    driveArm

commit 464435e
Author: stwiggy <[email protected]>
Date:   Sat Mar 2 23:32:42 2024 -0800

    deleted comment

commit 531f1ab
Merge: fb43ede 24c1634
Author: Sofie Budman <[email protected]>
Date:   Sat Mar 2 23:32:15 2024 -0800

    Merge branch 'sofie-arm' of https://github.com/deepbluerobotics/RobotCode2024 into sofie-arm

commit fb43ede
Author: Sofie Budman <[email protected]>
Date:   Sat Mar 2 23:32:11 2024 -0800

    got rid of unnesecary constants

commit 24c1634
Author: Brandon <[email protected]>
Date:   Sat Mar 2 23:31:06 2024 -0800

    rps

commit fc8fdb8
Author: Brandon <[email protected]>
Date:   Sat Mar 2 23:30:31 2024 -0800

    degrees instead of rotations

commit e0c668d
Author: Brandon <[email protected]>
Date:   Sat Mar 2 23:28:21 2024 -0800

    boundaries

commit 9535134
Merge: 8d7ea0c dc02eae
Author: stwiggy <[email protected]>
Date:   Sat Mar 2 23:27:29 2024 -0800

    Merge branch 'sofie-arm' of https://github.com/DeepBlueRobotics/RobotCode2024 into sofie-arm

commit 8d7ea0c
Author: stwiggy <[email protected]>
Date:   Sat Mar 2 23:27:26 2024 -0800

    placeholder confirmation

commit dc02eae
Author: Sofie Budman <[email protected]>
Date:   Sat Mar 2 23:26:53 2024 -0800

    got rid of comment

commit 40ddd51
Merge: 42216a1 ee7d0b7
Author: Sofie Budman <[email protected]>
Date:   Sat Mar 2 23:25:46 2024 -0800

    Merge branch 'sofie-arm' of https://github.com/deepbluerobotics/RobotCode2024 into sofie-arm

commit 42216a1
Author: Sofie Budman <[email protected]>
Date:   Sat Mar 2 23:25:43 2024 -0800

    got rid of extra setvoltage

commit ee7d0b7
Author: Brandon <[email protected]>
Date:   Sat Mar 2 23:25:35 2024 -0800

    renamed constants

commit 94e9c83
Merge: 49c5df7 cdaa7ac
Author: Brandon <[email protected]>
Date:   Sat Mar 2 23:24:21 2024 -0800

    Merge branch 'sofie-arm' of https://github.com/DeepBlueRobotics/RobotCode2024 into sofie-arm

commit 49c5df7
Author: Brandon <[email protected]>
Date:   Sat Mar 2 23:24:10 2024 -0800

    Renamed motor ports

commit cdaa7ac
Author: Sofie Budman <[email protected]>
Date:   Sat Mar 2 23:24:03 2024 -0800

    no more eerors

commit c10e296
Author: Sofie Budman <[email protected]>
Date:   Sat Mar 2 23:23:26 2024 -0800

    added motor inverted

commit aa2233f
Author: Brandon <[email protected]>
Date:   Sat Mar 2 23:22:18 2024 -0800

    added space for charleses eyes

commit 4f8ad80
Author: Brandon <[email protected]>
Date:   Sat Mar 2 23:18:58 2024 -0800

    fixed ArmTeleop

commit 986bbb1
Merge: 244d642 ec758ef
Author: stwiggy <[email protected]>
Date:   Sat Mar 2 23:18:02 2024 -0800

    Merge branch 'sofie-arm' of https://github.com/DeepBlueRobotics/RobotCode2024 into sofie-arm

commit 244d642
Author: stwiggy <[email protected]>
Date:   Sat Mar 2 23:18:00 2024 -0800

    ff volts

commit ec758ef
Author: Sofie Budman <[email protected]>
Date:   Sat Mar 2 23:15:45 2024 -0800

    changed to setarm target in robocontainer

commit e11233c
Author: Sofie Budman <[email protected]>
Date:   Sat Mar 2 23:10:09 2024 -0800

    public --> private

commit ff59c77
Author: Brandon <[email protected]>
Date:   Sat Mar 2 23:04:41 2024 -0800

    deleted timer

commit d30dc08
Author: Sofie Budman <[email protected]>
Date:   Sat Mar 2 23:03:28 2024 -0800

    encoders worky

commit dba004b
Merge: 987a02d 1ac6c55
Author: stwiggy <[email protected]>
Date:   Sat Mar 2 22:52:35 2024 -0800

    Merge branch 'sofie-arm' of https://github.com/DeepBlueRobotics/RobotCode2024 into sofie-arm

commit 987a02d
Author: stwiggy <[email protected]>
Date:   Sat Mar 2 22:52:32 2024 -0800

    wrapped arm pids

commit 1ac6c55
Author: Sofie Budman <[email protected]>
Date:   Sat Mar 2 22:49:53 2024 -0800

    armpid stuff

commit 9dc6e76
Merge: 1c54cf6 fe8fcb4
Author: stwiggy <[email protected]>
Date:   Sat Mar 2 22:20:54 2024 -0800

    Merge branch 'sofie-arm' of https://github.com/DeepBlueRobotics/RobotCode2024 into sofie-arm

commit 1c54cf6
Author: stwiggy <[email protected]>
Date:   Sat Mar 2 22:20:09 2024 -0800

    arm boundaries

commit fe8fcb4
Author: Brandon <[email protected]>
Date:   Sat Mar 2 22:18:32 2024 -0800

    Readded COM stuff

commit 832386d
Merge: 79e1759 6dd5c89
Author: Brandon <[email protected]>
Date:   Sat Mar 2 22:16:11 2024 -0800

    Merge branch 'sofie-arm' of https://github.com/DeepBlueRobotics/RobotCode2024 into sofie-arm

commit 79e1759
Author: Brandon <[email protected]>
Date:   Sat Mar 2 22:16:00 2024 -0800

    Fixed feed forward

commit 6dd5c89
Merge: 83100c2 2072eeb
Author: stwiggy <[email protected]>
Date:   Sat Mar 2 22:14:26 2024 -0800

    Merge branch 'sofie-arm' of https://github.com/DeepBlueRobotics/RobotCode2024 into sofie-arm

commit 83100c2
Author: stwiggy <[email protected]>
Date:   Sat Mar 2 22:14:22 2024 -0800

    no constants in arm

commit 2072eeb
Author: Sofie Budman <[email protected]>
Date:   Sat Mar 2 22:02:55 2024 -0800

    got rid of timer again

commit dc69883
Author: Sofie Budman <[email protected]>
Date:   Sat Mar 2 22:00:22 2024 -0800

    move pid

commit f55615e
Merge: e00d3c1 3ecde74
Author: aaronC34 <[email protected]>
Date:   Sat Mar 2 21:59:20 2024 -0800

    Merge branch 'sofie-arm' of https://github.com/DeepBlueRobotics/RobotCode2024 into sofie-arm

commit e00d3c1
Author: aaronC34 <[email protected]>
Date:   Sat Mar 2 21:58:51 2024 -0800

    added two motors in set volts

commit 3ecde74
Author: Sofie Budman <[email protected]>
Date:   Sat Mar 2 21:58:09 2024 -0800

    arm current state worky

commit 69c94ff
Author: Brandon <[email protected]>
Date:   Sat Mar 2 21:56:55 2024 -0800

    fixed feed forward

commit 8ff445e
Author: Brandon <[email protected]>
Date:   Sat Mar 2 21:52:30 2024 -0800

    armAtSetpoint()

commit 8fe76e7
Merge: f3e0171 36d1890
Author: aaronC34 <[email protected]>
Date:   Sat Mar 2 21:50:43 2024 -0800

    Merge branch 'sofie-arm' of https://github.com/DeepBlueRobotics/RobotCode2024 into sofie-arm

commit f3e0171
Author: aaronC34 <[email protected]>
Date:   Sat Mar 2 21:50:41 2024 -0800

    adeed sysid

commit 36d1890
Merge: 30e00bf 7ce82f7
Author: stwiggy <[email protected]>
Date:   Sat Mar 2 21:46:31 2024 -0800

    Merge branch 'sofie-arm' of https://github.com/DeepBlueRobotics/RobotCode2024 into sofie-arm

commit 30e00bf
Author: stwiggy <[email protected]>
Date:   Sat Mar 2 21:46:27 2024 -0800

    no timer

commit 7ce82f7
Author: Brandon <[email protected]>
Date:   Sat Mar 2 21:45:56 2024 -0800

    fixed arm teleop

commit c31cefc
Merge: fc75e06 09e2322
Author: Sofie Budman <[email protected]>
Date:   Sat Mar 2 21:42:23 2024 -0800

    Merge branch 'sofie-arm' of https://github.com/deepbluerobotics/RobotCode2024 into sofie-arm

commit fc75e06
Author: Sofie Budman <[email protected]>
Date:   Sat Mar 2 21:42:20 2024 -0800

    public now it was bad

commit 09e2322
Merge: 814cd64 67be12a
Author: stwiggy <[email protected]>
Date:   Sat Mar 2 21:42:16 2024 -0800

    Merge branch 'sofie-arm' of https://github.com/DeepBlueRobotics/RobotCode2024 into sofie-arm

commit 814cd64
Author: stwiggy <[email protected]>
Date:   Sat Mar 2 21:42:12 2024 -0800

    deleted smartdashboards

commit 67be12a
Author: Sofie Budman <[email protected]>
Date:   Sat Mar 2 21:41:41 2024 -0800

    public -->private

commit a84323d
Author: Sofie Budman <[email protected]>
Date:   Sat Mar 2 21:40:30 2024 -0800

    delted timer

commit e5326cd
Author: stwiggy <[email protected]>
Date:   Sat Mar 2 21:38:04 2024 -0800

    fixed ff

commit 4905679
Author: Sofie Budman <[email protected]>
Date:   Sat Mar 2 21:36:12 2024 -0800

    made constants good

commit 9f78fd7
Author: stwiggy <[email protected]>
Date:   Sat Mar 2 21:34:50 2024 -0800

    merge

commit 5e97f39
Merge: e779bdb 7814fcd
Author: stwiggy <[email protected]>
Date:   Sat Mar 2 21:34:40 2024 -0800

    Merge branch 'sofie-arm' of https://github.com/DeepBlueRobotics/RobotCode2024 into sofie-arm

commit e779bdb
Merge: f4b812c 21f8ba9
Author: stwiggy <[email protected]>
Date:   Sat Mar 2 21:33:47 2024 -0800

    Merge branch 'sofie-arm' of https://github.com/DeepBlueRobotics/RobotCode2024 into sofie-arm

commit 7814fcd
Merge: aa97468 d2608ba
Author: Sofie Budman <[email protected]>
Date:   Sat Mar 2 21:29:45 2024 -0800

    Merge branch 'sofie-arm' of https://github.com/deepbluerobotics/RobotCode2024 into sofie-arm

commit aa97468
Author: Sofie Budman <[email protected]>
Date:   Sat Mar 2 21:27:27 2024 -0800

    fixed encoders + motor to follower

commit d2608ba
Author: Brandon <[email protected]>
Date:   Sat Mar 2 21:27:11 2024 -0800

    Master + Follower Motors

commit f4b812c
Author: stwiggy <[email protected]>
Date:   Sat Mar 2 21:27:10 2024 -0800

    formatted constants

commit 21f8ba9
Author: Brandon <[email protected]>
Date:   Sat Mar 2 21:25:52 2024 -0800

    More organization

commit 84db8ea
Author: Brandon <[email protected]>
Date:   Sat Mar 2 21:24:53 2024 -0800

    Degrees to raidans

commit 656e1ad
Author: Brandon <[email protected]>
Date:   Sat Mar 2 21:24:03 2024 -0800

    Fixed erros within constants

commit 242e288
Author: Brandon <[email protected]>
Date:   Sat Mar 2 21:23:10 2024 -0800

    More Constants things

commit 48c0c01
Merge: a9d3dfa 92e51b8
Author: Brandon <[email protected]>
Date:   Sat Mar 2 21:22:52 2024 -0800

    Merge branch 'sofie-arm' of https://github.com/DeepBlueRobotics/RobotCode2024 into sofie-arm

commit a9d3dfa
Author: Brandon <[email protected]>
Date:   Sat Mar 2 21:15:57 2024 -0800

    added snake casing

commit 92e51b8
Author: Sofie Budman <[email protected]>
Date:   Sat Mar 2 21:15:02 2024 -0800

    organized constants

commit e0b5447
Author: ProfessorAtomicManiac <[email protected]>
Date:   Sat Mar 2 11:49:18 2024 -0800

    Revert "renamed variables to master and follower"

    This reverts commit 2ca1604.

commit 840a3b4
Author: ProfessorAtomicManiac <[email protected]>
Date:   Sat Mar 2 11:49:04 2024 -0800

    Revert "Cleaned Constants.java"

    This reverts commit ae60a0c.

commit 60e0d4b
Author: ProfessorAtomicManiac <[email protected]>
Date:   Sat Mar 2 11:48:43 2024 -0800

    Revert "set correct motor and encoder configs"

    This reverts commit ed4363b.

commit 74bd52f
Author: ProfessorAtomicManiac <[email protected]>
Date:   Sat Mar 2 11:48:32 2024 -0800

    Revert "coded Arm Subsystem"

    This reverts commit 3e9b62f.

commit 52bca56
Author: ProfessorAtomicManiac <[email protected]>
Date:   Sat Mar 2 11:48:19 2024 -0800

    Revert "finished arm subsystem"

    This reverts commit f141379.

commit 65c7640
Author: ProfessorAtomicManiac <[email protected]>
Date:   Sat Mar 2 11:47:43 2024 -0800

    Revert "added SysID"

    This reverts commit 3aa63e4.

commit 3aa63e4
Author: ProfessorAtomicManiac <[email protected]>
Date:   Sat Mar 2 11:14:35 2024 -0800

    added SysID

commit f141379
Author: ProfessorAtomicManiac <[email protected]>
Date:   Sat Mar 2 10:17:06 2024 -0800

    finished arm subsystem

commit 3e9b62f
Author: ProfessorAtomicManiac <[email protected]>
Date:   Sat Mar 2 09:56:10 2024 -0800

    coded Arm Subsystem

commit ed4363b
Author: ProfessorAtomicManiac <[email protected]>
Date:   Sat Mar 2 09:28:20 2024 -0800

    set correct motor and encoder configs

commit ae60a0c
Author: ProfessorAtomicManiac <[email protected]>
Date:   Sat Mar 2 01:56:59 2024 -0800

    Cleaned Constants.java

commit 2ca1604
Author: ProfessorAtomicManiac <[email protected]>
Date:   Sat Mar 2 01:37:54 2024 -0800

    renamed variables to master and follower

commit 4af15e5
Author: ProfessorAtomicManiac <[email protected]>
Date:   Sat Mar 2 01:32:46 2024 -0800

    deleted unrelated subsystem

commit e4bdef2
Author: ProfessorAtomicManiac <[email protected]>
Date:   Sat Mar 2 01:32:31 2024 -0800

    delete unused command

commit 3e58126
Author: Juliaaaahhhh <[email protected]>
Date:   Sat Mar 2 00:40:52 2024 -0800

    minor misc changes

commit bd5793c
Author: stwiggy <[email protected]>
Date:   Sat Mar 2 00:21:51 2024 -0800

    no more errors in armteleop woo hoo

commit 5d98af8
Author: stwiggy <[email protected]>
Date:   Sat Mar 2 00:17:10 2024 -0800

    calculateCustomSetPoint added - 1 error left

commit aba73c3
Author: Sofie Budman <[email protected]>
Date:   Fri Mar 1 23:43:17 2024 -0800

    fixed arm.java

commit 3160103
Author: Sofie Budman <[email protected]>
Date:   Fri Mar 1 23:33:31 2024 -0800

    fixed driveArm

commit 07eec62
Author: Sofie Budman <[email protected]>
Date:   Fri Mar 1 23:09:17 2024 -0800

    constants + issues

commit a6b4f82
Merge: 44b3ccd e472500
Author: Sofie Budman <[email protected]>
Date:   Fri Mar 1 23:04:51 2024 -0800

    Merge branch 'sofie-arm' of https://github.com/deepbluerobotics/RobotCode2024 into sofie-arm

commit 44b3ccd
Author: Sofie Budman <[email protected]>
Date:   Fri Mar 1 23:04:47 2024 -0800

    fixed constants

commit e472500
Author: Kenneth-Choothakan <[email protected]>
Date:   Fri Mar 1 23:01:57 2024 -0800

    fixed requested speeds

commit 494eea0
Author: Kenneth-Choothakan <[email protected]>
Date:   Fri Mar 1 22:57:06 2024 -0800

    removed commands

commit 8a91064
Merge: 23581c6 26f86ef
Author: Sofie Budman <[email protected]>
Date:   Fri Mar 1 22:52:08 2024 -0800

    Merge branch 'sofie-arm' of https://github.com/deepbluerobotics/RobotCode2024 into sofie-arm

commit 23581c6
Author: Sofie Budman <[email protected]>
Date:   Fri Mar 1 22:51:21 2024 -0800

    got rid of issues and made the drive arm method

commit 26f86ef
Author: Kenneth-Choothakan <[email protected]>
Date:   Fri Mar 1 22:47:35 2024 -0800

    Added master and slave motor

commit cbd88d6
Author: Sofie Budman <[email protected]>
Date:   Fri Mar 1 20:35:08 2024 -0800

    copied 2023

commit 9d871f8
Author: Juliaaaahhhh <[email protected]>
Date:   Fri Mar 1 19:18:21 2024 -0800

    Trapezoid Math Stuff :]

commit ab2ef91
Author: Juliaaaahhhh <[email protected]>
Date:   Fri Mar 1 17:18:35 2024 -0800

    max feed forward acceleration added + max voltage in constants

commit 5b7a4a5
Author: Juliaaaahhhh <[email protected]>
Date:   Fri Mar 1 16:30:24 2024 -0800

    got rid of Constants.arm on all constants

commit 0686ec9
Author: Kenneth-Choothakan <[email protected]>
Date:   Thu Feb 29 21:28:05 2024 -0800

    Added a lot more buttons for 3 dif speaker, and climber

commit 1957d0c
Author: Kenneth-Choothakan <[email protected]>
Date:   Fri Mar 1 04:53:12 2024 +0000

    resolved some comments B)

commit 9092c78
Author: Kenneth-Choothakan <[email protected]>
Date:   Thu Feb 29 20:01:29 2024 -0800

    ArmTeleop first attempt

commit 04b8b67
Merge: adee65f 8f1e2a1
Author: Sofie Budman <[email protected]>
Date:   Tue Feb 27 19:45:38 2024 -0800

    Merge branch 'aaron-and-Raya-Arm' of https://github.com/deepbluerobotics/RobotCode2024 into aaron-and-Raya-Arm

commit adee65f
Author: Sofie Budman <[email protected]>
Date:   Tue Feb 27 19:45:33 2024 -0800

    izone

commit 8f1e2a1
Author: Juliaaaahhhh <[email protected]>
Date:   Tue Feb 27 19:24:25 2024 -0800

    driveArm, delete unecessary stuff, etc.

commit 6acb48c
Author: Sofie Budman <[email protected]>
Date:   Tue Feb 27 17:19:41 2024 -0800

    changed ot use setFF

commit 20fab44
Author: asthmaticlibrarian <[email protected]>
Date:   Tue Feb 27 16:51:50 2024 -0800

    Update Constants.java

    added arm weight and length

commit 1c09a6d
Author: Sofie Budman <[email protected]>
Date:   Tue Feb 27 16:37:09 2024 -0800

    arm pid

commit 5e3faf0
Author: Sofie Budman <[email protected]>
Date:   Tue Feb 27 16:14:21 2024 -0800

    Update Arm.java

commit 99ae01a
Author: theodoremui <[email protected]>
Date:   Tue Feb 20 18:34:14 2024 -0800

    adding changes to ArmTeleop

commit 4e126b8
Author: Sofie Budman <[email protected]>
Date:   Tue Feb 20 21:49:44 2024 +0000

    fgggf

commit 247d9d7
Merge: 28ca8a5 a28d22c
Author: Kenneth-Choothakan <[email protected]>
Date:   Tue Feb 20 13:19:17 2024 -0800

    Kenneth merge the thing

commit 28ca8a5
Author: Kenneth-Choothakan <[email protected]>
Date:   Tue Feb 20 13:08:46 2024 -0800

    Kenneth first attempt at the button arm angle movements

commit a28d22c
Author: Sofie Budman <[email protected]>
Date:   Tue Feb 20 07:05:58 2024 +0900

    timer

commit bbeca40
Author: Sofie Budman <[email protected]>
Date:   Mon Feb 19 22:18:39 2024 +0900

    changed trapezoid profile to not use deprecated stuff

commit b2b20e2
Author: Kenneth-Choothakan <[email protected]>
Date:   Sat Feb 3 03:34:28 2024 +0000

    Tried implementing trapezoidal profile

commit 47e0138
Author: Kenneth-Choothakan <[email protected]>
Date:   Sat Feb 3 01:21:42 2024 +0000

    Merged code

commit 3c19b6a
Merge: edc775e c5f4b75
Author: Kenneth-Choothakan <[email protected]>
Date:   Sat Feb 3 01:21:33 2024 +0000

    Completed merge of new stuff

commit edc775e
Author: Kenneth-Choothakan <[email protected]>
Date:   Sat Feb 3 01:18:50 2024 +0000

    Merged some code

commit c5f4b75
Author: ProfessorAtomicManiac <[email protected]>
Date:   Tue Jan 30 16:45:23 2024 -0800

    improved arm code stubs - Aaron

commit aedeadd
Merge: 67b9564 4438311
Author: Kenneth-Choothakan <[email protected]>
Date:   Sun Jan 28 20:23:16 2024 +0000

    I merged the changes from friday

commit 67b9564
Author: Kenneth-Choothakan <[email protected]>
Date:   Sun Jan 28 19:51:02 2024 +0000

    Arm teleop mode code is 3/4 way done except trapezoidal movement

commit 4438311
Author: FriedLongJohns <[email protected]>
Date:   Sat Jan 27 18:57:48 2024 -0800

    remove random 'd'

commit ead8ece
Author: RayaSkaf <[email protected]>
Date:   Sun Jan 28 00:21:17 2024 +0000

    Created the commands for the different positions for arm

commit 74d6d96
Author: RayaSkaf <[email protected]>
Date:   Sat Jan 27 23:58:27 2024 +0000

    Commented out one of the arms because only one is needed

commit abcee2b
Author: theodoremui <[email protected]>
Date:   Fri Jan 26 17:17:22 2024 -0800

    fixing ArmTeleop constructor arguments

commit 459871d
Author: theodoremui <[email protected]>
Date:   Fri Jan 26 17:08:21 2024 -0800

    adding pid controller

commit 4eb5cc5
Author: theodoremui <[email protected]>
Date:   Fri Jan 26 17:05:19 2024 -0800

    adding pid controller

commit 8b3cf15
Author: aaron345678 <[email protected]>
Date:   Sat Jan 20 00:22:11 2024 +0000

    Resolved all prs from Alexander the great

commit 2f54547
Author: Kenneth-Choothakan <[email protected]>
Date:   Sat Jan 20 00:09:55 2024 +0000

    fixed some of the comments

commit fa169c8
Author: aaronchun345678 <[email protected]>
Date:   Thu Jan 18 22:23:55 2024 -0800

    Fix arm code stubs

commit 71cabca
Merge: 1496ebd 956c274
Author: Kenneth-Choothakan <[email protected]>
Date:   Thu Jan 18 20:43:18 2024 -0800

    Merge branch 'master' into aaron-and-Raya-Arm

commit 1496ebd
Author: Kenneth-Choothakan <[email protected]>
Date:   Fri Jan 19 04:25:26 2024 +0000

    changed armteleop so its a defualt command instead of periodic in subsystem

commit 65dc6b6
Author: aaronchun345678 <[email protected]>
Date:   Thu Jan 18 19:38:52 2024 -0800

    Arm code stubs

commit 07dffeb
Author: DriverStationComputer <[email protected]>
Date:   Tue Jan 16 19:45:18 2024 -0800

    added getArmClampedGoal

commit b9495e1
Author: DriverStationComputer <[email protected]>
Date:   Tue Jan 16 18:06:18 2024 -0800

    added more ath

commit 26d86d3
Author: DriverStationComputer <[email protected]>
Date:   Tue Jan 16 17:32:51 2024 -0800

    fixed most errors
  • Loading branch information
@FriedLongJohns
FriedLongJohns committed Mar 10, 2024
1 parent 93e0da3 commit a851b49
Show file tree
Hide file tree
Showing 12 changed files with 623 additions and 45 deletions.
84 changes: 77 additions & 7 deletions src/main/java/org/carlmontrobotics/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,13 @@
import edu.wpi.first.wpilibj.XboxController.Button;
import edu.wpi.first.wpilibj.util.Color;
import edu.wpi.first.wpilibj.util.Color8Bit;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.units.Angle;
import edu.wpi.first.units.Measure;
import edu.wpi.first.units.Units.*;
import edu.wpi.first.wpilibj.XboxController.Button;

/**
* The Constants class provides a convenient place for teams to hold robot-wide
Expand All @@ -26,7 +33,7 @@ public final class Constants {

public static final class Led {
public static final int ledLength = 5;

public static final Color8Bit defaultColor = new Color8Bit(0, 0, 200);
public static final Color8Bit detectNote = new Color8Bit(250,140,3);
public static final Color8Bit holding = new Color8Bit(0,250,0);
Expand All @@ -38,7 +45,7 @@ public static final class Led {
//Red when nothing, purple/blue when intake/outtake detect only, green when both
}

public static final class IntakeShoot {
public static final class Effectorc {
// PID values
public static final int INTAKE = 0;
public static final int OUTTAKE = 1;
Expand All @@ -60,7 +67,7 @@ public static final class IntakeShoot {

public static final double OUTAKE_RPM_CLOSE = 4000;
public static final double OUTAKE_RPM_SAFE = 7000;

public static final double INTAKE_RPM = 6000;
public static final double INTAKE_SLOWDOWN_RPM = 1500;

Expand All @@ -72,9 +79,11 @@ public static final class IntakeShoot {
public static final double EJECT_RPM_INTAKE = 750;
public static final double EJECT_RPM_OUTAKE = 750;

public static final double MANUAL_RPM_MAX = 4000;

public static final double RPM_TOLERANCE = 10;
public static final double SPEAKER_HEIGHT_INCHES = 83; // inches

public static final double RPM_TOLERANCE = 20;
public static final double SPEAKER_HEIGHT_INCHES = 83; // inches

public static final boolean INTAKE_MOTOR_INVERSION = false;
public static final boolean OUTAKE_MOTOR_INVERSION = true;
Expand All @@ -90,17 +99,79 @@ public static final class IntakeShoot {

}

public static final class Armc {

// Motor port
public static final int ARM_MOTOR_PORT_MASTER = 13;
public final static int ARM_MOTOR_PORT_FOLLOWER = 18;
// Config for motors
public static final boolean MOTOR_INVERTED_MASTER = false;
public static final boolean MOTOR_INVERTED_FOLLOWER = true; //verifyed by design AND physical testing

public static final double ROTATION_TO_RAD = 2 * Math.PI;
public static final boolean ENCODER_INVERTED = false;

public static final int MAX_VOLTAGE = 12;
public static final double ENCODER_OFFSET_RAD = 0;

// TODO: finish understand why this is broken public static final Measure<Angle>
// INTAKE_ANGLE = Degrees.to(-1);

// USE RADIANS FOR THE ARM
public static final double INTAKE_ANGLE_RAD = Units.degreesToRadians(0);
public static final double AMP_ANGLE_RAD = Units.degreesToRadians(105);
public static final double SUBWOFFER_ANGLE_RAD = Units.degreesToRadians(24);
public static final double SAFE_ZONE_ANGLE_RAD = Units.degreesToRadians(24);
public static final double PODIUM_ANGLE_RAD = Units.degreesToRadians(24);
public static final double CLIMBER_UP_ANGLE_RAD = Units.degreesToRadians(24);
public static final double CLIMBER_DOWN_ANGLE_RAD = Units.degreesToRadians(24);

// PID, Feedforward, Trapezoid
public static final double kP = 0.1;
public static final double kI = 0.1;
public static final double kD = 0.1;
public static final double kS = 0.1;
public static final double kG = 0.1;
public static final double kV = 0.1;
public static final double kA = 0.1;
public static final double IZONE_RAD = .09;
//fine for now, change it later before use - ("Incorect use of setIZone()" Issue #22)
public static final double MAX_FF_VEL_RAD_P_S = 9.92; //rad/s Aarav did the work
public static final double MAX_FF_ACCEL_RAD_P_S = .0183; // rad / s^2 Aarav did the math

// if needed
public static final double COM_ARM_LENGTH_METERS = 0.381;
public static final double ARM_MASS_KG = 9.59302503;

public static TrapezoidProfile.Constraints TRAP_CONSTRAINTS;//initalized by arm constructor
// other0;

// public static final double MARGIN_OF_ERROR = Math.PI / 18;

// Boundaries
public static final double ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD = 1.8345; // placeholder
public static final double POS_TOLERANCE_RAD = Math.PI/512; // placeholder //Whether or not this is the actual account
// idk TODO: test on actual encoder without a conversion
// factor
public static final double VEL_TOLERANCE_RAD_P_SEC = (POS_TOLERANCE_RAD/0.02); // 20ms per robot loop
public static final double UPPER_ANGLE_LIMIT_RAD = Units.degreesToRadians(70);
public static final double LOWER_ANGLE_LIMIT_RAD = Units.degreesToRadians(0);
public static final double ARM_DISCONT_RAD = (LOWER_ANGLE_LIMIT_RAD + UPPER_ANGLE_LIMIT_RAD) / 2 - Math.PI;

}

public static final class Limelight {
}


public static final class OI {
public static final class Driver {
public static final int port = 0;

public static final int slowDriveButton = Button.kLeftBumper.value;
public static final int resetFieldOrientationButton = Button.kRightBumper.value;
public static final int toggleFieldOrientedButton = Button.kStart.value;

public static final int rotateFieldRelative0Deg = Button.kY.value;
public static final int rotateFieldRelative90Deg = Button.kB.value;
public static final int rotateFieldRelative180Deg = Button.kA.value;
Expand All @@ -125,7 +196,6 @@ public static final class Manipulator {
public static final int SPEAKER_POS = Button.kA.value;
public static final int INTAKE_POS = Button.kX.value;
public static final int EJECT_RPM = Button.kY.value;

}
public static final double JOY_THRESH = 0.01;
public static final double MIN_AXIS_TRIGGER_VALUE = 0.2;//woah, this is high.
Expand Down
115 changes: 84 additions & 31 deletions src/main/java/org/carlmontrobotics/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,13 +8,10 @@
//199 files
import org.carlmontrobotics.commands.*;
import static org.carlmontrobotics.Constants.*;

import org.carlmontrobotics.Constants.OI;
import org.carlmontrobotics.Constants.OI.*;

//subsystems
import org.carlmontrobotics.subsystems.*;

//controllers
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.GenericHID.RumbleType;
Expand All @@ -23,26 +20,31 @@
//commands
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
import edu.wpi.first.wpilibj2.command.PrintCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
//control bindings
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import edu.wpi.first.wpilibj2.command.button.POVButton;
import edu.wpi.first.wpilibj2.command.button.Trigger;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;

public class RobotContainer {
// 1. using GenericHID allows us to use different kinds of controllers
// 2. Use absolute paths from constants to reduce confusion
//private final GenericHID driverController = new GenericHID(OI.Driver.port);
private final GenericHID manipulatorController = new GenericHID(OI.Manipulator.port);
public final GenericHID driverController = new GenericHID(OI.Driver.port);
public final GenericHID manipulatorController = new GenericHID(OI.Manipulator.port);

private final IntakeShooter intakeShooter = new IntakeShooter();
public Arm arm = new Arm();

public RobotContainer() {
// setupAutos();
setDefaultCommands();
setBindingsDriver();
setBindingsManipulator();
}

private void setDefaultCommands() {
// drivetrain.setDefaultCommand(new TeleopDrive(
// drivetrain,
Expand All @@ -51,17 +53,17 @@ private void setDefaultCommands() {
// () -> ProcessedAxisValue(driverController, Axis.kRightX),
// () -> driverController.getRawButton(OI.Driver.slowDriveButton)
// ));
// intakeShooter.setDefaultCommand(new TeleopEffector(
// intakeShooter,
// () -> ProcessedAxisValue(manipulatorController, Axis.kLeftY),
// ));
// arm.setDefaultCommand(new TeleopArm(
// arm,
// () -> ProcessedAxisValue(manipulatorController, Axis.kRightY)
// ));
intakeShooter.setDefaultCommand(new TeleopEffector(
intakeShooter,
() -> ProcessedAxisValue(manipulatorController, Axis.kLeftY)
));
arm.setDefaultCommand(new TeleopArm(
arm,
() -> ProcessedAxisValue(manipulatorController, Axis.kRightY)
));
}
private void setBindingsDriver() {}

private void setBindingsManipulator() {
//NEW BINDINGS(easier for manipulator)
//Xbox left joy Y axis -> raw Intake/Outtake control
Expand All @@ -72,41 +74,40 @@ private void setBindingsManipulator() {
//Xbox right bumper button -> SAFE Speaker pos , Fire
//Xbox X button -> goto Intake pos
//Xbox Y button -> Eject rpm

/*/Multi-commands/*/
new JoystickButton(manipulatorController, OI.Manipulator.INTAKE)
.onTrue(new SequentialCommandGroup(
//needs movearm
.onTrue(new ParallelCommandGroup(
new MoveToPos(arm, Armc.INTAKE_ANGLE_RAD),
new Intake(intakeShooter)
));
/*/Shooting/*/
new JoystickButton(manipulatorController, OI.Manipulator.SPEAKER_CLOSE)
new JoystickButton(manipulatorController, OI.Manipulator.SPEAKER_CLOSE)//aka podium
.onTrue(new SequentialCommandGroup(
//needs movearm
//needs ramprpm
new MoveToPos(arm, Armc.PODIUM_ANGLE_RAD),
new RampToRPM(intakeShooter, Effectorc.OUTAKE_RPM_CLOSE),
new PassToOutake(intakeShooter)
));
new JoystickButton(manipulatorController, OI.Manipulator.SPEAKER_SAFE)
.onTrue(new SequentialCommandGroup(
//needs movearm
//needs ramprpm
new MoveToPos(arm, Armc.SAFE_ZONE_ANGLE_RAD),
new RampToRPM(intakeShooter, Effectorc.OUTAKE_RPM_SAFE),
new PassToOutake(intakeShooter)
));
new JoystickButton(manipulatorController, OI.Manipulator.AMP)
new JoystickButton(manipulatorController, OI.Manipulator.AMP)//MELEE ATTACK
.onTrue(new SequentialCommandGroup(
//needs movearm
//needs ramprpm
new MoveToPos(arm, Armc.AMP_ANGLE_RAD),
new Eject(intakeShooter)
));
/*/Singulars/*/
/*new JoystickButton(manipulatorController, OI.Manipulator.INTAKE_POS)
.onTrue(NEEDS MOVEARM);*/
/*/Singulars/*/
new JoystickButton(manipulatorController, OI.Manipulator.INTAKE_POS)
.onTrue(new MoveToPos(arm, Armc.INTAKE_ANGLE_RAD));
new JoystickButton(manipulatorController, OI.Manipulator.EJECT_RPM)
.onTrue(new Eject(intakeShooter));
// new JoystickButton(manipulatorController, Button.kLeftStick.value)
// .onTrue(new InstantCommand(() -> {manipulatorController.setRumble(RumbleType.kBothRumble, 1);}));


//TODO: ask charles if passing in controller is okay
}

Expand All @@ -116,4 +117,56 @@ public Command getAutonomousCommand() {
//throw new UnsupportedOperationException("Unimplemented method 'getAutonomousCommand'");
}

/**
* Flips an axis' Y coordinates upside down, but only if the select axis is a
* joystick axis
*
* @param hid The controller/plane joystick the axis is on
* @param axis The processed axis
* @return The processed value.
*/
private double getStickValue(GenericHID hid, Axis axis) {
return hid.getRawAxis(axis.value) * (axis == Axis.kLeftY || axis == Axis.kRightY ? -1 : 1);
}

/**
* Processes an input from the joystick into a value between -1 and 1,
* sinusoidally instead of linearly
*
* @param value The value to be processed.
* @return The processed value.
*/
private double inputProcessing(double value) {
double processedInput;
// processedInput =
// (((1-Math.cos(value*Math.PI))/2)*((1-Math.cos(value*Math.PI))/2))*(value/Math.abs(value));
processedInput = Math.copySign(((1 - Math.cos(value * Math.PI)) / 2) * ((1 - Math.cos(value * Math.PI)) / 2),
value);
return processedInput;
}

/**
* Combines both getStickValue and inputProcessing into a single function for
* processing joystick outputs
*
* @param hid The controller/plane joystick the axis is on
* @param axis The processed axis
* @return The processed value.
*/
private double ProcessedAxisValue(GenericHID hid, Axis axis) {
return DeadzonedAxis(inputProcessing(getStickValue(hid, axis)));
}

/**
* Returns zero if a axis input is inside the deadzone
*
* @param hid The controller/plane joystick the axis is on
* @param axis The processed axis
* @return The processed value.
*/
private double DeadzonedAxis(double axOut) {
return (Math.abs(axOut) <= OI.JOY_THRESH) ? 0.0 : axOut;
}


}
2 changes: 1 addition & 1 deletion src/main/java/org/carlmontrobotics/commands/Eject.java
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
package org.carlmontrobotics.commands;

import static org.carlmontrobotics.Constants.IntakeShoot.*;
import static org.carlmontrobotics.Constants.Effectorc.*;

import org.carlmontrobotics.subsystems.IntakeShooter;

Expand Down
2 changes: 1 addition & 1 deletion src/main/java/org/carlmontrobotics/commands/Intake.java
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
package org.carlmontrobotics.commands;

import static org.carlmontrobotics.Constants.IntakeShoot.*;
import static org.carlmontrobotics.Constants.Effectorc.*;
import static org.carlmontrobotics.Constants.Led.*;

import org.carlmontrobotics.Constants;
Expand Down
45 changes: 45 additions & 0 deletions src/main/java/org/carlmontrobotics/commands/MoveToPos.java
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,45 @@
package org.carlmontrobotics.commands;

import static org.carlmontrobotics.Constants.Effectorc.*;
import static org.carlmontrobotics.Constants.Led.*;

import org.carlmontrobotics.Constants;
import org.carlmontrobotics.subsystems.Arm;
import org.carlmontrobotics.subsystems.AuxSystems;
import org.carlmontrobotics.subsystems.IntakeShooter;

import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.StartEndCommand;
public class MoveToPos extends Command {
//return true when close enough
private final Arm arm;
private double goal;

public MoveToPos(Arm armSubsystem, double goal) {
addRequirements(this.arm = armSubsystem);
this.goal=goal;
}

@Override
public void initialize() {
arm.setArmTarget(goal);
}


// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
}

// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
}

// Returns true when the command should end.
@Override
public boolean isFinished() {
return arm.armAtSetpoint();
}
}
Loading

0 comments on commit a851b49

Please sign in to comment.