diff --git a/Limelight Pipelines/GoodTPMORNINGMARCH7 (1).vpr b/Limelight Pipelines/GoodTPMORNINGMARCH7 (1).vpr new file mode 100644 index 0000000..66df1e5 --- /dev/null +++ b/Limelight Pipelines/GoodTPMORNINGMARCH7 (1).vpr @@ -0,0 +1,58 @@ +area_max:0.5393580481000001 +area_min:0.2097273616 +area_similarity:66 +aspect_max:20 +aspect_min:0.0000010124999999999998 +black_level:1 +blue_balance:1536 +calibration_type:0 +contour_grouping:0 +contour_sort_final:6 +convexity_max:64.5 +convexity_min:23.1 +corner_approx:2 +cross_a_a:1 +cross_a_x:0 +cross_a_y:0 +cross_b_a:1 +cross_b_x:0 +cross_b_y:0 +desc:GoodTPMORNINGMARCH7 +desired_contour_region:0 +dilation_steps:1 +direction_filter:0 +dual_close_sort_origin:0 +erosion_steps:0 +exposure:10 +force_convex:1 +hue_max:144 +hue_min:58 +image_flip:0 +image_source:0 +img_to_show:0 +intersection_filter:0 +pipeline_led_enabled:1 +pipeline_led_power:100 +pipeline_res:0 +pipeline_type:0 +red_balance:500 +roi_x:0 +roi_y:0 +sat_max:255 +sat_min:100 +send_corners:0 +send_raw_contours:0 +solve3d:0 +solve3d_algo:0 +solve3d_bindtarget:1 +solve3d_conf:0.990000 +solve3d_error:8 +solve3d_guess:0 +solve3d_iterations:50 +solve3d_precies:1 +solve3d_precise:0 +solve3d_zoffset:0 +val_max:254 +val_min:132 +y_max:1.000000 +y_min:-1.000000 diff --git a/Limelight Pipelines/GoodTPMORNINGMARCH7 (2).vpr b/Limelight Pipelines/GoodTPMORNINGMARCH7 (2).vpr new file mode 100644 index 0000000..fd27c61 --- /dev/null +++ b/Limelight Pipelines/GoodTPMORNINGMARCH7 (2).vpr @@ -0,0 +1,58 @@ +area_max:0.5393580481000001 +area_min:0.2097273616 +area_similarity:66 +aspect_max:20 +aspect_min:0.0000010124999999999998 +black_level:1 +blue_balance:1536 +calibration_type:0 +contour_grouping:0 +contour_sort_final:6 +convexity_max:64.5 +convexity_min:23.1 +corner_approx:2 +cross_a_a:1 +cross_a_x:0 +cross_a_y:0 +cross_b_a:1 +cross_b_x:0 +cross_b_y:0 +desc:GoodTPMORNINGMARCH7 +desired_contour_region:0 +dilation_steps:1 +direction_filter:0 +dual_close_sort_origin:0 +erosion_steps:0 +exposure:8 +force_convex:1 +hue_max:144 +hue_min:58 +image_flip:0 +image_source:0 +img_to_show:0 +intersection_filter:0 +pipeline_led_enabled:1 +pipeline_led_power:100 +pipeline_res:0 +pipeline_type:0 +red_balance:500 +roi_x:0 +roi_y:0 +sat_max:255 +sat_min:100 +send_corners:0 +send_raw_contours:0 +solve3d:0 +solve3d_algo:0 +solve3d_bindtarget:1 +solve3d_conf:0.990000 +solve3d_error:8 +solve3d_guess:0 +solve3d_iterations:50 +solve3d_precies:1 +solve3d_precise:0 +solve3d_zoffset:0 +val_max:254 +val_min:132 +y_max:1.000000 +y_min:-1.000000 diff --git a/Limelight Pipelines/GoodTPMORNINGMARCH7 (3).vpr b/Limelight Pipelines/GoodTPMORNINGMARCH7 (3).vpr new file mode 100644 index 0000000..8d255b1 --- /dev/null +++ b/Limelight Pipelines/GoodTPMORNINGMARCH7 (3).vpr @@ -0,0 +1,58 @@ +area_max:0.5393580481000001 +area_min:0.2097273616 +area_similarity:66 +aspect_max:20 +aspect_min:0.0000010124999999999998 +black_level:1 +blue_balance:1536 +calibration_type:0 +contour_grouping:0 +contour_sort_final:6 +convexity_max:64.5 +convexity_min:23.1 +corner_approx:2 +cross_a_a:1 +cross_a_x:0 +cross_a_y:0 +cross_b_a:1 +cross_b_x:0 +cross_b_y:0 +desc:GoodTPMORNINGMARCH7 +desired_contour_region:0 +dilation_steps:1 +direction_filter:0 +dual_close_sort_origin:0 +erosion_steps:0 +exposure:6 +force_convex:1 +hue_max:144 +hue_min:58 +image_flip:0 +image_source:0 +img_to_show:0 +intersection_filter:0 +pipeline_led_enabled:1 +pipeline_led_power:100 +pipeline_res:0 +pipeline_type:0 +red_balance:500 +roi_x:0 +roi_y:0 +sat_max:255 +sat_min:100 +send_corners:0 +send_raw_contours:0 +solve3d:0 +solve3d_algo:0 +solve3d_bindtarget:1 +solve3d_conf:0.990000 +solve3d_error:8 +solve3d_guess:0 +solve3d_iterations:50 +solve3d_precies:1 +solve3d_precise:0 +solve3d_zoffset:0 +val_max:254 +val_min:132 +y_max:1.000000 +y_min:-1.000000 diff --git a/Limelight Pipelines/GoodTPMORNINGMARCH7.vpr b/Limelight Pipelines/GoodTPMORNINGMARCH7.vpr new file mode 100644 index 0000000..fd27c61 --- /dev/null +++ b/Limelight Pipelines/GoodTPMORNINGMARCH7.vpr @@ -0,0 +1,58 @@ +area_max:0.5393580481000001 +area_min:0.2097273616 +area_similarity:66 +aspect_max:20 +aspect_min:0.0000010124999999999998 +black_level:1 +blue_balance:1536 +calibration_type:0 +contour_grouping:0 +contour_sort_final:6 +convexity_max:64.5 +convexity_min:23.1 +corner_approx:2 +cross_a_a:1 +cross_a_x:0 +cross_a_y:0 +cross_b_a:1 +cross_b_x:0 +cross_b_y:0 +desc:GoodTPMORNINGMARCH7 +desired_contour_region:0 +dilation_steps:1 +direction_filter:0 +dual_close_sort_origin:0 +erosion_steps:0 +exposure:8 +force_convex:1 +hue_max:144 +hue_min:58 +image_flip:0 +image_source:0 +img_to_show:0 +intersection_filter:0 +pipeline_led_enabled:1 +pipeline_led_power:100 +pipeline_res:0 +pipeline_type:0 +red_balance:500 +roi_x:0 +roi_y:0 +sat_max:255 +sat_min:100 +send_corners:0 +send_raw_contours:0 +solve3d:0 +solve3d_algo:0 +solve3d_bindtarget:1 +solve3d_conf:0.990000 +solve3d_error:8 +solve3d_guess:0 +solve3d_iterations:50 +solve3d_precies:1 +solve3d_precise:0 +solve3d_zoffset:0 +val_max:254 +val_min:132 +y_max:1.000000 +y_min:-1.000000 diff --git a/Limelight Pipelines/Pipeline_Name (1).vpr b/Limelight Pipelines/Pipeline_Name (1).vpr new file mode 100644 index 0000000..b132ff4 --- /dev/null +++ b/Limelight Pipelines/Pipeline_Name (1).vpr @@ -0,0 +1,58 @@ +area_max:100 +area_min:0.0244140625 +area_similarity:0 +aspect_max:20 +aspect_min:0.0000010124999999999998 +black_level:0 +blue_balance:1944 +calibration_type:0 +contour_grouping:0 +contour_sort_final:6 +convexity_max:43 +convexity_min:10 +corner_approx:2 +cross_a_a:1 +cross_a_x:0 +cross_a_y:0 +cross_b_a:1 +cross_b_x:0 +cross_b_y:0 +desc:Pipeline_Name +desired_contour_region:0 +dilation_steps:1 +direction_filter:0 +dual_close_sort_origin:0 +erosion_steps:0 +exposure:28 +force_convex:1 +hue_max:147 +hue_min:60 +image_flip:0 +image_source:0 +img_to_show:1 +intersection_filter:0 +pipeline_led_enabled:1 +pipeline_led_power:100 +pipeline_res:0 +pipeline_type:0 +red_balance:1130 +roi_x:0 +roi_y:0 +sat_max:255 +sat_min:95 +send_corners:0 +send_raw_contours:0 +solve3d:0 +solve3d_algo:0 +solve3d_bindtarget:1 +solve3d_conf:0.990000 +solve3d_error:8 +solve3d_guess:0 +solve3d_iterations:50 +solve3d_precies:1 +solve3d_precise:0 +solve3d_zoffset:0 +val_max:254 +val_min:163 +y_max:1.000000 +y_min:-1.000000 diff --git a/Limelight Pipelines/Pipeline_Name (10).vpr b/Limelight Pipelines/Pipeline_Name (10).vpr new file mode 100644 index 0000000..19abfbc --- /dev/null +++ b/Limelight Pipelines/Pipeline_Name (10).vpr @@ -0,0 +1,58 @@ +area_max:2.853430424100001 +area_min:0.005996953600000002 +area_similarity:0 +aspect_max:20 +aspect_min:0 +black_level:24 +blue_balance:1361 +calibration_type:0 +contour_grouping:0 +contour_sort_final:0 +convexity_max:100 +convexity_min:10 +corner_approx:2 +cross_a_a:1 +cross_a_x:-0.07 +cross_a_y:0 +cross_b_a:1 +cross_b_x:0 +cross_b_y:0 +desc:Pipeline_Name +desired_contour_region:0 +dilation_steps:1 +direction_filter:0 +dual_close_sort_origin:0 +erosion_steps:0 +exposure:41 +force_convex:1 +hue_max:65 +hue_min:55 +image_flip:0 +image_source:0 +img_to_show:0 +intersection_filter:0 +pipeline_led_enabled:1 +pipeline_led_power:100 +pipeline_res:0 +pipeline_type:0 +red_balance:2104 +roi_x:0 +roi_y:0 +sat_max:255 +sat_min:240 +send_corners:0 +send_raw_contours:0 +solve3d:0 +solve3d_algo:0 +solve3d_bindtarget:1 +solve3d_conf:0.990000 +solve3d_error:8 +solve3d_guess:0 +solve3d_iterations:50 +solve3d_precies:1 +solve3d_precise:0 +solve3d_zoffset:0 +val_max:58 +val_min:28 +y_max:1.000000 +y_min:-1.000000 diff --git a/Limelight Pipelines/Pipeline_Name (11).vpr b/Limelight Pipelines/Pipeline_Name (11).vpr new file mode 100644 index 0000000..ad6ee61 --- /dev/null +++ b/Limelight Pipelines/Pipeline_Name (11).vpr @@ -0,0 +1,58 @@ +area_max:100 +area_min:0.0244140625 +area_similarity:0 +aspect_max:20 +aspect_min:0.0000010124999999999998 +black_level:0 +blue_balance:1396 +calibration_type:0 +contour_grouping:0 +contour_sort_final:6 +convexity_max:43 +convexity_min:10 +corner_approx:2 +cross_a_a:1 +cross_a_x:0 +cross_a_y:0 +cross_b_a:1 +cross_b_x:0 +cross_b_y:0 +desc:Pipeline_Name +desired_contour_region:0 +dilation_steps:0 +direction_filter:0 +dual_close_sort_origin:0 +erosion_steps:0 +exposure:2 +force_convex:1 +hue_max:67 +hue_min:57 +image_flip:0 +image_source:0 +img_to_show:0 +intersection_filter:0 +pipeline_led_enabled:1 +pipeline_led_power:100 +pipeline_res:0 +pipeline_type:0 +red_balance:2500 +roi_x:0 +roi_y:0 +sat_max:255 +sat_min:60 +send_corners:0 +send_raw_contours:0 +solve3d:0 +solve3d_algo:0 +solve3d_bindtarget:1 +solve3d_conf:0.990000 +solve3d_error:8 +solve3d_guess:0 +solve3d_iterations:50 +solve3d_precies:1 +solve3d_precise:0 +solve3d_zoffset:0 +val_max:255 +val_min:35 +y_max:1.000000 +y_min:-1.000000 diff --git a/Limelight Pipelines/Pipeline_Name (12).vpr b/Limelight Pipelines/Pipeline_Name (12).vpr new file mode 100644 index 0000000..cb8c2ee --- /dev/null +++ b/Limelight Pipelines/Pipeline_Name (12).vpr @@ -0,0 +1,58 @@ +area_max:74.80520100000001 +area_min:1.0355301121 +area_similarity:32 +aspect_max:20 +aspect_min:0.0000010124999999999998 +black_level:0 +blue_balance:1396 +calibration_type:0 +contour_grouping:0 +contour_sort_final:2 +convexity_max:50.7 +convexity_min:27.6 +corner_approx:3.6 +cross_a_a:0.000000 +cross_a_x:0.000000 +cross_a_y:0 +cross_b_a:1 +cross_b_x:0 +cross_b_y:0 +desc:Pipeline_Name +desired_contour_region:1 +dilation_steps:0 +direction_filter:2 +dual_close_sort_origin:0 +erosion_steps:0 +exposure:63 +force_convex:1 +hue_max:33 +hue_min:23 +image_flip:0 +image_source:0 +img_to_show:0 +intersection_filter:0 +pipeline_led_enabled:1 +pipeline_led_power:100 +pipeline_res:0 +pipeline_type:0 +red_balance:2326 +roi_x:0 +roi_y:0 +sat_max:255 +sat_min:240 +send_corners:0 +send_raw_contours:0 +solve3d:0 +solve3d_algo:0 +solve3d_bindtarget:1 +solve3d_conf:0.990000 +solve3d_error:8 +solve3d_guess:0 +solve3d_iterations:50 +solve3d_precies:1 +solve3d_precise:0 +solve3d_zoffset:0 +val_max:255 +val_min:201 +y_max:1.000000 +y_min:-1.000000 diff --git a/Limelight Pipelines/Pipeline_Name (13).vpr b/Limelight Pipelines/Pipeline_Name (13).vpr new file mode 100644 index 0000000..319ce2a --- /dev/null +++ b/Limelight Pipelines/Pipeline_Name (13).vpr @@ -0,0 +1,58 @@ +area_max:5.9072816401 +area_min:0.1330863361 +area_similarity:66 +aspect_max:20 +aspect_min:0.0000010124999999999998 +black_level:1 +blue_balance:1536 +calibration_type:0 +contour_grouping:0 +contour_sort_final:6 +convexity_max:64.5 +convexity_min:23.1 +corner_approx:2 +cross_a_a:1 +cross_a_x:0 +cross_a_y:0 +cross_b_a:1 +cross_b_x:0 +cross_b_y:0 +desc:Pipeline_Name +desired_contour_region:0 +dilation_steps:1 +direction_filter:0 +dual_close_sort_origin:0 +erosion_steps:0 +exposure:12 +force_convex:1 +hue_max:144 +hue_min:59 +image_flip:0 +image_source:0 +img_to_show:0 +intersection_filter:0 +pipeline_led_enabled:1 +pipeline_led_power:100 +pipeline_res:0 +pipeline_type:0 +red_balance:500 +roi_x:0 +roi_y:0 +sat_max:255 +sat_min:100 +send_corners:0 +send_raw_contours:0 +solve3d:0 +solve3d_algo:0 +solve3d_bindtarget:1 +solve3d_conf:0.990000 +solve3d_error:8 +solve3d_guess:0 +solve3d_iterations:50 +solve3d_precies:1 +solve3d_precise:0 +solve3d_zoffset:0 +val_max:254 +val_min:84 +y_max:1.000000 +y_min:-1.000000 diff --git a/Limelight Pipelines/Pipeline_Name (14).vpr b/Limelight Pipelines/Pipeline_Name (14).vpr new file mode 100644 index 0000000..5f662c7 --- /dev/null +++ b/Limelight Pipelines/Pipeline_Name (14).vpr @@ -0,0 +1,58 @@ +area_max:25.843904016099994 +area_min:0.0244140625 +area_similarity:66 +aspect_max:20 +aspect_min:0.0000010124999999999998 +black_level:1 +blue_balance:1536 +calibration_type:0 +contour_grouping:0 +contour_sort_final:6 +convexity_max:64.5 +convexity_min:23.1 +corner_approx:2 +cross_a_a:1 +cross_a_x:0 +cross_a_y:0 +cross_b_a:1 +cross_b_x:0 +cross_b_y:0 +desc:Pipeline_Name +desired_contour_region:0 +dilation_steps:1 +direction_filter:0 +dual_close_sort_origin:0 +erosion_steps:0 +exposure:12 +force_convex:1 +hue_max:144 +hue_min:60 +image_flip:0 +image_source:0 +img_to_show:0 +intersection_filter:0 +pipeline_led_enabled:1 +pipeline_led_power:100 +pipeline_res:0 +pipeline_type:0 +red_balance:500 +roi_x:0 +roi_y:0 +sat_max:255 +sat_min:100 +send_corners:0 +send_raw_contours:0 +solve3d:0 +solve3d_algo:0 +solve3d_bindtarget:1 +solve3d_conf:0.990000 +solve3d_error:8 +solve3d_guess:0 +solve3d_iterations:50 +solve3d_precies:1 +solve3d_precise:0 +solve3d_zoffset:0 +val_max:254 +val_min:132 +y_max:1.000000 +y_min:-1.000000 diff --git a/Limelight Pipelines/Pipeline_Name (15).vpr b/Limelight Pipelines/Pipeline_Name (15).vpr new file mode 100644 index 0000000..16a44ce --- /dev/null +++ b/Limelight Pipelines/Pipeline_Name (15).vpr @@ -0,0 +1,58 @@ +area_max:12.199721696099997 +area_min:0.8999178496 +area_similarity:66 +aspect_max:20 +aspect_min:0.0000010124999999999998 +black_level:0 +blue_balance:1555 +calibration_type:0 +contour_grouping:0 +contour_sort_final:6 +convexity_max:64.5 +convexity_min:23.1 +corner_approx:2 +cross_a_a:1 +cross_a_x:0 +cross_a_y:0 +cross_b_a:1 +cross_b_x:0 +cross_b_y:0 +desc:Pipeline_Name +desired_contour_region:0 +dilation_steps:1 +direction_filter:0 +dual_close_sort_origin:0 +erosion_steps:0 +exposure:2 +force_convex:1 +hue_max:144 +hue_min:60 +image_flip:0 +image_source:0 +img_to_show:0 +intersection_filter:0 +pipeline_led_enabled:1 +pipeline_led_power:100 +pipeline_res:0 +pipeline_type:0 +red_balance:500 +roi_x:0 +roi_y:0 +sat_max:255 +sat_min:100 +send_corners:0 +send_raw_contours:0 +solve3d:0 +solve3d_algo:0 +solve3d_bindtarget:1 +solve3d_conf:0.990000 +solve3d_error:8 +solve3d_guess:0 +solve3d_iterations:50 +solve3d_precies:1 +solve3d_precise:0 +solve3d_zoffset:0 +val_max:254 +val_min:132 +y_max:1.000000 +y_min:-1.000000 diff --git a/Limelight Pipelines/Pipeline_Name (2).vpr b/Limelight Pipelines/Pipeline_Name (2).vpr new file mode 100644 index 0000000..3c21d14 --- /dev/null +++ b/Limelight Pipelines/Pipeline_Name (2).vpr @@ -0,0 +1,58 @@ +area_max:100 +area_min:0.0244140625 +area_similarity:0 +aspect_max:20 +aspect_min:0.0000010124999999999998 +black_level:0 +blue_balance:1944 +calibration_type:0 +contour_grouping:0 +contour_sort_final:6 +convexity_max:43 +convexity_min:10 +corner_approx:2 +cross_a_a:1 +cross_a_x:0 +cross_a_y:0 +cross_b_a:1 +cross_b_x:0 +cross_b_y:0 +desc:Pipeline_Name +desired_contour_region:0 +dilation_steps:1 +direction_filter:0 +dual_close_sort_origin:0 +erosion_steps:0 +exposure:28 +force_convex:1 +hue_max:147 +hue_min:60 +image_flip:0 +image_source:0 +img_to_show:0 +intersection_filter:0 +pipeline_led_enabled:1 +pipeline_led_power:100 +pipeline_res:0 +pipeline_type:0 +red_balance:1130 +roi_x:0 +roi_y:0 +sat_max:255 +sat_min:95 +send_corners:0 +send_raw_contours:0 +solve3d:0 +solve3d_algo:0 +solve3d_bindtarget:1 +solve3d_conf:0.990000 +solve3d_error:8 +solve3d_guess:0 +solve3d_iterations:50 +solve3d_precies:1 +solve3d_precise:0 +solve3d_zoffset:0 +val_max:254 +val_min:163 +y_max:1.000000 +y_min:-1.000000 diff --git a/Limelight Pipelines/Pipeline_Name (3).vpr b/Limelight Pipelines/Pipeline_Name (3).vpr new file mode 100644 index 0000000..a7490c2 --- /dev/null +++ b/Limelight Pipelines/Pipeline_Name (3).vpr @@ -0,0 +1,58 @@ +area_max:100 +area_min:0.0244140625 +area_similarity:66 +aspect_max:20 +aspect_min:0.0000010124999999999998 +black_level:0 +blue_balance:1944 +calibration_type:0 +contour_grouping:0 +contour_sort_final:6 +convexity_max:43 +convexity_min:10 +corner_approx:2 +cross_a_a:1 +cross_a_x:0 +cross_a_y:0 +cross_b_a:1 +cross_b_x:0 +cross_b_y:0 +desc:Pipeline_Name +desired_contour_region:0 +dilation_steps:1 +direction_filter:0 +dual_close_sort_origin:0 +erosion_steps:0 +exposure:28 +force_convex:1 +hue_max:147 +hue_min:60 +image_flip:0 +image_source:0 +img_to_show:0 +intersection_filter:0 +pipeline_led_enabled:1 +pipeline_led_power:100 +pipeline_res:0 +pipeline_type:0 +red_balance:1130 +roi_x:0 +roi_y:0 +sat_max:255 +sat_min:95 +send_corners:0 +send_raw_contours:0 +solve3d:0 +solve3d_algo:0 +solve3d_bindtarget:1 +solve3d_conf:0.990000 +solve3d_error:8 +solve3d_guess:0 +solve3d_iterations:50 +solve3d_precies:1 +solve3d_precise:0 +solve3d_zoffset:0 +val_max:254 +val_min:163 +y_max:1.000000 +y_min:-1.000000 diff --git a/Limelight Pipelines/Pipeline_Name (4).vpr b/Limelight Pipelines/Pipeline_Name (4).vpr new file mode 100644 index 0000000..223c668 --- /dev/null +++ b/Limelight Pipelines/Pipeline_Name (4).vpr @@ -0,0 +1,58 @@ +area_max:100 +area_min:0.0244140625 +area_similarity:66 +aspect_max:20 +aspect_min:0.0000010124999999999998 +black_level:0 +blue_balance:1944 +calibration_type:0 +contour_grouping:0 +contour_sort_final:6 +convexity_max:64.5 +convexity_min:23.1 +corner_approx:2 +cross_a_a:1 +cross_a_x:0 +cross_a_y:0 +cross_b_a:1 +cross_b_x:0 +cross_b_y:0 +desc:Pipeline_Name +desired_contour_region:0 +dilation_steps:1 +direction_filter:0 +dual_close_sort_origin:0 +erosion_steps:0 +exposure:28 +force_convex:1 +hue_max:147 +hue_min:60 +image_flip:0 +image_source:0 +img_to_show:0 +intersection_filter:0 +pipeline_led_enabled:1 +pipeline_led_power:100 +pipeline_res:0 +pipeline_type:0 +red_balance:1130 +roi_x:0 +roi_y:0 +sat_max:255 +sat_min:100 +send_corners:0 +send_raw_contours:0 +solve3d:0 +solve3d_algo:0 +solve3d_bindtarget:1 +solve3d_conf:0.990000 +solve3d_error:8 +solve3d_guess:0 +solve3d_iterations:50 +solve3d_precies:1 +solve3d_precise:0 +solve3d_zoffset:0 +val_max:254 +val_min:132 +y_max:1.000000 +y_min:-1.000000 diff --git a/Limelight Pipelines/Pipeline_Name (5).vpr b/Limelight Pipelines/Pipeline_Name (5).vpr new file mode 100644 index 0000000..a72fcc1 --- /dev/null +++ b/Limelight Pipelines/Pipeline_Name (5).vpr @@ -0,0 +1,58 @@ +area_max:25.843904016099994 +area_min:0.0244140625 +area_similarity:66 +aspect_max:20 +aspect_min:0.0000010124999999999998 +black_level:0 +blue_balance:1944 +calibration_type:0 +contour_grouping:0 +contour_sort_final:6 +convexity_max:64.5 +convexity_min:23.1 +corner_approx:2 +cross_a_a:1 +cross_a_x:0 +cross_a_y:0 +cross_b_a:1 +cross_b_x:0 +cross_b_y:0 +desc:Pipeline_Name +desired_contour_region:0 +dilation_steps:1 +direction_filter:0 +dual_close_sort_origin:0 +erosion_steps:0 +exposure:28 +force_convex:1 +hue_max:147 +hue_min:60 +image_flip:0 +image_source:0 +img_to_show:0 +intersection_filter:0 +pipeline_led_enabled:1 +pipeline_led_power:100 +pipeline_res:0 +pipeline_type:0 +red_balance:1130 +roi_x:0 +roi_y:0 +sat_max:255 +sat_min:100 +send_corners:0 +send_raw_contours:0 +solve3d:0 +solve3d_algo:0 +solve3d_bindtarget:1 +solve3d_conf:0.990000 +solve3d_error:8 +solve3d_guess:0 +solve3d_iterations:50 +solve3d_precies:1 +solve3d_precise:0 +solve3d_zoffset:0 +val_max:254 +val_min:132 +y_max:1.000000 +y_min:-1.000000 diff --git a/Limelight Pipelines/Pipeline_Name (6).vpr b/Limelight Pipelines/Pipeline_Name (6).vpr new file mode 100644 index 0000000..5f662c7 --- /dev/null +++ b/Limelight Pipelines/Pipeline_Name (6).vpr @@ -0,0 +1,58 @@ +area_max:25.843904016099994 +area_min:0.0244140625 +area_similarity:66 +aspect_max:20 +aspect_min:0.0000010124999999999998 +black_level:1 +blue_balance:1536 +calibration_type:0 +contour_grouping:0 +contour_sort_final:6 +convexity_max:64.5 +convexity_min:23.1 +corner_approx:2 +cross_a_a:1 +cross_a_x:0 +cross_a_y:0 +cross_b_a:1 +cross_b_x:0 +cross_b_y:0 +desc:Pipeline_Name +desired_contour_region:0 +dilation_steps:1 +direction_filter:0 +dual_close_sort_origin:0 +erosion_steps:0 +exposure:12 +force_convex:1 +hue_max:144 +hue_min:60 +image_flip:0 +image_source:0 +img_to_show:0 +intersection_filter:0 +pipeline_led_enabled:1 +pipeline_led_power:100 +pipeline_res:0 +pipeline_type:0 +red_balance:500 +roi_x:0 +roi_y:0 +sat_max:255 +sat_min:100 +send_corners:0 +send_raw_contours:0 +solve3d:0 +solve3d_algo:0 +solve3d_bindtarget:1 +solve3d_conf:0.990000 +solve3d_error:8 +solve3d_guess:0 +solve3d_iterations:50 +solve3d_precies:1 +solve3d_precise:0 +solve3d_zoffset:0 +val_max:254 +val_min:132 +y_max:1.000000 +y_min:-1.000000 diff --git a/Limelight Pipelines/Pipeline_Name (7).vpr b/Limelight Pipelines/Pipeline_Name (7).vpr new file mode 100644 index 0000000..72ff7fb --- /dev/null +++ b/Limelight Pipelines/Pipeline_Name (7).vpr @@ -0,0 +1,58 @@ +area_max:25.843904016099994 +area_min:0.0244140625 +area_similarity:83 +aspect_max:20 +aspect_min:2e-7 +black_level:3 +blue_balance:1536 +calibration_type:0 +contour_grouping:1 +contour_sort_final:6 +convexity_max:87 +convexity_min:41.9 +corner_approx:5.8 +cross_a_a:1 +cross_a_x:0 +cross_a_y:0 +cross_b_a:1 +cross_b_x:0 +cross_b_y:0 +desc:Pipeline_Name +desired_contour_region:1 +dilation_steps:1 +direction_filter:0 +dual_close_sort_origin:0 +erosion_steps:0 +exposure:24 +force_convex:1 +hue_max:65 +hue_min:55 +image_flip:0 +image_source:0 +img_to_show:0 +intersection_filter:0 +pipeline_led_enabled:1 +pipeline_led_power:100 +pipeline_res:0 +pipeline_type:0 +red_balance:500 +roi_x:0 +roi_y:0 +sat_max:255 +sat_min:240 +send_corners:0 +send_raw_contours:0 +solve3d:0 +solve3d_algo:0 +solve3d_bindtarget:1 +solve3d_conf:0.990000 +solve3d_error:8 +solve3d_guess:0 +solve3d_iterations:50 +solve3d_precies:1 +solve3d_precise:0 +solve3d_zoffset:0 +val_max:221 +val_min:191 +y_max:1.000000 +y_min:-1.000000 diff --git a/Limelight Pipelines/Pipeline_Name (8) - Copy - Copy - Copy.vpr b/Limelight Pipelines/Pipeline_Name (8) - Copy - Copy - Copy.vpr new file mode 100644 index 0000000..cc3521c --- /dev/null +++ b/Limelight Pipelines/Pipeline_Name (8) - Copy - Copy - Copy.vpr @@ -0,0 +1,58 @@ +area_max:100 +area_min:0.0244140625 +area_similarity:0 +aspect_max:20 +aspect_min:0.0000010124999999999998 +black_level:0 +blue_balance:1944 +calibration_type:0 +contour_grouping:0 +contour_sort_final:6 +convexity_max:43 +convexity_min:10 +corner_approx:2 +cross_a_a:1 +cross_a_x:0 +cross_a_y:0 +cross_b_a:1 +cross_b_x:0 +cross_b_y:0 +desc:Pipeline_Name +desired_contour_region:0 +dilation_steps:1 +direction_filter:0 +dual_close_sort_origin:0 +erosion_steps:0 +exposure:2 +force_convex:1 +hue_max:147 +hue_min:60 +image_flip:0 +image_source:0 +img_to_show:0 +intersection_filter:0 +pipeline_led_enabled:1 +pipeline_led_power:100 +pipeline_res:0 +pipeline_type:0 +red_balance:1130 +roi_x:0 +roi_y:0 +sat_max:255 +sat_min:95 +send_corners:0 +send_raw_contours:0 +solve3d:0 +solve3d_algo:0 +solve3d_bindtarget:1 +solve3d_conf:0.990000 +solve3d_error:8 +solve3d_guess:0 +solve3d_iterations:50 +solve3d_precies:1 +solve3d_precise:0 +solve3d_zoffset:0 +val_max:254 +val_min:163 +y_max:1.000000 +y_min:-1.000000 \ No newline at end of file diff --git a/Limelight Pipelines/Pipeline_Name (8) - Copy - Copy.vpr b/Limelight Pipelines/Pipeline_Name (8) - Copy - Copy.vpr new file mode 100644 index 0000000..262f302 --- /dev/null +++ b/Limelight Pipelines/Pipeline_Name (8) - Copy - Copy.vpr @@ -0,0 +1,56 @@ +area_max:100 +area_min:0.07233948159999996 +area_similarity:10 +aspect_max:15.285387012499996 +aspect_min:0 +black_level:0 +blue_balance:1975 +calibration_type:0 +contour_grouping:0 +contour_sort_final:0 +convexity_max:60 +convexity_min:0 +corner_approx:2 +cross_a_a:1 +cross_a_x:0 +cross_a_y:0 +cross_b_a:1 +cross_b_x:0 +cross_b_y:0 +desc:PowerPortCalibrated +desired_contour_region:0 +dilation_steps:0 +direction_filter:0 +dual_close_sort_origin:0 +erosion_steps:0 +exposure:2 +force_convex:1 +hue_max:98 +hue_min:59 +image_flip:0 +image_source:0 +img_to_show:1 +intersection_filter:0 +pipeline_led_enabled:1 +pipeline_led_power:100 +pipeline_res:0 +pipeline_type:0 +red_balance:1200 +sat_max:255 +sat_min:94 +send_corners:0 +send_raw_contours:0 +solve3d:0 +solve3d_algo:0 +solve3d_bindtarget:1 +solve3d_conf:0.990000 +solve3d_error:8 +solve3d_guess:0 +solve3d_iterations:50 +solve3d_precies:1 +solve3d_precise:0 +solve3d_zoffset:0 +val_max:255 +val_min:90 +y_max:1.000000 +y_min:-1.000000 \ No newline at end of file diff --git a/Limelight Pipelines/Pipeline_Name (8) - Copy.vpr b/Limelight Pipelines/Pipeline_Name (8) - Copy.vpr new file mode 100644 index 0000000..6685e36 --- /dev/null +++ b/Limelight Pipelines/Pipeline_Name (8) - Copy.vpr @@ -0,0 +1,58 @@ +area_max:100 +area_min:0.0163047361 +area_similarity:0 +aspect_max:20.000000 +aspect_min:0.000000 +black_level:17 +blue_balance:1526 +calibration_type:0 +contour_grouping:0 +contour_sort_final:0 +convexity_max:100 +convexity_min:10 +corner_approx:2 +cross_a_a:1 +cross_a_x:-0.07 +cross_a_y:0 +cross_b_a:1 +cross_b_x:0 +cross_b_y:0 +desc:Pipeline_Name +desired_contour_region:1 +dilation_steps:1 +direction_filter:0 +dual_close_sort_origin:0 +erosion_steps:0 +exposure:2 +force_convex:1 +hue_max:83 +hue_min:38 +image_flip:0 +image_source:0 +img_to_show:0 +intersection_filter:0 +pipeline_led_enabled:1 +pipeline_led_power:100 +pipeline_res:0 +pipeline_type:0 +red_balance:1323 +roi_x:0 +roi_y:0 +sat_max:255 +sat_min:124 +send_corners:0 +send_raw_contours:0 +solve3d:0 +solve3d_algo:0 +solve3d_bindtarget:1 +solve3d_conf:0.990000 +solve3d_error:8 +solve3d_guess:0 +solve3d_iterations:50 +solve3d_precies:1 +solve3d_precise:0 +solve3d_zoffset:0 +val_max:250 +val_min:114 +y_max:1.000000 +y_min:-1.000000 \ No newline at end of file diff --git a/Limelight Pipelines/Pipeline_Name (8).vpr b/Limelight Pipelines/Pipeline_Name (8).vpr new file mode 100644 index 0000000..06ebbd1 --- /dev/null +++ b/Limelight Pipelines/Pipeline_Name (8).vpr @@ -0,0 +1,58 @@ +area_max:25.843904016099994 +area_min:0.0244140625 +area_similarity:83 +aspect_max:20 +aspect_min:2e-7 +black_level:3 +blue_balance:1536 +calibration_type:0 +contour_grouping:1 +contour_sort_final:6 +convexity_max:87 +convexity_min:41.9 +corner_approx:5.8 +cross_a_a:1 +cross_a_x:0 +cross_a_y:0 +cross_b_a:1 +cross_b_x:0 +cross_b_y:0 +desc:Pipeline_Name +desired_contour_region:1 +dilation_steps:1 +direction_filter:0 +dual_close_sort_origin:0 +erosion_steps:0 +exposure:24 +force_convex:1 +hue_max:61 +hue_min:41 +image_flip:0 +image_source:0 +img_to_show:0 +intersection_filter:0 +pipeline_led_enabled:1 +pipeline_led_power:100 +pipeline_res:0 +pipeline_type:0 +red_balance:500 +roi_x:0 +roi_y:0 +sat_max:255 +sat_min:240 +send_corners:0 +send_raw_contours:0 +solve3d:0 +solve3d_algo:0 +solve3d_bindtarget:1 +solve3d_conf:0.990000 +solve3d_error:8 +solve3d_guess:0 +solve3d_iterations:50 +solve3d_precies:1 +solve3d_precise:0 +solve3d_zoffset:0 +val_max:221 +val_min:191 +y_max:1.000000 +y_min:-1.000000 diff --git a/Limelight Pipelines/Pipeline_Name (9).vpr b/Limelight Pipelines/Pipeline_Name (9).vpr new file mode 100644 index 0000000..f990528 --- /dev/null +++ b/Limelight Pipelines/Pipeline_Name (9).vpr @@ -0,0 +1,58 @@ +area_max:2.853430424100001 +area_min:0.005996953600000002 +area_similarity:0 +aspect_max:20 +aspect_min:0 +black_level:17 +blue_balance:1972 +calibration_type:0 +contour_grouping:0 +contour_sort_final:0 +convexity_max:100 +convexity_min:10 +corner_approx:2 +cross_a_a:1 +cross_a_x:-0.07 +cross_a_y:0 +cross_b_a:1 +cross_b_x:0 +cross_b_y:0 +desc:Pipeline_Name +desired_contour_region:0 +dilation_steps:1 +direction_filter:0 +dual_close_sort_origin:0 +erosion_steps:0 +exposure:33 +force_convex:1 +hue_max:65 +hue_min:55 +image_flip:0 +image_source:0 +img_to_show:0 +intersection_filter:0 +pipeline_led_enabled:1 +pipeline_led_power:100 +pipeline_res:0 +pipeline_type:0 +red_balance:1830 +roi_x:0 +roi_y:0 +sat_max:255 +sat_min:240 +send_corners:0 +send_raw_contours:0 +solve3d:0 +solve3d_algo:0 +solve3d_bindtarget:1 +solve3d_conf:0.990000 +solve3d_error:8 +solve3d_guess:0 +solve3d_iterations:50 +solve3d_precies:1 +solve3d_precise:0 +solve3d_zoffset:0 +val_max:58 +val_min:28 +y_max:1.000000 +y_min:-1.000000 diff --git a/Limelight Pipelines/Pipeline_Name.vpr b/Limelight Pipelines/Pipeline_Name.vpr new file mode 100644 index 0000000..c688629 --- /dev/null +++ b/Limelight Pipelines/Pipeline_Name.vpr @@ -0,0 +1,58 @@ +area_max:4.921342928100001 +area_min:0 +area_similarity:13 +aspect_max:20 +aspect_min:0 +black_level:0 +blue_balance:1876 +calibration_type:0 +contour_grouping:0 +contour_sort_final:2 +convexity_max:45.2 +convexity_min:0 +corner_approx:2 +cross_a_a:1 +cross_a_x:0 +cross_a_y:0 +cross_b_a:1 +cross_b_x:0 +cross_b_y:0 +desc:Pipeline_Name +desired_contour_region:0 +dilation_steps:1 +direction_filter:0 +dual_close_sort_origin:0 +erosion_steps:0 +exposure:13 +force_convex:1 +hue_max:148 +hue_min:61 +image_flip:0 +image_source:0 +img_to_show:1 +intersection_filter:0 +pipeline_led_enabled:1 +pipeline_led_power:100 +pipeline_res:0 +pipeline_type:0 +red_balance:927 +roi_x:0 +roi_y:0 +sat_max:254 +sat_min:114 +send_corners:0 +send_raw_contours:0 +solve3d:0 +solve3d_algo:0 +solve3d_bindtarget:1 +solve3d_conf:0.990000 +solve3d_error:8 +solve3d_guess:0 +solve3d_iterations:50 +solve3d_precies:1 +solve3d_precise:0 +solve3d_zoffset:0 +val_max:255 +val_min:113 +y_max:1.000000 +y_min:-1.000000 diff --git a/PathWeaver/Groups/Unnamed b/PathWeaver/Groups/Unnamed new file mode 100644 index 0000000..e69de29 diff --git a/PathWeaver/Paths/Unnamed.path b/PathWeaver/Paths/Unnamed.path new file mode 100644 index 0000000..610bd7c --- /dev/null +++ b/PathWeaver/Paths/Unnamed.path @@ -0,0 +1,3 @@ +X,Y,Tangent X,Tangent Y,Fixed Theta,Name +0.0,0.0,10.0,0.0,true, +10.0,-10.0,0.0,-10.0,true, diff --git a/PathWeaver/Paths/Unnamed_0.path b/PathWeaver/Paths/Unnamed_0.path new file mode 100644 index 0000000..610bd7c --- /dev/null +++ b/PathWeaver/Paths/Unnamed_0.path @@ -0,0 +1,3 @@ +X,Y,Tangent X,Tangent Y,Fixed Theta,Name +0.0,0.0,10.0,0.0,true, +10.0,-10.0,0.0,-10.0,true, diff --git a/PathWeaver/pathweaver.json b/PathWeaver/pathweaver.json new file mode 100644 index 0000000..b5ebd1b --- /dev/null +++ b/PathWeaver/pathweaver.json @@ -0,0 +1,8 @@ +{ + "lengthUnit": "Foot", + "maxVelocity": 10.0, + "maxAcceleration": 5.0, + "wheelBase": 4.0, + "gameName": "Destination: Deep Space", + "outputDir": "" +} \ No newline at end of file diff --git a/SmartDashboard Layout/save.xml b/SmartDashboard Layout/save.xml new file mode 100644 index 0000000..7f4e7a6 --- /dev/null +++ b/SmartDashboard Layout/save.xml @@ -0,0 +1,246 @@ + + + + + + + + + + + + 226 + 109 + + + + + + + + + + + + + 199 + 63 + + + + 162 + 67 + + + + + + + 354 + 317 + + + + + 325 + 125 + + + + + + + + + + 427 + 323 + + + + + + + + + + + + + + 187 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 20 + 341 + + + + 194 + 341 + + + + 20 + 341 + + + + 155 + 341 + + + + 23 + 122 + + + + 20 + 341 + + + + 23 + 122 + + + + 20 + 341 + + + + 23 + 132 + + + + 23 + 122 + + + + 23 + 122 + + + + 23 + 122 + + + + 20 + 341 + + + + 20 + 341 + + + + 20 + 341 + + + 353 + 649 + + + \ No newline at end of file diff --git a/build.gradle b/build.gradle index becc3d0..9eb9d8f 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2020.2.2" + id "edu.wpi.first.GradleRIO" version "2020.3.2" } sourceCompatibility = JavaVersion.VERSION_11 diff --git a/src/main/java/frc/robot/AutonomousMode/AUTOGetBallAndShoot.java b/src/main/java/frc/robot/AutonomousMode/AUTOGetBallAndShoot.java new file mode 100644 index 0000000..06c65e5 --- /dev/null +++ b/src/main/java/frc/robot/AutonomousMode/AUTOGetBallAndShoot.java @@ -0,0 +1,19 @@ +package frc.robot.AutonomousMode; + +import edu.wpi.first.wpilibj.command.CommandGroup; +import frc.robot.Autons.TrenchHoodAndFire; +import frc.robot.commands.AutoCommands.*; +import frc.robot.commands.DrivetrainAlignToGoal; +import frc.robot.commands.IntakeToHopper.*; + +public class AUTOGetBallAndShoot extends CommandGroup{ + public AUTOGetBallAndShoot(){ + addSequential(new MoveManipulator()); + addParallel(new FourBallTrench(), 5.0); + addParallel(new BeamBreak()); + addSequential(new StopManipulator()); + addSequential(new StopHopper()); + addSequential(new DrivetrainAlignToGoal()); + addSequential(new TrenchHoodAndFire()); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/AutonomousMode/tempTwoCycle.java b/src/main/java/frc/robot/AutonomousMode/tempTwoCycle.java new file mode 100644 index 0000000..f43e618 --- /dev/null +++ b/src/main/java/frc/robot/AutonomousMode/tempTwoCycle.java @@ -0,0 +1,21 @@ +package frc.robot.AutonomousMode; + +import edu.wpi.first.wpilibj.command.CommandGroup; +import frc.robot.Autons.TrenchHoodAndFire; +import frc.robot.commands.AutoCommands.*; +import frc.robot.commands.DrivetrainAlignToGoal; +import frc.robot.commands.IntakeToHopper.*; + +public class tempTwoCycle extends CommandGroup{ + public tempTwoCycle(){ + addSequential(new MoveManipulator()); + addParallel(new FourBallTrench(), 5.0); + addParallel(new BeamBreak()); + addSequential(new StopManipulator()); + addSequential(new StopHopper()); + addSequential(new SaveOffset()); + addSequential(new DrivetrainAlignToGoal()); + addSequential(new TrenchHoodAndFire()); + addSequential(new ReturnPosition()); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Autons/ManipulatorToIrHopper.java b/src/main/java/frc/robot/Autons/ManipulatorToIrHopper.java new file mode 100644 index 0000000..7c870e2 --- /dev/null +++ b/src/main/java/frc/robot/Autons/ManipulatorToIrHopper.java @@ -0,0 +1,13 @@ +package frc.robot.Autons; + +import edu.wpi.first.wpilibj.command.CommandGroup; +import frc.robot.commands.IntakeToHopper.*; +import frc.robot.commands.Shooter.OpenHopperPiston; + +public class ManipulatorToIrHopper extends CommandGroup{ + public ManipulatorToIrHopper(){ + addSequential(new MoveManipulator()); + addSequential(new IrHopper()); + //addSequential(new OpenHopperPiston()); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Autons/ManipulatorToLimitHopper.java b/src/main/java/frc/robot/Autons/ManipulatorToLimitHopper.java new file mode 100644 index 0000000..dc11541 --- /dev/null +++ b/src/main/java/frc/robot/Autons/ManipulatorToLimitHopper.java @@ -0,0 +1,13 @@ +package frc.robot.Autons; + +import edu.wpi.first.wpilibj.command.CommandGroup; +import frc.robot.commands.IntakeToHopper.*; +import frc.robot.commands.Shooter.OpenHopperPiston; + +public class ManipulatorToLimitHopper extends CommandGroup{ + public ManipulatorToLimitHopper(){ + addSequential(new MoveManipulator()); + addSequential(new BeamBreak()); + //addSequential(new OpenHopperPiston()); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Autons/StopShooterGroup.java b/src/main/java/frc/robot/Autons/StopShooterGroup.java new file mode 100644 index 0000000..a09e646 --- /dev/null +++ b/src/main/java/frc/robot/Autons/StopShooterGroup.java @@ -0,0 +1,17 @@ +package frc.robot.Autons; + +import edu.wpi.first.wpilibj.command.CommandGroup; +import frc.robot.commands.IntakeToHopper.StopHopper; +import frc.robot.commands.Shooter.CloseHopperPiston; +import frc.robot.commands.Shooter.StopShooter; +import frc.robot.commands.Shooter.WallShotHood; + +public class StopShooterGroup extends CommandGroup { + + public StopShooterGroup() { + addSequential(new WallShotHood()); + // addSequential(new StopShooter()); + addSequential(new CloseHopperPiston()); + addSequential(new StopHopper()); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Autons/TrenchHoodAndFire.java b/src/main/java/frc/robot/Autons/TrenchHoodAndFire.java new file mode 100644 index 0000000..aaf39b8 --- /dev/null +++ b/src/main/java/frc/robot/Autons/TrenchHoodAndFire.java @@ -0,0 +1,17 @@ +package frc.robot.Autons; + +import edu.wpi.first.wpilibj.command.CommandGroup; +import frc.robot.commands.IntakeToHopper.*; + +import frc.robot.commands.Shooter.LongShotHood; +import frc.robot.commands.Shooter.OpenHopperPiston; + +public class TrenchHoodAndFire extends CommandGroup{ + + public TrenchHoodAndFire(){ + addSequential(new LongShotHood(), 0.21); + addSequential(new OpenPistonsAndMoveHopper()); + // addSequential(new OpenHopperPiston()); + // addSequential(new MoveHopperLong()); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Autons/WallHoodAndFire.java b/src/main/java/frc/robot/Autons/WallHoodAndFire.java new file mode 100644 index 0000000..448c443 --- /dev/null +++ b/src/main/java/frc/robot/Autons/WallHoodAndFire.java @@ -0,0 +1,17 @@ +package frc.robot.Autons; + +import edu.wpi.first.wpilibj.command.CommandGroup; +import frc.robot.commands.IntakeToHopper.*; + +import frc.robot.commands.Shooter.WallShotHood; +import frc.robot.commands.Shooter.OpenHopperPiston; + +public class WallHoodAndFire extends CommandGroup{ + + public WallHoodAndFire(){ + addSequential(new WallShotHood(), 0.2); + addSequential(new OpenPistonsAndMoveHopper()); + // addSequential(new OpenHopperPiston()); + // addSequential(new MoveHopperLong()); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Autons/runDrivetrainShooter.java b/src/main/java/frc/robot/Autons/runDrivetrainShooter.java deleted file mode 100644 index 62fece3..0000000 --- a/src/main/java/frc/robot/Autons/runDrivetrainShooter.java +++ /dev/null @@ -1,38 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc.robot.Autons; - -import edu.wpi.first.wpilibj.command.CommandGroup; -import frc.robot.commands.MoveDrivetrain; -import frc.robot.commands.MoveShooter; - -public class runDrivetrainShooter extends CommandGroup { - /** - * Add your docs here. - */ - public runDrivetrainShooter() { - addParallel(new MoveDrivetrain()); - addParallel(new MoveShooter()); - // Add Commands here: - // e.g. addSequential(new Command1()); - // addSequential(new Command2()); - // these will run in order. - - // To run multiple commands at the same time, - // use addParallel() - // e.g. addParallel(new Command1()); - // addSequential(new Command2()); - // Command1 and Command2 will run in parallel. - - // A command group will require all of the subsystems that each member - // would require. - // e.g. if Command1 requires chassis, and Command2 requires arm, - // a CommandGroup containing them would require both the chassis and the - // arm. - } -} diff --git a/src/main/java/frc/robot/Autons/runElevatorShooter.java b/src/main/java/frc/robot/Autons/runElevatorShooter.java deleted file mode 100644 index a04732b..0000000 --- a/src/main/java/frc/robot/Autons/runElevatorShooter.java +++ /dev/null @@ -1,38 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc.robot.Autons; - -import edu.wpi.first.wpilibj.command.CommandGroup; -import frc.robot.commands.MoveElevator; -import frc.robot.commands.MoveShooter; - -public class runElevatorShooter extends CommandGroup { - /** - * Add your docs here. - */ - public runElevatorShooter() { - addParallel(new MoveElevator()); - addParallel(new MoveShooter()); - // Add Commands here: - // e.g. addSequential(new Command1()); - // addSequential(new Command2()); - // these will run in order. - - // To run multiple commands at the same time, - // use addParallel() - // e.g. addParallel(new Command1()); - // addSequential(new Command2()); - // Command1 and Command2 will run in parallel. - - // A command group will require all of the subsystems that each member - // would require. - // e.g. if Command1 requires chassis, and Command2 requires arm, - // a CommandGroup containing them would require both the chassis and the - // arm. - } -} diff --git a/src/main/java/frc/robot/Autons/runHopperElevator.java b/src/main/java/frc/robot/Autons/runHopperElevator.java deleted file mode 100644 index 7c96842..0000000 --- a/src/main/java/frc/robot/Autons/runHopperElevator.java +++ /dev/null @@ -1,39 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc.robot.Autons; - -import edu.wpi.first.wpilibj.command.CommandGroup; -import frc.robot.commands.MoveElevator; -import frc.robot.commands.MoveHopper; - -public class runHopperElevator extends CommandGroup { - /** - * Add your docs here. - */ - public runHopperElevator() { - - addParallel(new MoveHopper()); - addParallel(new MoveElevator()); - // Add Commands here: - // e.g. addSequential(new Command1()); - // addSequential(new Command2()); - // these will run in order. - - // To run multiple commands at the same time, - // use addParallel() - // e.g. addParallel(new Command1()); - // addSequential(new Command2()); - // Command1 and Command2 will run in parallel. - - // A command group will require all of the subsystems that each member - // would require. - // e.g. if Command1 requires chassis, and Command2 requires arm, - // a CommandGroup containing them would require both the chassis and the - // arm. - } -} diff --git a/src/main/java/frc/robot/Autons/stopDrivetrainShooter.java b/src/main/java/frc/robot/Autons/stopDrivetrainShooter.java deleted file mode 100644 index e1f6e08..0000000 --- a/src/main/java/frc/robot/Autons/stopDrivetrainShooter.java +++ /dev/null @@ -1,33 +0,0 @@ -package frc.robot.Autons; - -import edu.wpi.first.wpilibj.command.CommandGroup; -import frc.robot.commands.MoveDrivetrain; -import frc.robot.commands.MoveShooter; -import frc.robot.commands.StopDrivetrain; -import frc.robot.commands.StopShooter; - -public class stopDrivetrainShooter extends CommandGroup { - /** - * Add your docs here. - */ - public stopDrivetrainShooter() { - addParallel(new StopDrivetrain()); - addParallel(new StopShooter()); - // Add Commands here: - // e.g. addSequential(new Command1()); - // addSequential(new Command2()); - // these will run in order. - - // To run multiple commands at the same time, - // use addParallel() - // e.g. addParallel(new Command1()); - // addSequential(new Command2()); - // Command1 and Command2 will run in parallel. - - // A command group will require all of the subsystems that each member - // would require. - // e.g. if Command1 requires chassis, and Command2 requires arm, - // a CommandGroup containing them would require both the chassis and the - // arm. - } -} diff --git a/src/main/java/frc/robot/Autons/stopElevatorShooter.java b/src/main/java/frc/robot/Autons/stopElevatorShooter.java deleted file mode 100644 index a163344..0000000 --- a/src/main/java/frc/robot/Autons/stopElevatorShooter.java +++ /dev/null @@ -1,38 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc.robot.Autons; - -import edu.wpi.first.wpilibj.command.CommandGroup; -import frc.robot.commands.StopElevator; -import frc.robot.commands.StopShooter; - -public class stopElevatorShooter extends CommandGroup { - /** - * Add your docs here. - */ - public stopElevatorShooter() { - addParallel(new StopElevator()); - addParallel(new StopShooter()); - // Add Commands here: - // e.g. addSequential(new Command1()); - // addSequential(new Command2()); - // these will run in order. - - // To run multiple commands at the same time, - // use addParallel() - // e.g. addParallel(new Command1()); - // addSequential(new Command2()); - // Command1 and Command2 will run in parallel. - - // A command group will require all of the subsystems that each member - // would require. - // e.g. if Command1 requires chassis, and Command2 requires arm, - // a CommandGroup containing them would require both the chassis and the - // arm. - } -} diff --git a/src/main/java/frc/robot/Autons/stopHopperElevator.java b/src/main/java/frc/robot/Autons/stopHopperElevator.java deleted file mode 100644 index 3ff8a1e..0000000 --- a/src/main/java/frc/robot/Autons/stopHopperElevator.java +++ /dev/null @@ -1,38 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc.robot.Autons; - -import edu.wpi.first.wpilibj.command.CommandGroup; -import frc.robot.commands.StopElevator; -import frc.robot.commands.StopHopper; - -public class stopHopperElevator extends CommandGroup { - /** - * Add your docs here. - */ - public stopHopperElevator() { - addParallel(new StopHopper()); - addParallel(new StopElevator()); - // Add Commands here: - // e.g. addSequential(new Command1()); - // addSequential(new Command2()); - // these will run in order. - - // To run multiple commands at the same time, - // use addParallel() - // e.g. addParallel(new Command1()); - // addSequential(new Command2()); - // Command1 and Command2 will run in parallel. - - // A command group will require all of the subsystems that each member - // would require. - // e.g. if Command1 requires chassis, and Command2 requires arm, - // a CommandGroup containing them would require both the chassis and the - // arm. - } -} diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 0b9f650..1d1ecc3 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -8,6 +8,7 @@ public final class Constants { // Drivetrain constants public static final double kFastDrive = 1.0; public static final double kSlowDrive = 0.5; + public static final double kCreep = 0.2; public static final double kSlowish = 0.5; //Testing should be 0.5 public static final double kLineFollowStraight = 0.19; //0.27; @@ -21,28 +22,103 @@ public final class Constants { public static final double kCamStraightSuperFast = 0.75; public static final double kCamStraightMedium = 0.45; public static final double kCamStraightSlow = 0.25; + public static final double wheelDiameterInches = 5; + + public static final double ShooterTrenchkP = 0.002535; //.002535 good for comp //was .002525 + public static final double ShooterTrenchkD = 0.22; + public static final double ShooterTrenchkFF = 0.0002; + public static final double ShooterWallkP = 0.001500; + public static final double ShooterWallkD = 0.1; + public static final double ShooterWallkF = 0.000180; + public static final double ShooterkIz = 0; + public static final double ShooterMaxOutput = 1; + public static final double ShooterMinOutput = -1; // max and min outputs that pidcontroller can send to the sparkmax + public static final double kGravity = 32.1741; //acceleration due to gravity in ft/s/s + public static final double ShooterDiameter = 6; //inches + public static final double ShooterGearing = 2; //Shooter spins twice for every one time motor spins + public static final double kDrag = 1; + public static final double kMagnus = 1; + + public static final double jamCurrent = 20; + public static final double fixSpaceTime = 0.2; + + public static final double DrivekP = 0.000025; + public static final double DrivekI = 0; + public static final double DrivekD = 0; + public static final double DriveIz = 0; + public static final double DrivekFF = 0; + public static final double DriveMaxOutput = 1; + public static final double DriveMinOutput = -1; + public static final double DrivemaxRPM = 4500; + + public static final double LimelightkP = 0.025; + public static final double LimelightkI = 0.0; + public static final double LimelightkD = 0.0; + + public static final double kDistInnerOuter = 8; + + public static final double kIrSensorVal = 20.0; //what the value of the ir sensor should read w/o a ball + // public static final double kCamOffset = 0; //Joystick constant public static final double DEADZONE = 0.4; - public static final double ELEVATOR_OUTPUT = -.05; + public static final double ELEVATOR_OUTPUT = -.05; + + + public static final double HOPPER_LOADING_HORIZONTAL_OUTPUT = 0.82;//.82; //.5 for limit + + public static final double HOPPER_WALL_HORIZONTAL_OUTPUT = 0.60; // 0.30; 0.70; + //FIX 0.86 - public static final double HOPPER_OUTPUT = -.20; // 20% is sweet spot + + public static final double HOPPER_VERTICAL_OUTPUT = 1.0; // .35; 0.80;[\] + + + public static final double HOPPER_LOADING_VERTICAL_OUTPUT = .15; //0.15 for limit + + + public static final double REVERSE_HOPPER = -.40; public static final double SHOOTER_OUTPUT = -.8; - public static final double DRIVETRAIN_OUTPUT = -.5; + public static final double DRIVETRAIN_OUTPUT = -1.0; + public static final double SHOOTER_OUTPUT_PASSIVE = -.1; - public static final double MANUAL_POWER = .2; + + public static final double MANUAL_POWER = .2; + + + public static final double INTAKE_MOTORSPEED = 0.18; // .135 old speed: Temp value please test it out and do stuff yes + + + public static final double SHOOTER_VELOCITY = 100; //Temporary value + + + public static final double SHOOTER_OUTPUT_LONG = -1; + + + public static final double SHOOTER_OUTPUT_WALL = -.4; //changed from .47 //definitely make negative though + public static final double SHOOTER_OUTPUT_WALL_RPM = -3500; + + public static final double TURN_FACTOR = 0.7; + + + public static final int BALL_VALUE = 2350; + + + //TODO: MAKE CONSTANTS FOR SHOOTER PID SPEEDS FOR WALL AND + //TODO: LONG SHOT SO THEY ARE NOT IN THE COMMANDS AND ARE EASIER + //TODO: CHANGED FOR LATER USE AND REPURPOSE!!!!!!!!!!! //climber public static final double CLIMBER_CREEP = 0.25; @@ -54,6 +130,35 @@ public final class Constants { //Panel Mech public static final double PANEL_MECH_CREEP = 0.1; - public static final double PANEL_MECH_FAST = 0.5; - + + + public static final double kSelfClimbGoRight = 0; //change + + + public static final double kSelfClimbGoLeft = 0; //change + public static final double PANEL_MECH_FAST = 0.45; + + + public static final double HOPPER_LONG_HORIZONTAL_OUTPUT = 0.25; + public static final double HOPPER_AUTO_HORIZONTAL = 0.4; + public static final double HOPPER_AUTO_VERTICAL = 0.9; + + + public static final double kHopperTimer = 0.06;//0.037;//0.12; + + + public static final double LimelightSetpoint = -1; + + + public static final double TrenchBallOneDist = 7; //was 7 + + + public static final double kHopperTimerBeams = 0.02; + + + public static double reverseTime = 1.0; + + + public static double tempOffset; + } \ No newline at end of file diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java index 90c8903..76dc7b8 100644 --- a/src/main/java/frc/robot/OI.java +++ b/src/main/java/frc/robot/OI.java @@ -3,120 +3,173 @@ import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.buttons.Button; import edu.wpi.first.wpilibj.buttons.JoystickButton; -import frc.robot.Autons.RunElevatorWithJoystick; -import frc.robot.Autons.RunHopperWithJoystick; -import frc.robot.Autons.runDrivetrainShooter; -import frc.robot.Autons.runElevatorShooter; -import frc.robot.Autons.runHopperElevator; -import frc.robot.Autons.stopDrivetrainShooter; -import frc.robot.Autons.stopElevatorShooter; -import frc.robot.Autons.stopHopperElevator; +import edu.wpi.first.wpilibj.buttons.Trigger; +import edu.wpi.first.wpilibj.command.InstantCommand; +import frc.robot.Autons.ManipulatorToIrHopper; +import frc.robot.Autons.ManipulatorToLimitHopper; +import frc.robot.Autons.StopShooterGroup; +import frc.robot.Autons.TrenchHoodAndFire; +import frc.robot.Autons.WallHoodAndFire; +import frc.robot.commands.Shooter.*; +import frc.robot.commands.Climb.*; +import frc.robot.commands.IntakeToHopper.*; +import frc.robot.commands.PanelMech.*; + import frc.robot.commands.*; +import frc.robot.commands.Shooter.StopShooter; +import frc.robot.commands.tempTest.*; import frc.robot.commands.Climb.*; -// import frc.robot.commands.auto.SetLimit; import frc.robot.controller.AnalogButton; import frc.robot.controller.DPadButton; import frc.robot.controller.MultiButton; -// import frc.robot.commands.Lifter.ExtendBothLifters; +import frc.robot.util.Camera_Switch.CameraSwitch; import frc.robot.util.Util; -// import frc.robot.commands.CargoManipulator.ScoreInRocketCalculated; -// import frc.robot.commands.CargoManipulator.ScoreInRocketDropper; -// import frc.robot.commands.auto.AutoSetLifterPots; - // _____ - // | | - // | | | | - // |_____| - // ____ ___|_|___ ____ - // ()___) ()___) - // // /| |\ \\ - // // / | | \ \\ - // (___) |___________| (___) - // (___) (_______) (___) - // (___) (___) (___) - // (___) |_| (___) - // (___) ___/___\___ | | - // | | | | | | - // | | |___________| /___\ - // /___\ ||| ||| // \\ - // // \\ ||| ||| \\ // - // \\ // ||| ||| \\ // - // \\ // ()__) (__() - // /// \\\ - // /// \\\ - // _///___ ___\\\_ - // |_______| |_______| public class OI { public static Joystick driverStick; public static Joystick operatorStick; + public static Joystick pitStick; private Button driveButton; private Button driverLB, driverRB; private Button driverStart, driverBack; private Button operatorStart; private Button driverA, driverB, driverY; - private Button driverDPadDown, driverDPadRight, driverDPadLeft; + private Button driverDPadDown, driverDPadRight, driverDPadLeft, driverDPadUp; private Button operatorRB, operatorLT, operatorLB, operatorRT; public Button operatorLS, operatorBack; private Button driverX; private Button driverRS, driverLS; private Button driverRX; - protected Button operatorLeftJoystickUsed, operatorRightJoystickUsed, operatorDPadDown, operatorDPadLeft; + protected Button operatorLeftJoystickUsed, operatorRightJoystickUsed, operatorDPadDown, operatorDPadLeft, operatorDPadRight; private Button operatorA, operatorB, operatorX, operatorY; + private Button pitA, pitB, pitX, pitY; + + public OI() { + + driverStick = new Joystick(0); operatorStick = new Joystick(1); + pitStick = new Joystick(3); + initButtons(); initUsed(); - // driveButton.whileHeld(new DriveWithJoystick(driverStick, 0.1)); //this should be the default command of the DT - //LEAVE OUT driverStart.whileHeld(new ExtendBothLifters(.8,false,driverStick)); - - //THESE TWO LINES ARE FOR TESTING - //LEAVE OUT driverA.whenPressed(new AutoSetLifterPots()); - //LEAVE OUT driverB.whenPressed(new ExtendBothLifters(.8,false,driverStick,false)); - // driverA.whenPressed(new MoveShooter()); - // driverA.whenReleased(new StopShooter()); - // driverB.whenPressed(new MoveElevator()); - // driverB.whenReleased(new StopElevator()); - driverA.whenPressed(new ExtendPanelMech()); - driverB.whenPressed(new SetPanelMech()); - driverA.whenReleased(new DefaultPanelMech()); - - // driverX.whenPressed(new MoveHopper()); - driverX.whenReleased(new StopHopper()); - driverY.whenPressed(new MoveDrivetrain()); - driverY.whenReleased(new StopDrivetrain()); - driverLB.whenPressed(new runHopperElevator()); - driverLB.whenReleased(new stopHopperElevator()); - driverRB.whenPressed(new runDrivetrainShooter()); - driverRB.whenReleased(new stopDrivetrainShooter()); - driverLS.whenPressed(new runElevatorShooter()); - driverLS.whenReleased(new stopElevatorShooter()); - operatorLeftJoystickUsed.whenPressed(new RunHopperWithJoystick(operatorLeftJoystickUsed)); - + driveButton.whileHeld(new DriveWithJoystick(driverStick, 0.1)); // TODO CHANGE DEADZONE VALUE IT MIGHT NOT BE THE SAME + + operatorDPadLeft.whenPressed(new LongShotHood()); + operatorDPadRight.whenPressed(new WallShotHood()); + + new Trigger(){ + @Override + public boolean get() { + // TODO Auto-generated method stub + return operatorStick.getRawAxis(1) > 0.8; + } + }.whenActive(new ReverseHopper()); //or try ReverseUntilBeamBreak + + new Trigger(){ + @Override + public boolean get() { + // TODO Auto-generated method stub + return operatorStick.getRawAxis(1) > 0.8; + } + }.whenInactive(new StopHopper()); + + // operatorA.whenPressed(new InstantCommand(() -> { //PLEASE CHANGE THIS BUTTON BC IT IS THE SAME AS PANEL MECH STUFF!!! + // Robot.hopper.hopper_stopper.set(true); + // })) ; + + operatorB.whileHeld(new SetPanelMechSlow()); + // operatorB.whenPressed(new ExtendPanelMech()); + operatorB.whenReleased(new DefaultPanelMech()); + operatorB.whenReleased(new StopPanelMech()); + + operatorA.whileHeld(new SetPanelMechFast()); + // operatorA.whenPressed(new ExtendPanelMech()); + operatorA.whenReleased(new StopPanelMech()); + operatorA.whenReleased(new DefaultPanelMech()); + + operatorY.whileHeld(new MoveHopperWall());//moves hopper at constant speeds + operatorY.whenReleased(new StopHopper()); + + operatorX.whileHeld(new BeamBreak()); + operatorX.whenPressed(new InstantCommand(() -> { + Robot.hopper.setRetracted();; + + })); + operatorX.whenReleased(new StopHopper()); + + operatorRB.whenPressed(new WallShot()); + // // operatorRB.whenPressed(new InstantCommand(() -> { + // Robot.Cam_switch.select(CameraSwitch.kcamera1);; + // })); + + operatorLB.whenPressed(new LongShot()); + + new Trigger(){ + @Override + public boolean get() { + // TODO Auto-generated method stub + return operatorStick.getRawAxis(3) > 0.25; + } + }.whenActive(new WallHoodAndFire()); + + new Trigger(){ + @Override + public boolean get() { + // TODO Auto-generated method stub + return operatorStick.getRawAxis(3) > 0.25; + } + }.whenInactive(new StopShooterGroup()); + + new Trigger(){ + + @Override + public boolean get() { + // TODO Auto-generated method stub + return operatorStick.getRawAxis(2) > 0.25; + } + }.whenActive(new StopShooter()); + + driverA.whenPressed(new ManipulatorToLimitHopper()); + // driverA.whileHeld(new ManipulatorToLimitHopper()); + + driverA.whenPressed(new InstantCommand(() -> { + Robot.Cam_switch.select(CameraSwitch.kcamera2);; + })); + driverA.whenPressed(new InstantCommand(() -> { + Robot.hopper.setRetracted();; + + })); + driverA.whenReleased(new StopManipulator()); + driverA.whenReleased(new StopHopper()); - operatorX.whenPressed(new ReleasePin()); - operatorY.whenPressed(new WinchClimb()); - operatorY.whenReleased(new StopWinchClimb()); - // operatorA.whenPressed(new ActivePosition()); - + driverDPadRight.whileHeld(new DrivetrainAlignToGoal()); + driverDPadDown.whenPressed(new InstantCommand(() -> { + Robot.climber.setRetracted();; + })); + driverStart.whenPressed(new ReleasePin(true, true)); + + driverX.whileHeld(new WinchClimb(true)); + driverX.whenReleased(new StopWinchClimb()); + driverY.whileHeld(new WinchClimb(false)); + driverY.whenReleased(new StopWinchClimb()); - // //true does right hp far rocket path, false does right hp bay 1 ship path - // // driverY.whenReleased(new StopCargoMotor()); - // driverRB.whileHeld(new DriveWithJoystickLeftTalon()); - // driverLB.whileHeld(new DriveWithJoystickRightTalon()); - // // driverY.whileHeld(new DriveWithJoystickLeftTalon()); - // //driverX.whenPressed(new DriveWithJoystickLeft(driverStick, 0.1)); - // driverB.whileHeld(new DriveWithJoystickRight()); - // // driverX.whenPressed(new DriveWithJoystickLeft()); - // driverX.whenReleased(new StopDrive()); - // // driverRX.whileHeld(new PreciseTurnJoystick(driverStick, 0.1)); + driverB.whileHeld(new TrenchHoodAndFire()); + driverB.whenReleased(new StopShooterGroup()); + //Pit Controller commands to reset the climb + pitA.whenPressed(new testManipulator()); //Extends Manipulator + pitB.whenPressed(new testStopManipulator()); //Retracts Manipulator + pitX.whenPressed(new testExtendClimber()); //Extends Climber + pitY.whenPressed(new testRetractClimber()); //Retracts Climber + } private void initButtons(){ @@ -135,7 +188,7 @@ private void initButtons(){ driverRX = new AnalogButton(driverStick, 4); driverDPadDown = new DPadButton(driverStick, DPadButton.kDPadDown); driverDPadRight = new DPadButton(driverStick, DPadButton.kDPadRight); - // driverDPadUp = new DPadButton(driverStick, DPadButton.kDPadUp); + driverDPadUp = new DPadButton(driverStick, DPadButton.kDPadUp); driverDPadLeft = new DPadButton(driverStick, DPadButton.kDPadLeft); driveButton = new MultiButton(new Button[] { new AnalogButton(driverStick, 3, 2, 0, 0.2), @@ -157,12 +210,18 @@ private void initButtons(){ operatorLS = new AnalogButton(operatorStick, 1); operatorDPadDown = new DPadButton(operatorStick, DPadButton.kDPadDown); operatorDPadLeft = new DPadButton(operatorStick, DPadButton.kDPadLeft); + operatorDPadRight = new DPadButton(operatorStick, DPadButton.kDPadRight); } catch (Exception error){ System.out.println("Error Init With Buttons"); error.printStackTrace(); } +//TEMPORARY PIT CONTROLS// + pitA = new JoystickButton(pitStick, 1); + pitB = new JoystickButton(pitStick, 2); + pitX = new JoystickButton(pitStick, 3); + pitY = new JoystickButton(pitStick, 4); } private void initUsed(){ @@ -173,4 +232,4 @@ public boolean get() { } }; } -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index f01bc05..35a18ba 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -2,19 +2,28 @@ import edu.wpi.first.wpilibj.PowerDistributionPanel; import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.buttons.Button; import edu.wpi.first.wpilibj.command.Scheduler; -import frc.robot.subsystems.Drivetrain; -import frc.robot.subsystems.Elevator; -import frc.robot.subsystems.Hopper; -import frc.robot.subsystems.Shooter; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.controller.DPadButton; +import frc.robot.subsystems.*; +import frc.robot.util.Camera_Switch.CameraSwitch; import edu.wpi.first.wpilibj.Compressor; +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.cameraserver.CameraServer; import frc.robot.subsystems.Climber; -import frc.robot.subsystems.*; + + public class Robot extends TimedRobot { - long loopCounter = 0; + long loopCounter = 0; + + + + + public static Joystick pitStick; public static OI oi; public static Drivetrain drivetrain; public static PowerDistributionPanel pdp; @@ -22,68 +31,150 @@ public class Robot extends TimedRobot { public static Shooter shooter; public static Elevator elevator; public static Hopper hopper; + public static Limelight limelight; public static RobotMap robotmap; - public static Climber climber; + + public static Manipulator manipulator; public static PanelMech panelMech; + public static CameraServer Cam; + public static CameraSwitch Cam_switch; + private Button pitDPadDown, pitDPadRight, pitDPadLeft, pitDPadUp; + + public static Climber climber; + @Override public void robotInit() { hopper = new Hopper(); - elevator = new Elevator(); shooter = new Shooter(); drivetrain = new Drivetrain(); + manipulator = new Manipulator(); + panelMech = new PanelMech(); + limelight = new Limelight(); + climber = new Climber(); pdp = new PowerDistributionPanel(RobotMap.kPDP); oi = new OI(); robotmap = new RobotMap(); - climber = new Climber(); + + Cam_switch = new CameraSwitch(0,1); + Cam = CameraServer.getInstance(); + compressor = new Compressor(RobotMap.kPCM); + compressor.start(); + Cam.startAutomaticCapture(0); + pitStick = new Joystick(3); + pitDPadDown = new DPadButton(pitStick, DPadButton.kDPadDown); + pitDPadLeft = new DPadButton(pitStick, DPadButton.kDPadLeft); + pitDPadUp = new DPadButton(pitStick, DPadButton.kDPadUp); + pitDPadRight = new DPadButton(pitStick, DPadButton.kDPadRight); + + SmartDashboard.putBoolean("Drivetrain Align Complete", false); } - - @Override + + // @Override public void robotPeriodic() { // //EACH debug only runs once per 10 loops + // DriveWithJoystick joystick_command = new DriveWithJoystick(OI.driverStick, 0.1) drivetrain.debug(); + compressor.start(); + Cam_switch.debug(); + SmartDashboard.putBoolean("Compressor Status", compressor.enabled()); hopper.debug(); - + climber.debug(); + shooter.debug(); + manipulator.debug(); + + if(pitDPadUp.get() == true) { + Cam_switch.select(CameraSwitch.kcamera1); + } + else if(pitDPadRight.get() == true) { + Cam_switch.select(CameraSwitch.kcamera2); + } + else if(pitDPadDown.get() == true){ + Cam_switch.select(CameraSwitch.kcamera3); + } + else if(pitDPadLeft.get()) { + Cam_switch.select(CameraSwitch.kcamera4); + } } @Override public void disabledInit() { - drivetrain.setMotors(0); + drivetrain.setMotors(0,0); + hopper.setHopper(0,0); + manipulator.setManipulator(0); + manipulator.setRetracted(); + shooter.setShooter(0); + //add emergency stops for panelMech } @Override public void disabledPeriodic() { - drivetrain.setMotors(0); - // Robot.drivetrain.resetGyro(); + drivetrain.setMotors(0,0); + hopper.setHopper(0,0); + manipulator.setManipulator(0); + manipulator.setRetracted(); + shooter.setShooter(0); + //add emergency stops for panelMech + // autoCommand.start(); } + @Override + public void autonomousInit(){ + drivetrain.driveTime.start(); + + //LOAD COMMAND BASED AUTO HERE + + /*******************************/ + } + @Override public void autonomousPeriodic() { Scheduler.getInstance().run(); + + //LOAD ITERATIVE BASED AUTO HERE + drivetrain.BackupAndShoot(); + /*******************************/ + } @Override public void teleopInit() { // autoCommand.cancel(); + compressor.start(); + drivetrain.driveTime.stop(); System.out.println("This is init"); + SmartDashboard.putNumber("Shooter kP", Constants.ShooterTrenchkP); + SmartDashboard.putNumber("Shooter kF", Constants.ShooterTrenchkFF); + SmartDashboard.putNumber("Shooter kD", Constants.ShooterTrenchkD); + SmartDashboard.putNumber("Shooter setpoint", 0); + + manipulator.setRetracted(); + Robot.hopper.hopper_stopper.set(true); + Robot.shooter.setHood1(true); } + @Override public void teleopPeriodic() { - // Robot.elevator.setElevator(-.1); - // Robot.hopper.setHopper(-1); - // Robot.shooter.setShooter(-1); - // Robot.drivetrain.setMotors(-1); - - // Robot.drivetrain.setMotors(-.0000001); - // Robot.drivetrain.setRightTalon(-1); - // Robot.drivetrain.setLeftNeo(-1); - // Robot.drivetrain.setLeftTalon(.7); - // Robot.drivetrain.setRightNeo(1); - // 0.5 power is the sweet spot for wall, 0.8 for current at angle of 39 degrees + Scheduler.getInstance().run(); + + + double kp = SmartDashboard.getNumber("Shooter kP", Constants.DrivekP); + double kf = SmartDashboard.getNumber("Shooter kF", Constants.DrivekFF); + double kd = SmartDashboard.getNumber("Shooter kD", Constants.DrivekD); + + shooter.shooter_leader.getPIDController().setP(kp); + shooter.shooter_leader.getPIDController().setFF(kf); + shooter.shooter_leader.getPIDController().setD(kd); + + double setpoint = SmartDashboard.getNumber("Shooter setpoint", 0); + if (setpoint==0) + shooter.setShooter(0); + else + shooter.setShooterPID(setpoint); } @Override public void testPeriodic() { diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java index 927cf09..bd8bb7f 100644 --- a/src/main/java/frc/robot/RobotMap.java +++ b/src/main/java/frc/robot/RobotMap.java @@ -7,7 +7,7 @@ package frc.robot; -import edu.wpi.first.wpilibj.DigitalOutput; + /** * The RobotMap is a mapping from the ports sensors and actuators are wired into @@ -22,16 +22,29 @@ public class RobotMap { public static final int kPDP = 0; //Drivetrain - public static final int kLeftLeader = 1; - public static final int kRightLeader = 3; - public static final int[] kLeftFollowers = {24}; - public static final int[] kRightFollowers = {23}; + public static final int kLeftLeader = 20; + public static final int kRightLeader = 22; + public static final int leftFollower = 21; + public static final int rightFollower = 23; + public static final int[] kLeftFollowers = {21};// check if these are the right ids + public static final int[] kRightFollowers = {23}; // check if these are the right ids // public static final int[] kLineSensors = {7, 6, 5, 4}; public static final int[] kLineSensors = {4,5,6,7}; public static final int kLEDBlueSolenoid = 7; public static final int kLEDGreenSolenoid = 6; + public static final int kShooterLeft = 31; + public static final int kShooterRight = 32; + + public static final int kIntake = 40; + + public static final int kVertHopper = 41; + public static final int kHoriHopper = 42; + + + public static final int kWinch = 60; + public static final int kActivePos = 61; //Lifters public static final int kFrontLiftTalon = 23; public static final int kBackLiftTalon = 22; @@ -66,15 +79,21 @@ public class RobotMap { public static final int kCargoLimitSwitch = 0; // Shoooter - public static final int SHOOTER_NEO = 2; + public static final int SHOOTER_NEO = 2; //what is this for??? // Elevator public static final int ELEVATOR_TALON = 23; //Hopper - - public static final int HOPPER_TALON = 24; + // public static final int HOPPER_TALON = 40; //Drivetrain +public static final int HOOD_SOLENOID = 3; +public static final int HOPPER_SOLENOID = 1; +public static final int VERTICAL_HOPPER = 41; +public static final int HORIZONTAL_HOPPER = 42; +public static final int MANIPULATOR_SOLENOID = 6; +public static final int MANIPULATOR_NEO = 40; + public static final int DRIVETRAIN_NEO = 22; //Climber @@ -87,4 +106,5 @@ public class RobotMap { //PanelMech public static final int PANEL_NEO=50; public static final int PANEL_SOLENOID=2; + } diff --git a/src/main/java/frc/robot/commands/AutoCommands/FourBallTrench.java b/src/main/java/frc/robot/commands/AutoCommands/FourBallTrench.java new file mode 100644 index 0000000..d4f302e --- /dev/null +++ b/src/main/java/frc/robot/commands/AutoCommands/FourBallTrench.java @@ -0,0 +1,59 @@ +package frc.robot.commands.AutoCommands; + +import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.controller.PIDController; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.Constants; +import frc.robot.Robot; + + +public class FourBallTrench extends Command { + + PIDController pid; + double setpoint; + + public FourBallTrench() { + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + requires(Robot.drivetrain); + pid = new PIDController(Constants.DrivekP, Constants.DrivekI, Constants.DrivekD); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + setpoint = Constants.TrenchBallOneDist; + setpoint += Robot.drivetrain.getAverageDistance(); + pid.setSetpoint(setpoint); + pid.setTolerance(0.3); + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + double output = pid.calculate(Robot.drivetrain.getAverageDistance()); + Robot.drivetrain.setMotors(output, output); + SmartDashboard.putBoolean("Drivetrain Distance Complete", false); + SmartDashboard.putNumber("Drivetrain Auto Output", output); + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + return pid.atSetpoint(); + } + + // Called once after isFinished returns true + @Override + protected void end() { + // Robot.limelight.setCamMode(CamMode.DRIVER_CAM); + SmartDashboard.putBoolean("Drivetrain Distance Complete", true); + Robot.drivetrain.setMotors(0, 0); + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + } +} diff --git a/src/main/java/frc/robot/Autons/RunHopperWithJoystick.java b/src/main/java/frc/robot/commands/AutoCommands/ReturnPosition.java similarity index 51% rename from src/main/java/frc/robot/Autons/RunHopperWithJoystick.java rename to src/main/java/frc/robot/commands/AutoCommands/ReturnPosition.java index 2b6f72f..d281d4e 100644 --- a/src/main/java/frc/robot/Autons/RunHopperWithJoystick.java +++ b/src/main/java/frc/robot/commands/AutoCommands/ReturnPosition.java @@ -5,54 +5,58 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc.robot.Autons; +package frc.robot.commands.AutoCommands; -import edu.wpi.first.wpilibj.buttons.Button; import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.controller.PIDController; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.Constants; -import frc.robot.OI; import frc.robot.Robot; -import frc.robot.util.Util; +import frc.robot.subsystems.Limelight.CamMode; +import frc.robot.subsystems.Limelight.LedMode; -public class RunHopperWithJoystick extends Command { - Button hopper_button; - public RunHopperWithJoystick(Button hopper_button) { - requires(Robot.shooter); - this.hopper_button = hopper_button; +import frc.robot.commands.AutoCommands.SaveOffset; + +public class ReturnPosition extends Command { + + PIDController pid; + + public ReturnPosition() { // Use requires() here to declare subsystem dependencies // eg. requires(chassis); + requires(Robot.drivetrain); + requires(Robot.limelight); + pid = new PIDController(Constants.LimelightkP, Constants.LimelightkI, Constants.LimelightkD); } // Called just before this Command runs the first time @Override protected void initialize() { + Robot.limelight.setCamMode(CamMode.VISION_CAM); + Robot.limelight.setLedMode(LedMode.PIPELINE); + pid.setSetpoint(Constants.tempOffset); + pid.setTolerance(0.1); //0.3 old } // Called repeatedly when this Command is scheduled to run @Override protected void execute() { - double output = Util.deadzone(Constants.DEADZONE, OI.operatorStick.getRawAxis(1), 1.0) * Constants.MANUAL_POWER * -1; - if(Robot.oi.operatorLS.get()) { - Robot.hopper.setHopper(output); //THIS SHOULD BE SET MANUAL POWER - + double output = pid.calculate(Robot.limelight.getHorizontalOffset()); + Robot.drivetrain.setMotors(-output, output); + SmartDashboard.putBoolean("Drivetrain Align Complete", false); } - - - - else{ - } -} - - // Make this return true when this Command no longer needs to run execute() @Override protected boolean isFinished() { - return !hopper_button.get(); - } + return pid.atSetpoint(); + } // Called once after isFinished returns true @Override protected void end() { + // Robot.limelight.setCamMode(CamMode.DRIVER_CAM); + SmartDashboard.putBoolean("Drivetrain Align Complete", true); + Robot.drivetrain.setMotors(0, 0); } // Called when another command which requires one or more of the same diff --git a/src/main/java/frc/robot/commands/AutoCommands/SaveOffset.java b/src/main/java/frc/robot/commands/AutoCommands/SaveOffset.java new file mode 100644 index 0000000..c1220e1 --- /dev/null +++ b/src/main/java/frc/robot/commands/AutoCommands/SaveOffset.java @@ -0,0 +1,25 @@ +package frc.robot.commands.AutoCommands; + +import edu.wpi.first.wpilibj.command.Command; +import frc.robot.Constants; +import frc.robot.Robot; + +public class SaveOffset extends Command { + //public double tempOffset; + + public SaveOffset() { + super("SaveOffset"); + requires(Robot.limelight); + } + + public void execute() { + Constants.tempOffset = Robot.limelight.getHorizontalOffset(); + } + + public void end() { + } + + public boolean isFinished() { + return true; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/Climb/ActivePosition.java b/src/main/java/frc/robot/commands/Climb/ActivePosition.java index 9d39358..59f4e8c 100644 --- a/src/main/java/frc/robot/commands/Climb/ActivePosition.java +++ b/src/main/java/frc/robot/commands/Climb/ActivePosition.java @@ -1,83 +1,77 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ +package frc.robot.commands.Climb; -// package frc.robot.commands.Climb; +import edu.wpi.first.wpilibj.command.Command; +import frc.robot.Robot; +import edu.wpi.first.wpilibj.GyroBase; +import frc.robot.Constants; -// import edu.wpi.first.wpilibj.command.Command; -// import frc.robot.Robot; -// import edu.wpi.first.wpilibj.GyroBase; -// import frc.robot.Constants; +public class ActivePosition extends Command { + private GyroBase climbGyro; + double angle, turn; + public ActivePosition() { + super("ActivePosition"); + // requires(Robot.Climber); + } -// public class ActivePosition extends Command { -// private GyroBase climbGyro; -// double angle, turn; -// public ActivePosition() { -// super("ActivePosition"); -// // requires(Robot.Climber); -// } + public double getGyroAngle() { -// public double getGyroAngle() { - -// angle=climbGyro.getAngle(); -// return climbGyro.getAngle(); + angle=climbGyro.getAngle(); + return climbGyro.getAngle(); -// } +} -// // Called just before this Command runs the first time -// @Override -// protected void initialize() { -// } + // Called just before this Command runs the first time + @Override + protected void initialize() { + } -// // Called repeatedly when this Command is scheduled to run -// @Override -// protected void execute() { -// angle=climbGyro.getAngle(); + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + angle=climbGyro.getAngle(); -// if(angle==0){ -// turn=0; - -// } -// else if(angle/360==0){ - -// turn=0; -// } -// else if(angle>270&&angle<360){ - -// turn=Constants.kSelfClimbGoRight; -// } - -// else if(angle<90&&angle>0){ -// turn=Constants.kSelfClimbGoLeft; -// } + if(angle==0){ + turn=0; + Robot.climber.setSelfClimbOutput(turn); + } + else if(angle/360==0){ + turn=0; + Robot.climber.setSelfClimbOutput(turn); + } + else if(angle>270&&angle<360){ + turn=Constants.kSelfClimbGoRight; + Robot.climber.setSelfClimbOutput(turn); + } + + else if(angle<90&&angle>0){ + turn=Constants.kSelfClimbGoLeft; + Robot.climber.setSelfClimbOutput(turn); + } -// Robot.climber.setSelfClimbOutput(turn); -// } + + } -// // Make this return true when this Command no longer needs to run execute() -// @Override -// protected boolean isFinished() { -// return false; -// } - -// // Called once after isFinished returns true -// @Override -// protected void end() { -// Robot.climber.setSelfClimbOutput(0); -// } - -// // Called when another command which requires one or more of the same -// // subsystems is scheduled to run -// @Override -// protected void interrupted() { -// } -// } + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + return false; + } + + // Called once after isFinished returns true + @Override + protected void end() { + Robot.climber.setSelfClimbOutput(0); + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + } +} diff --git a/src/main/java/frc/robot/commands/Climb/ReleasePin.java b/src/main/java/frc/robot/commands/Climb/ReleasePin.java index b2ecf11..7444911 100644 --- a/src/main/java/frc/robot/commands/Climb/ReleasePin.java +++ b/src/main/java/frc/robot/commands/Climb/ReleasePin.java @@ -7,31 +7,53 @@ package frc.robot.commands.Climb; import frc.robot.Robot; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.command.Command; import frc.robot.subsystems.Climber; public class ReleasePin extends Command { - public ReleasePin() { + private boolean lock, lowerIntake; + private Timer intakeRetractTimer; + public ReleasePin(boolean extend, boolean lowerIntake) { super("ReleasePin"); requires(Robot.climber); + requires(Robot.manipulator); + lock = extend; + this.lowerIntake = lowerIntake; + intakeRetractTimer = new Timer(); } protected void initialize() { - + intakeRetractTimer.start(); } protected void execute() { - Robot.climber.setPinExtender(false); + if (intakeRetractTimer.get() > 0.25) { + Robot.climber.setPinExtender(lock); + } + + if (lowerIntake) { + if (intakeRetractTimer.get() < 2) { + Robot.manipulator.setExtended(); + } else { + Robot.manipulator.setRetracted(); + } + } } protected boolean isFinished() { - return false; + if(lowerIntake){ + return intakeRetractTimer.get() > 0.1 && Robot.manipulator.getRetracted(); + } + else { + return true; + } } protected void end() { - Robot.climber.setPinExtender(true); + // Robot.climber.setPinExtender(true); } protected void interrupted() { diff --git a/src/main/java/frc/robot/commands/Climb/StopWinchClimb.java b/src/main/java/frc/robot/commands/Climb/StopWinchClimb.java index 832de5e..eb238f9 100644 --- a/src/main/java/frc/robot/commands/Climb/StopWinchClimb.java +++ b/src/main/java/frc/robot/commands/Climb/StopWinchClimb.java @@ -14,6 +14,7 @@ public class StopWinchClimb extends Command { public StopWinchClimb() { // Use requires() here to declare subsystem dependencies // eg. requires(chassis); + requires(Robot.climber); } // Called just before this Command runs the first time @@ -25,18 +26,19 @@ protected void initialize() { @Override protected void execute() { Robot.climber.setWinchOutput(0); + Robot.climber.setRetracted(); } // Make this return true when this Command no longer needs to run execute() @Override protected boolean isFinished() { - return false; + return true; } // Called once after isFinished returns true @Override protected void end() { - Robot.climber.setWinchOutput(0); + // Robot.climber.setWinchOutput(0); } diff --git a/src/main/java/frc/robot/commands/Climb/WinchClimb.java b/src/main/java/frc/robot/commands/Climb/WinchClimb.java index 990f8e9..7d1040b 100644 --- a/src/main/java/frc/robot/commands/Climb/WinchClimb.java +++ b/src/main/java/frc/robot/commands/Climb/WinchClimb.java @@ -7,36 +7,53 @@ package frc.robot.commands.Climb; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.command.Command; import frc.robot.Robot; public class WinchClimb extends Command { - public WinchClimb() { + private boolean direction; + private Timer waitTimer; + public WinchClimb(boolean down) { + super("WinchClimb"); // Use requires() here to declare subsystem dependencies // eg. requires(chassis); + direction = down; + waitTimer = new Timer(); + requires(Robot.climber); } // Called just before this Command runs the first time @Override protected void initialize() { + waitTimer.start(); } // Called repeatedly when this Command is scheduled to run @Override protected void execute() { - Robot.climber.setWinchOutput(1); + Robot.climber.setExtended(); + if (waitTimer.get() > 0.25) { + if (direction){ + Robot.climber.setWinchOutput(-1); + } + else if (!direction){ + Robot.climber.setWinchOutput(1); + } + } + Robot.manipulator.setRetracted(); } // Make this return true when this Command no longer needs to run execute() @Override protected boolean isFinished() { - return false; + return waitTimer.get() > 0.27; } // Called once after isFinished returns true @Override protected void end() { - Robot.climber.setWinchOutput(1); + // Robot.climber.setWinchOutput(0); } diff --git a/src/main/java/frc/robot/commands/DriveWithJoystick.java b/src/main/java/frc/robot/commands/DriveWithJoystick.java index b7a7deb..2e6141d 100644 --- a/src/main/java/frc/robot/commands/DriveWithJoystick.java +++ b/src/main/java/frc/robot/commands/DriveWithJoystick.java @@ -1,69 +1,79 @@ -// package frc.robot.commands; +package frc.robot.commands; -// import edu.wpi.first.wpilibj.Joystick; -// import edu.wpi.first.wpilibj.command.Command; -// import frc.robot.Constants; -// import frc.robot.Robot; +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.command.Command; +import frc.robot.Constants; +import frc.robot.Robot; -// public class DriveWithJoystick extends Command { -// private Joystick stick; -// private boolean isSquaredTurn; -// private double deadzone; +public class DriveWithJoystick extends Command { + private Joystick stick; + private boolean isSquaredTurn; + private double deadzone; -// public DriveWithJoystick(Joystick stick, double deadzone) { -// super("DriveWithJoystick"); -// requires(Robot.drivetrain); -// this.stick = stick; -// this.deadzone = deadzone; -// } + public DriveWithJoystick(Joystick stick, double deadzone) { + super("DriveWithJoystick"); + requires(Robot.drivetrain); + this.stick = stick; + this.deadzone = deadzone; + } -// public void execute() { -// double thrust = 0; -// if(stick.getRawButton(6)) { -// thrust = Constants.kCreep; -// } -// else if(stick.getRawButton(5)) { -// thrust = -Constants.kCreep; -// } else { -// thrust = stick.getRawAxis(3) - stick.getRawAxis(2); -// if(Math.abs(thrust) < deadzone) { -// thrust = 0; -// } -// } + public void execute() { + // if(stick.getRawButton(5) && stick.getRawButton(6)){ + // Robot.drivetrain.setMotors(Constants.kCreep, Constants.kCreep); + // } else if (stick.getRawButton(5)){ + // Robot.drivetrain.setMotors(-Constants.kCreep, Constants.kCreep); + // } else if (stick.getRawButton(6)){ + // Robot.drivetrain.setMotors( Constants.kCreep, -Constants.kCreep); + // } else { + // Robot.drivetrain.setMotors(0, 0); + // } + double thrust = 0; + if(stick.getRawButton(6)) { + thrust = Constants.kCreep; + } + else if(stick.getRawButton(5)) { + thrust = -Constants.kCreep; + } else { + thrust = stick.getRawAxis(3) - stick.getRawAxis(2); + if(Math.abs(thrust) < deadzone) { + thrust = 0; + } + } -// double turn = stick.getRawAxis(0); + double turn = stick.getRawAxis(0) * Constants.TURN_FACTOR; -// if(turn < deadzone && turn > -deadzone) { -// turn = 0; -// } + if(turn < deadzone && turn > -deadzone) { + turn = 0; + } -// if(isSquaredTurn) { -// turn *= Math.abs(turn); -// } + if(isSquaredTurn) { + turn *= Math.abs(turn); + } -// if(stick.getRawButton(2)) { -// turn /= 3; -// } + if(stick.getRawButton(2)) { + turn /= 3; + } -// // This causes the robot to crash. It is the ideal solution to wheelies. -// // if(Math.abs(turn) > 0.05) { -// // Robot.drivetrain.setLimit(Constants.kNeoAmpLimitTurn); -// // } -// // else { -// // Robot.drivetrain.setLimit(Constants.kNeoAmpLimit); -// // } + // This causes the robot to crash. It is the ideal solution to wheelies. + // if(Math.abs(turn) > 0.05) { + // Robot.drivetrain.setLimit(Constants.kNeoAmpLimitTurn); + // } + // else { + // Robot.drivetrain.setLimit(Constants.kNeoAmpLimit); + // } -// double left = Math.max(Math.min(thrust + turn, 1), -1); -// double right = Math.max(Math.min(thrust - turn, 1), -1); + double left = Math.max(Math.min(thrust + turn, 1), -1); + double right = Math.max(Math.min(thrust - turn, 1), -1); -// Robot.drivetrain.setMotors(left, right); -// } + // Robot.drivetrain.setMotors(left, right); + Robot.drivetrain.setMotors(left, right); + } -// public void end() { -// Robot.drivetrain.setMotors(0, 0); -// } + public void end() { + Robot.drivetrain.setMotors(0, 0); + } -// public boolean isFinished() { -// return false; -// } -// } \ No newline at end of file + public boolean isFinished() { + return false; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/DrivetrainAlignToGoal.java b/src/main/java/frc/robot/commands/DrivetrainAlignToGoal.java new file mode 100644 index 0000000..f60c079 --- /dev/null +++ b/src/main/java/frc/robot/commands/DrivetrainAlignToGoal.java @@ -0,0 +1,66 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc.robot.commands; + +import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.controller.PIDController; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.Constants; +import frc.robot.Robot; +import frc.robot.subsystems.Limelight.CamMode; +import frc.robot.subsystems.Limelight.LedMode; + +public class DrivetrainAlignToGoal extends Command { + + PIDController pid; + + public DrivetrainAlignToGoal() { + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + requires(Robot.drivetrain); + requires(Robot.limelight); + pid = new PIDController(Constants.LimelightkP, Constants.LimelightkI, Constants.LimelightkD); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + Robot.limelight.setCamMode(CamMode.VISION_CAM); + Robot.limelight.setLedMode(LedMode.PIPELINE); + pid.setSetpoint(Constants.LimelightSetpoint); + pid.setTolerance(0.1); //0.3 old + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + double output = pid.calculate(Robot.limelight.getHorizontalOffset()); + Robot.drivetrain.setMotors(-output, output); + SmartDashboard.putBoolean("Drivetrain Align Complete", false); + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + return pid.atSetpoint(); + } + + // Called once after isFinished returns true + @Override + protected void end() { + // Robot.limelight.setCamMode(CamMode.DRIVER_CAM); + SmartDashboard.putBoolean("Drivetrain Align Complete", true); + Robot.drivetrain.setMotors(0, 0); + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + } +} diff --git a/src/main/java/frc/robot/commands/DriveWithJoystickLeft.java b/src/main/java/frc/robot/commands/DrivetrainBackwards.java similarity index 58% rename from src/main/java/frc/robot/commands/DriveWithJoystickLeft.java rename to src/main/java/frc/robot/commands/DrivetrainBackwards.java index d69c018..cd50d33 100644 --- a/src/main/java/frc/robot/commands/DriveWithJoystickLeft.java +++ b/src/main/java/frc/robot/commands/DrivetrainBackwards.java @@ -2,26 +2,24 @@ import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.command.Command; -import frc.robot.Constants; import frc.robot.Robot; -public class DriveWithJoystickLeft extends Command { +public class DrivetrainBackwards extends Command { private Joystick stick; private boolean isSquaredTurn; private double deadzone; - public DriveWithJoystickLeft() { - super("DriveWithJoystickLeft"); + public DrivetrainBackwards() { + super("DrivetrainBackwards"); requires(Robot.drivetrain); } public void execute() { - System.out.println("This is left Neo"); - Robot.drivetrain.setMotors(-1); + Robot.drivetrain.setMotors(-1,-1); } public void end() { - Robot.drivetrain.setMotors(-1); + Robot.drivetrain.setMotors(-1,-1); } public boolean isFinished() { diff --git a/src/main/java/frc/robot/commands/IntakeToHopper/BeamBreak.java b/src/main/java/frc/robot/commands/IntakeToHopper/BeamBreak.java new file mode 100644 index 0000000..d61e473 --- /dev/null +++ b/src/main/java/frc/robot/commands/IntakeToHopper/BeamBreak.java @@ -0,0 +1,76 @@ +package frc.robot.commands.IntakeToHopper; + +import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.Constants; +import frc.robot.Robot; +import frc.robot.commands.IntakeToHopper.MoveManipulator; +import edu.wpi.first.wpilibj.Timer; + +public class BeamBreak extends Command { + public int count; + private boolean timerStart = false; + public Timer timer= new Timer(); + public BeamBreak() { + super("BeamBreak"); + requires(Robot.hopper); + + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + // count = 0; + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + + SmartDashboard.putBoolean("Beam Break Hopper Running", true); + if(!Robot.hopper.beamBreak1.get()){ + timer.start(); + Robot.hopper.setHopper(Constants.HOPPER_LOADING_HORIZONTAL_OUTPUT, + Constants.HOPPER_LOADING_VERTICAL_OUTPUT); + timerStart = true; + } + if(timerStart){ + if (timer.get()>=Constants.kHopperTimer){ + timer.stop(); + timer.reset(); + Robot.hopper.setHopper(0,0); + } + } + + } + + // Make this return true when this Command no longer needs to run execute() + @Override + + protected boolean isFinished() { + // if (Robot.hopper.isHopperFull()==true){ + // return true; + // } + // else{ + // return false; + // } + return false; + + } + + // Called once after isFinished returns true + @Override + protected void end() { + // count++; + Robot.hopper.setHopper(0, 0); + SmartDashboard.putBoolean("Ir Hopper Running", false); + // Robot.hopper.counter.reset(); + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + } + +} diff --git a/src/main/java/frc/robot/commands/IntakeToHopper/DoubleBeamBreakHopper.java b/src/main/java/frc/robot/commands/IntakeToHopper/DoubleBeamBreakHopper.java new file mode 100644 index 0000000..e8e5f1d --- /dev/null +++ b/src/main/java/frc/robot/commands/IntakeToHopper/DoubleBeamBreakHopper.java @@ -0,0 +1,68 @@ +package frc.robot.commands.IntakeToHopper; + +import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.Constants; +import frc.robot.Robot; +import frc.robot.commands.IntakeToHopper.MoveManipulator; +import edu.wpi.first.wpilibj.Timer; + +public class DoubleBeamBreakHopper extends Command { + public boolean firstBeamBroken = false; + public DoubleBeamBreakHopper() { + super("DoubleBeamBreakHopper"); + requires(Robot.hopper); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + + SmartDashboard.putBoolean("Beam Break Hopper Running", true); + if(!Robot.hopper.beamBreak1.get()){ + Robot.hopper.setHopper(Constants.HOPPER_LOADING_HORIZONTAL_OUTPUT, + Constants.HOPPER_LOADING_VERTICAL_OUTPUT); + firstBeamBroken = true; + } + if(!Robot.hopper.beamBreak2.get() && firstBeamBroken){ + Robot.hopper.setHopper(0,0); + firstBeamBroken = false; + } + + } + + // Make this return true when this Command no longer needs to run execute() + @Override + + protected boolean isFinished() { + // if (Robot.hopper.isHopperFull()==true){ + // return true; + // } + // else{ + // return false; + // } + return Robot.hopper.isUpperSensorTripped(); + + } + + // Called once after isFinished returns true + @Override + protected void end() { + // count++; + Robot.hopper.setHopper(0, 0); + SmartDashboard.putBoolean("Ir Hopper Running", false); + // Robot.hopper.counter.reset(); + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + } + +} diff --git a/src/main/java/frc/robot/commands/IntakeToHopper/FifthBall.java b/src/main/java/frc/robot/commands/IntakeToHopper/FifthBall.java new file mode 100644 index 0000000..95c9260 --- /dev/null +++ b/src/main/java/frc/robot/commands/IntakeToHopper/FifthBall.java @@ -0,0 +1,76 @@ +package frc.robot.commands.IntakeToHopper; + +import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.Constants; +import frc.robot.Robot; +import frc.robot.commands.IntakeToHopper.MoveManipulator; +import edu.wpi.first.wpilibj.Timer; + +public class FifthBall extends Command { + public Timer timer = new Timer(); + public boolean HopperSet = false; + private boolean timerStart = true; + public FifthBall() { + super("FifthBall"); + requires(Robot.hopper); + + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + if(Robot.hopper.beamBreak1.get()){ + HopperSet = true; + } + + SmartDashboard.putBoolean("Beam Break Hopper Running", true); + if (HopperSet && !Robot.hopper.beamBreak1.get()){ + if(timerStart){ + timer.start(); + timerStart = false; + } + Robot.hopper.setHopper(Constants.HOPPER_LOADING_HORIZONTAL_OUTPUT, Constants.HOPPER_LOADING_VERTICAL_OUTPUT); + } + + if(timer.get()>=Constants.kHopperTimerBeams){ + Robot.hopper.setHopper(0,0); + } + + } + + // Make this return true when this Command no longer needs to run execute() + @Override + + protected boolean isFinished() { + // if (Robot.hopper.isHopperFull()==true){ + // return true; + // } + // else{ + // return false; + // } + return false; + } + + // Called once after isFinished returns true + @Override + protected void end() { + // count++; + Robot.hopper.setHopper(0, 0); + SmartDashboard.putBoolean("Ir Hopper Running", false); + // Robot.hopper.counter.reset(); + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + } + +} diff --git a/src/main/java/frc/robot/Autons/RunElevatorWithJoystick.java b/src/main/java/frc/robot/commands/IntakeToHopper/FixHopperSpacing.java similarity index 56% rename from src/main/java/frc/robot/Autons/RunElevatorWithJoystick.java rename to src/main/java/frc/robot/commands/IntakeToHopper/FixHopperSpacing.java index 99e9323..a2ea059 100644 --- a/src/main/java/frc/robot/Autons/RunElevatorWithJoystick.java +++ b/src/main/java/frc/robot/commands/IntakeToHopper/FixHopperSpacing.java @@ -5,20 +5,20 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc.robot.Autons; +package frc.robot.commands.IntakeToHopper; -import edu.wpi.first.wpilibj.buttons.Button; +import java.util.concurrent.DelayQueue; + +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.command.Command; import frc.robot.Constants; -import frc.robot.OI; import frc.robot.Robot; -import frc.robot.util.Util; -public class RunElevatorWithJoystick extends Command { - Button elevator_button; - public RunElevatorWithJoystick(Button elevator_button) { - requires(Robot.elevator); - this.elevator_button = elevator_button; +public class FixHopperSpacing extends Command { + Timer timeToFixSpace; + public FixHopperSpacing() { + super("FixHopperSpacing"); + requires(Robot.hopper); // Use requires() here to declare subsystem dependencies // eg. requires(chassis); } @@ -31,28 +31,29 @@ protected void initialize() { // Called repeatedly when this Command is scheduled to run @Override protected void execute() { - double output = Util.deadzone(Constants.DEADZONE, OI.operatorStick.getRawAxis(1), 1.0) * Constants.MANUAL_POWER * -1; - if(Robot.oi.operatorLS.get()) { - Robot.elevator.setElevator(output); //THIS SHOULD BE SET MANUAL POWER - - } - - - - else{ - + if(!Robot.hopper.isUpperSensorTripped()){ + Robot.hopper.setHopper(Constants.HOPPER_LOADING_HORIZONTAL_OUTPUT, Constants.HOPPER_LOADING_VERTICAL_OUTPUT); + } + else if(Robot.hopper.isUpperSensorTripped()){ + Robot.hopper.setHopper(Constants.HOPPER_LOADING_HORIZONTAL_OUTPUT, 0); + Timer.delay(Constants.fixSpaceTime); + Robot.hopper.setHopper(-Constants.HOPPER_LONG_HORIZONTAL_OUTPUT, -Constants.HOPPER_VERTICAL_OUTPUT); + } + //Pulls balls as far up as possible + //only runs the horizontal belt (not the vertical) for some time to decrease the space between balls 2 and 3 + //pulls balls back to first ir sensor } -} // Make this return true when this Command no longer needs to run execute() @Override protected boolean isFinished() { - return !elevator_button.get(); - } + return Robot.hopper.isBall(); + } // Called once after isFinished returns true @Override protected void end() { + Robot.hopper.setHopper(0, 0); } // Called when another command which requires one or more of the same diff --git a/src/main/java/frc/robot/commands/IntakeToHopper/IrHopper.java b/src/main/java/frc/robot/commands/IntakeToHopper/IrHopper.java new file mode 100644 index 0000000..ad507bf --- /dev/null +++ b/src/main/java/frc/robot/commands/IntakeToHopper/IrHopper.java @@ -0,0 +1,53 @@ +package frc.robot.commands.IntakeToHopper; + +import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.Robot; + + +public class IrHopper extends Command { + public int count; + public IrHopper() { + super("IrHopper"); + requires(Robot.hopper); + // Use requires() here to declare subsystem dependencies + + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + // count = 0; + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + Robot.hopper.loadingWithIR(); + + } + + // Make this return true when this Command no longer needs to run execute() + @Override + + protected boolean isFinished() { + return !Robot.hopper.isBall() || Robot.hopper.isUpperSensorTripped(); + + + } + + // Called once after isFinished returns true + @Override + protected void end() { + // count++; + Robot.hopper.setHopper(0, 0); + SmartDashboard.putBoolean("Ir Hopper Running", false); + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + } + +} diff --git a/src/main/java/frc/robot/commands/IntakeToHopper/LimitSwitch.java b/src/main/java/frc/robot/commands/IntakeToHopper/LimitSwitch.java new file mode 100644 index 0000000..61ed8a1 --- /dev/null +++ b/src/main/java/frc/robot/commands/IntakeToHopper/LimitSwitch.java @@ -0,0 +1,79 @@ +// limit switch swapped with Beam Break sensor + + +// package frc.robot.commands.IntakeToHopper; + +// import edu.wpi.first.wpilibj.command.Command; +// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +// import frc.robot.Constants; +// import frc.robot.Robot; +// import frc.robot.commands.IntakeToHopper.MoveManipulator; +// import edu.wpi.first.wpilibj.Timer; + +// public class LimitSwitch extends Command { +// public int count; +// private boolean timerStart = false; +// public Timer timer= new Timer(); +// public LimitSwitch() { +// super("LimitSwitch"); +// requires(Robot.hopper); + +// } + +// // Called just before this Command runs the first time +// @Override +// protected void initialize() { +// // count = 0; +// } + +// // Called repeatedly when this Command is scheduled to run +// @Override +// protected void execute() { + +// SmartDashboard.putBoolean("Limit Switch Hopper Running", true); +// if(!Robot.hopper.limitSwitch.get()){ +// timer.start(); +// Robot.hopper.setHopper(Constants.HOPPER_LOADING_HORIZONTAL_OUTPUT, +// Constants.HOPPER_LOADING_VERTICAL_OUTPUT); +// timerStart = true; +// } +// if(timerStart){ +// if (timer.get()>=Constants.kHopperTimer){ +// timer.stop(); +// timer.reset(); +// Robot.hopper.setHopper(0,0); +// } +// } + +// } + +// // Make this return true when this Command no longer needs to run execute() +// @Override + +// protected boolean isFinished() { +// // if (Robot.hopper.isHopperFull()==true){ +// // return true; +// // } +// // else{ +// // return false; +// // } +// return false; + +// } + +// // Called once after isFinished returns true +// @Override +// protected void end() { +// // count++; +// Robot.hopper.setHopper(0, 0); +// SmartDashboard.putBoolean("Ir Hopper Running", false); +// // Robot.hopper.counter.reset(); +// } + +// // Called when another command which requires one or more of the same +// // subsystems is scheduled to run +// @Override +// protected void interrupted() { +// } + +// } diff --git a/src/main/java/frc/robot/commands/MoveHopper.java b/src/main/java/frc/robot/commands/IntakeToHopper/MoveHopperLong.java similarity index 82% rename from src/main/java/frc/robot/commands/MoveHopper.java rename to src/main/java/frc/robot/commands/IntakeToHopper/MoveHopperLong.java index 4b04e0c..0833c0b 100644 --- a/src/main/java/frc/robot/commands/MoveHopper.java +++ b/src/main/java/frc/robot/commands/IntakeToHopper/MoveHopperLong.java @@ -5,15 +5,15 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc.robot.commands; +package frc.robot.commands.IntakeToHopper; import edu.wpi.first.wpilibj.command.Command; import frc.robot.Constants; import frc.robot.Robot; -public class MoveHopper extends Command { - public MoveHopper() { - super("MoveHopper"); +public class MoveHopperLong extends Command { + public MoveHopperLong() { + super("MoveHopperLong"); requires(Robot.hopper); // Use requires() here to declare subsystem dependencies // eg. requires(chassis); @@ -27,7 +27,7 @@ protected void initialize() { // Called repeatedly when this Command is scheduled to run @Override protected void execute() { - Robot.hopper.setHopper(Constants.HOPPER_OUTPUT); + Robot.hopper.setHopper(Constants.HOPPER_LONG_HORIZONTAL_OUTPUT, Constants.HOPPER_VERTICAL_OUTPUT); } // Make this return true when this Command no longer needs to run execute() @@ -39,7 +39,7 @@ protected boolean isFinished() { // Called once after isFinished returns true @Override protected void end() { - Robot.hopper.setHopper(0); + Robot.hopper.setHopper(0, 0); } // Called when another command which requires one or more of the same diff --git a/src/main/java/frc/robot/commands/MoveElevator.java b/src/main/java/frc/robot/commands/IntakeToHopper/MoveHopperWall.java similarity index 79% rename from src/main/java/frc/robot/commands/MoveElevator.java rename to src/main/java/frc/robot/commands/IntakeToHopper/MoveHopperWall.java index 70c402f..f9bc688 100644 --- a/src/main/java/frc/robot/commands/MoveElevator.java +++ b/src/main/java/frc/robot/commands/IntakeToHopper/MoveHopperWall.java @@ -5,16 +5,16 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc.robot.commands; +package frc.robot.commands.IntakeToHopper; import edu.wpi.first.wpilibj.command.Command; import frc.robot.Constants; import frc.robot.Robot; -public class MoveElevator extends Command { - public MoveElevator() { - super("MoveElevator"); - requires(Robot.elevator); +public class MoveHopperWall extends Command { + public MoveHopperWall() { + super("MoveHopperWall"); + requires(Robot.hopper); // Use requires() here to declare subsystem dependencies // eg. requires(chassis); } @@ -27,19 +27,19 @@ protected void initialize() { // Called repeatedly when this Command is scheduled to run @Override protected void execute() { - Robot.elevator.setElevator(Constants.ELEVATOR_OUTPUT); + Robot.hopper.setHopper(Constants.HOPPER_WALL_HORIZONTAL_OUTPUT, Constants.HOPPER_VERTICAL_OUTPUT); } // Make this return true when this Command no longer needs to run execute() @Override protected boolean isFinished() { - return false; + return true; } // Called once after isFinished returns true @Override protected void end() { - Robot.elevator.setElevator(0); + // Robot.hopper.setHopper(0, 0); } // Called when another command which requires one or more of the same diff --git a/src/main/java/frc/robot/commands/IntakeToHopper/MoveManipulator.java b/src/main/java/frc/robot/commands/IntakeToHopper/MoveManipulator.java new file mode 100644 index 0000000..4b3c593 --- /dev/null +++ b/src/main/java/frc/robot/commands/IntakeToHopper/MoveManipulator.java @@ -0,0 +1,48 @@ +package frc.robot.commands.IntakeToHopper; + +import edu.wpi.first.wpilibj.command.Command; +import frc.robot.Constants; +import frc.robot.Robot; + +public class MoveManipulator extends Command { + public MoveManipulator() { + super("MoveManipulator"); + requires(Robot.manipulator); + //Use requires() here to declare subsystem dependencies + //eg. requires(chassis); + } + + //Called just before this Command runs the first time + @Override + protected void initialize() { + Robot.manipulator.setExtended(); + } + + //Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + // Robot.manipulator.setExtended(); + Robot.manipulator.setManipulator(Constants.INTAKE_MOTORSPEED); + + } + + //Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + return true; + } + + //Called once after isFinished returns true + @Override + protected void end() { + // Robot.manipulator.setManipulator(0); + } + + //Called when another command which requires one or more of the same + //subsystems is scheduled to run + + @Override + protected void interrupted() { + //Yes + } +} diff --git a/src/main/java/frc/robot/commands/IntakeToHopper/OpenPistonsAndMoveHopper.java b/src/main/java/frc/robot/commands/IntakeToHopper/OpenPistonsAndMoveHopper.java new file mode 100644 index 0000000..63a555f --- /dev/null +++ b/src/main/java/frc/robot/commands/IntakeToHopper/OpenPistonsAndMoveHopper.java @@ -0,0 +1,59 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc.robot.commands.IntakeToHopper; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.command.Command; +import frc.robot.Constants; +import frc.robot.Robot; + +public class OpenPistonsAndMoveHopper extends Command { + Timer timeForHopper; + public OpenPistonsAndMoveHopper() { + super("OpenPistonsAndMoveHopper"); + requires(Robot.hopper); + timeForHopper = new Timer(); + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + timeForHopper.start(); + Robot.hopper.setExtended(); + + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + if(timeForHopper.get() > .25){ + Robot.hopper.setHopper(Constants.HOPPER_WALL_HORIZONTAL_OUTPUT, Constants.HOPPER_VERTICAL_OUTPUT); + } + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + return false; + } + + // Called once after isFinished returns true + @Override + protected void end() { + Robot.hopper.setHopper(0, 0); + Robot.hopper.setRetracted(); + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + } +} diff --git a/src/main/java/frc/robot/commands/IntakeToHopper/ReverseHopper.java b/src/main/java/frc/robot/commands/IntakeToHopper/ReverseHopper.java new file mode 100644 index 0000000..0f0450c --- /dev/null +++ b/src/main/java/frc/robot/commands/IntakeToHopper/ReverseHopper.java @@ -0,0 +1,43 @@ +package frc.robot.commands.IntakeToHopper; + +import edu.wpi.first.wpilibj.command.Command; +import frc.robot.Constants; +import frc.robot.Robot; + +public class ReverseHopper extends Command { + public ReverseHopper() { + super("ReverseHopper"); + requires(Robot.hopper); + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + Robot.hopper.setHopper(Constants.REVERSE_HOPPER, Constants.REVERSE_HOPPER); + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + return true; + } + + // Called once after isFinished returns true + @Override + protected void end() { + // Robot.hopper.setHopper(0, 0); + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/IntakeToHopper/ReverseHopperAtVoltage.java b/src/main/java/frc/robot/commands/IntakeToHopper/ReverseHopperAtVoltage.java new file mode 100644 index 0000000..7b0408b --- /dev/null +++ b/src/main/java/frc/robot/commands/IntakeToHopper/ReverseHopperAtVoltage.java @@ -0,0 +1,56 @@ +package frc.robot.commands.IntakeToHopper; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.command.Command; +import frc.robot.Constants; +import frc.robot.Robot; + +public class ReverseHopperAtVoltage extends Command { + Timer reverseTime; + private boolean reverse = false; + public ReverseHopperAtVoltage() { + super("ReverseHopperAtVoltage"); + requires(Robot.hopper); + reverseTime = new Timer(); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + if(Robot.hopper.getHorizontalCurrent() > Constants.jamCurrent){ + reverseTime.start(); + reverse = true; + } + if(reverse && reverseTime.get() < Constants.reverseTime){ + Robot.hopper.setHopper(Constants.REVERSE_HOPPER, Constants.REVERSE_HOPPER); + } + else{ + Robot.hopper.setHopper(0, 0); + reverseTime.stop(); + reverseTime.reset(); + } + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + return !reverse; + } + + // Called once after isFinished returns true + @Override + protected void end() { + // Robot.hopper.setHopper(0, 0); + } + + @Override + protected void interrupted() { + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/IntakeToHopper/ReverseIntake.java b/src/main/java/frc/robot/commands/IntakeToHopper/ReverseIntake.java new file mode 100644 index 0000000..ef15770 --- /dev/null +++ b/src/main/java/frc/robot/commands/IntakeToHopper/ReverseIntake.java @@ -0,0 +1,48 @@ +package frc.robot.commands.IntakeToHopper; + +import edu.wpi.first.wpilibj.command.Command; +import frc.robot.Constants; +import frc.robot.Robot; + +public class ReverseIntake extends Command { + public ReverseIntake() { + super("ReverseIntake"); + requires(Robot.manipulator); + //Use requires() here to declare subsystem dependencies + //eg. requires(chassis); + } + + //Called just before this Command runs the first time + @Override + protected void initialize() { + //Robot.manipulator.setExtended(); + } + + //Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + // Robot.manipulator.setExtended(); + Robot.manipulator.setManipulator(-Constants.INTAKE_MOTORSPEED); + + } + + //Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + return true; + } + + //Called once after isFinished returns true + @Override + protected void end() { + // Robot.manipulator.setManipulator(0); + } + + //Called when another command which requires one or more of the same + //subsystems is scheduled to run + + @Override + protected void interrupted() { + //Yes + } +} diff --git a/src/main/java/frc/robot/commands/IntakeToHopper/ReverseUntilBeamBreak.java b/src/main/java/frc/robot/commands/IntakeToHopper/ReverseUntilBeamBreak.java new file mode 100644 index 0000000..273dc51 --- /dev/null +++ b/src/main/java/frc/robot/commands/IntakeToHopper/ReverseUntilBeamBreak.java @@ -0,0 +1,45 @@ +package frc.robot.commands.IntakeToHopper; + +import edu.wpi.first.wpilibj.command.Command; +import frc.robot.Constants; +import frc.robot.Robot; + +public class ReverseUntilBeamBreak extends Command { + public ReverseUntilBeamBreak() { + super("ReverseUntilBeamBreak"); + requires(Robot.hopper); + + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + if(!Robot.hopper.beamBreak1.get()){ + Robot.hopper.setHopper(0, 0); + } + else{ + Robot.hopper.setHopper(Constants.REVERSE_HOPPER, Constants.REVERSE_HOPPER); + } + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + return true; + } + + // Called once after isFinished returns true + @Override + protected void end() { + // Robot.hopper.setHopper(0, 0); + } + + @Override + protected void interrupted() { + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/StopHopper.java b/src/main/java/frc/robot/commands/IntakeToHopper/StopHopper.java similarity index 92% rename from src/main/java/frc/robot/commands/StopHopper.java rename to src/main/java/frc/robot/commands/IntakeToHopper/StopHopper.java index 26c2f5b..135e944 100644 --- a/src/main/java/frc/robot/commands/StopHopper.java +++ b/src/main/java/frc/robot/commands/IntakeToHopper/StopHopper.java @@ -5,7 +5,7 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc.robot.commands; +package frc.robot.commands.IntakeToHopper; import edu.wpi.first.wpilibj.command.Command; import frc.robot.Robot; @@ -26,7 +26,7 @@ protected void initialize() { // Called repeatedly when this Command is scheduled to run @Override protected void execute() { - Robot.hopper.setHopper(0); + Robot.hopper.setHopper(0, 0); } // Make this return true when this Command no longer needs to run execute() @@ -38,7 +38,7 @@ protected boolean isFinished() { // Called once after isFinished returns true @Override protected void end() { - Robot.hopper.setHopper(0); + Robot.hopper.setHopper(0, 0); } // Called when another command which requires one or more of the same diff --git a/src/main/java/frc/robot/commands/IntakeToHopper/StopManipulator.java b/src/main/java/frc/robot/commands/IntakeToHopper/StopManipulator.java new file mode 100644 index 0000000..5d5482b --- /dev/null +++ b/src/main/java/frc/robot/commands/IntakeToHopper/StopManipulator.java @@ -0,0 +1,46 @@ +package frc.robot.commands.IntakeToHopper; + +import edu.wpi.first.wpilibj.command.Command; +import frc.robot.Robot; + +public class StopManipulator extends Command { + public StopManipulator() { + super("StopManipulator"); + requires(Robot.manipulator); + //Use requires() here to declare subsystem dependencies + //eg. requires(chassis); + } + + //Called just before this Command runs the first time + @Override + protected void initialize() { + + } + + //Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + Robot.manipulator.setManipulator(0); + Robot.manipulator.setRetracted(); + } + + //Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + return false; + } + + //Called once after isFinished returns true + @Override + protected void end() { + Robot.manipulator.setManipulator(0); + } + + //Called when another command which requires one or more of the same + //subsystems is scheduled to run + + @Override + protected void interrupted() { + //Yes + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/IntakeToHopper/TwoBeamBreak.java b/src/main/java/frc/robot/commands/IntakeToHopper/TwoBeamBreak.java new file mode 100644 index 0000000..e9d7d1a --- /dev/null +++ b/src/main/java/frc/robot/commands/IntakeToHopper/TwoBeamBreak.java @@ -0,0 +1,120 @@ +package frc.robot.commands.IntakeToHopper; + +import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.Constants; +import frc.robot.Robot; +import frc.robot.commands.IntakeToHopper.MoveManipulator; +import edu.wpi.first.wpilibj.Timer; + +public class TwoBeamBreak extends Command { + public int count; + private boolean timerStart = false; + private boolean firstBall = false; + private boolean on1 = false; + private boolean off1 = false; + private boolean on2 = false; + private boolean beenthrough = false; + private boolean trippitybop = true; + public Timer timer= new Timer(); + public TwoBeamBreak() { + super("TwoBeamBreak"); + requires(Robot.hopper); + + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + // count = 0; + SmartDashboard.putBoolean("Timer Running", false); + SmartDashboard.putBoolean("Started", false); + SmartDashboard.putBoolean("Beam Break", !Robot.hopper.beamBreak1.get()); + SmartDashboard.putBoolean("Upper Sensor", Robot.hopper.isUpperSensorTripped()); + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + SmartDashboard.putBoolean("Beam Break Hopper Running", true); + if(!Robot.hopper.isUpperSensorTripped()){ + if(!Robot.hopper.beamBreak1.get()){ + // timer.start(); + Robot.hopper.setHopper(Constants.HOPPER_LOADING_HORIZONTAL_OUTPUT, + Constants.HOPPER_LOADING_VERTICAL_OUTPUT); + on1 = true; + // timerStart = true; + } + on1 = !Robot.hopper.beamBreak2.get(); + + if(Robot.hopper.beamBreak2.get() || beenthrough){ + beenthrough = true; + if(!Robot.hopper.beamBreak2.get() && on1){ + Robot.hopper.setHopper(0,0); + on1 = false; + beenthrough = false; + } + + // if(timerStart){ + // if (timer.get()>=Constants.kHopperTimer){ + // timer.stop(); + // timer.reset(); + // Robot.hopper.setHopper(0,0); + // } + // } + } + } + else { + SmartDashboard.putBoolean("Started", true); + SmartDashboard.putBoolean("Beam Break", !Robot.hopper.beamBreak1.get()); + SmartDashboard.putBoolean("Upper Sensor", Robot.hopper.isUpperSensorTripped()); + if((!Robot.hopper.beamBreak1.get()&&Robot.hopper.isUpperSensorTripped())&&trippitybop){ + // if (Robot.hopper.isUpperSensorTripped(){//||trippitybop){ + timer.start(); + // trippitybop = true; + Robot.hopper.setHopper(Constants.HOPPER_LOADING_HORIZONTAL_OUTPUT, Constants.HOPPER_LOADING_VERTICAL_OUTPUT); + SmartDashboard.putBoolean("Timer Running", true); + if(timer.get()>=0){ + Robot.hopper.setHopper(0,0); + trippitybop = false; + SmartDashboard.putBoolean("Timer Running", false); + } + // } + + } + } + + } + + + + // Make this return true when this Command no longer needs to run execute() + @Override + + protected boolean isFinished() { + // if (Robot.hopper.isHopperFull()==true){ + // return true; + // } + // else{ + // return false; + // } + return false; + + } + + // Called once after isFinished returns true + @Override + protected void end() { + // count++; + Robot.hopper.setHopper(0, 0); + // SmartDashboard.putBoolean("Ir Hopper Running", false); + // Robot.hopper.counter.reset(); + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + } + +} diff --git a/src/main/java/frc/robot/commands/DefaultExtension.java b/src/main/java/frc/robot/commands/IntakeToHopper/UnjamHopper.java similarity index 75% rename from src/main/java/frc/robot/commands/DefaultExtension.java rename to src/main/java/frc/robot/commands/IntakeToHopper/UnjamHopper.java index b3b3c1b..11f691f 100644 --- a/src/main/java/frc/robot/commands/DefaultExtension.java +++ b/src/main/java/frc/robot/commands/IntakeToHopper/UnjamHopper.java @@ -5,15 +5,16 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc.robot.commands; -import frc.robot.Robot; -import edu.wpi.first.wpilibj.command.Command; +package frc.robot.commands.IntakeToHopper; -public class DefaultExtension extends Command { - public DefaultExtension() { - super("DefaultExtension"); +import edu.wpi.first.wpilibj.command.Command; +import frc.robot.Constants; +import frc.robot.Robot; - requires(Robot.panelMech); +public class UnjamHopper extends Command { + public UnjamHopper() { + super("UnjamHopper"); + requires(Robot.hopper); // Use requires() here to declare subsystem dependencies // eg. requires(chassis); } @@ -26,19 +27,19 @@ protected void initialize() { // Called repeatedly when this Command is scheduled to run @Override protected void execute() { - Robot.panelMech.extendPanelMech(false); - + Robot.hopper.setHopper(-Constants.HOPPER_LOADING_HORIZONTAL_OUTPUT, -Constants.HOPPER_LOADING_VERTICAL_OUTPUT); } // Make this return true when this Command no longer needs to run execute() @Override protected boolean isFinished() { - return false; + return !Robot.hopper.checkJammed() || Robot.hopper.isBall(); } // Called once after isFinished returns true @Override protected void end() { + Robot.hopper.setHopper(0, 0); } // Called when another command which requires one or more of the same diff --git a/src/main/java/frc/robot/commands/MoveDrivetrain.java b/src/main/java/frc/robot/commands/MoveDrivetrain.java index 9f64dbd..5c8ffc0 100644 --- a/src/main/java/frc/robot/commands/MoveDrivetrain.java +++ b/src/main/java/frc/robot/commands/MoveDrivetrain.java @@ -27,7 +27,7 @@ protected void initialize() { // Called repeatedly when this Command is scheduled to run @Override protected void execute() { - Robot.drivetrain.setMotors(Constants.DRIVETRAIN_OUTPUT); + Robot.drivetrain.setMotors(Constants.DRIVETRAIN_OUTPUT,-Constants.DRIVETRAIN_OUTPUT); } // Make this return true when this Command no longer needs to run execute() diff --git a/src/main/java/frc/robot/commands/DefaultPanelMech.java b/src/main/java/frc/robot/commands/PanelMech/DefaultPanelMech.java similarity index 91% rename from src/main/java/frc/robot/commands/DefaultPanelMech.java rename to src/main/java/frc/robot/commands/PanelMech/DefaultPanelMech.java index 67d26f6..e81e8b3 100644 --- a/src/main/java/frc/robot/commands/DefaultPanelMech.java +++ b/src/main/java/frc/robot/commands/PanelMech/DefaultPanelMech.java @@ -5,9 +5,10 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc.robot.commands; +package frc.robot.commands.PanelMech; import edu.wpi.first.wpilibj.command.Command; +import frc.robot.Robot; public class DefaultPanelMech extends Command { public DefaultPanelMech() { @@ -23,12 +24,13 @@ protected void initialize() { // Called repeatedly when this Command is scheduled to run @Override protected void execute() { + Robot.panelMech.retractPanelMech(); } // Make this return true when this Command no longer needs to run execute() @Override protected boolean isFinished() { - return false; + return true; } // Called once after isFinished returns true diff --git a/src/main/java/frc/robot/commands/ExtendPanelMech.java b/src/main/java/frc/robot/commands/PanelMech/ExtendPanelMech.java similarity index 82% rename from src/main/java/frc/robot/commands/ExtendPanelMech.java rename to src/main/java/frc/robot/commands/PanelMech/ExtendPanelMech.java index 531f6aa..cb26d7e 100644 --- a/src/main/java/frc/robot/commands/ExtendPanelMech.java +++ b/src/main/java/frc/robot/commands/PanelMech/ExtendPanelMech.java @@ -5,8 +5,11 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc.robot.commands; +package frc.robot.commands.PanelMech; + import frc.robot.Robot; +import frc.robot.util.Camera_Switch.CameraSwitch; +import edu.wpi.first.wpilibj.Relay; import edu.wpi.first.wpilibj.command.Command; public class ExtendPanelMech extends Command { @@ -21,19 +24,22 @@ public ExtendPanelMech() { // Called just before this Command runs the first time @Override protected void initialize() { + Robot.Cam_switch.setDirection(Relay.Direction.kBoth); + } // Called repeatedly when this Command is scheduled to run @Override protected void execute() { - Robot.panelMech.extendPanelMech(true); + Robot.Cam_switch.select(CameraSwitch.kcamera1); + Robot.panelMech.extendPanelMech(); } // Make this return true when this Command no longer needs to run execute() @Override protected boolean isFinished() { - return false; + return true; } // Called once after isFinished returns true diff --git a/src/main/java/frc/robot/commands/PanelMech/SetPanelMechFast.java b/src/main/java/frc/robot/commands/PanelMech/SetPanelMechFast.java new file mode 100644 index 0000000..2191f1b --- /dev/null +++ b/src/main/java/frc/robot/commands/PanelMech/SetPanelMechFast.java @@ -0,0 +1,48 @@ +package frc.robot.commands.PanelMech; + +import edu.wpi.first.wpilibj.command.Command; +import frc.robot.Constants; +import frc.robot.Robot; + +public class SetPanelMechFast extends Command { + double count = 1; + public SetPanelMechFast() { + super("SetPanelMechFast"); + requires(Robot.panelMech); + + } + + //Called just before this Command runs the first time + @Override + protected void initialize() { + Robot.panelMech.extendPanelMech(); + } + + + + //Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + Robot.panelMech.setPanelMech(Constants.PANEL_MECH_FAST); + } + + //Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + return true; + } + + //Called once after isFinished returns true + @Override + protected void end() { + // Robot.panelMech.setPanelMech(0); + } + + //Called when another command which requires one or more of the same + //subsystems is scheduled to run + + @Override + protected void interrupted() { + + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/PanelMech/SetPanelMechSlow.java b/src/main/java/frc/robot/commands/PanelMech/SetPanelMechSlow.java new file mode 100644 index 0000000..3c42a58 --- /dev/null +++ b/src/main/java/frc/robot/commands/PanelMech/SetPanelMechSlow.java @@ -0,0 +1,48 @@ +package frc.robot.commands.PanelMech; + +import edu.wpi.first.wpilibj.command.Command; +import frc.robot.Constants; +import frc.robot.Robot; + +public class SetPanelMechSlow extends Command { + double count = 1; + public SetPanelMechSlow() { + super("SetPanelMechSlow"); + requires(Robot.panelMech); + + } + + //Called just before this Command runs the first time + @Override + protected void initialize() { + Robot.panelMech.extendPanelMech(); + } + + + + //Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + Robot.panelMech.setPanelMech(Constants.PANEL_MECH_CREEP); + } + + //Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + return true; + } + + //Called once after isFinished returns true + @Override + protected void end() { + // Robot.panelMech.setPanelMech(0); + } + + //Called when another command which requires one or more of the same + //subsystems is scheduled to run + + @Override + protected void interrupted() { + + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/PanelMech/StopPanelMech.java b/src/main/java/frc/robot/commands/PanelMech/StopPanelMech.java new file mode 100644 index 0000000..6168b08 --- /dev/null +++ b/src/main/java/frc/robot/commands/PanelMech/StopPanelMech.java @@ -0,0 +1,48 @@ +package frc.robot.commands.PanelMech; + +import edu.wpi.first.wpilibj.command.Command; +import frc.robot.Constants; +import frc.robot.Robot; + +public class StopPanelMech extends Command { + double count = 1; + public StopPanelMech() { + super("StopPanelMech"); + requires(Robot.panelMech); + + } + + //Called just before this Command runs the first time + @Override + protected void initialize() { + + } + + + + //Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + Robot.panelMech.setPanelMech(0); + } + + //Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + return true; + } + + //Called once after isFinished returns true + @Override + protected void end() { + Robot.panelMech.setPanelMech(0); + } + + //Called when another command which requires one or more of the same + //subsystems is scheduled to run + + @Override + protected void interrupted() { + + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/RamseteCommand.java b/src/main/java/frc/robot/commands/RamseteCommand.java new file mode 100644 index 0000000..4ea4866 --- /dev/null +++ b/src/main/java/frc/robot/commands/RamseteCommand.java @@ -0,0 +1,5 @@ +package frc.robot.commands; + +public class RamseteCommand { + +} diff --git a/src/main/java/frc/robot/commands/Shooter/CloseHopperPiston.java b/src/main/java/frc/robot/commands/Shooter/CloseHopperPiston.java new file mode 100644 index 0000000..71f63bb --- /dev/null +++ b/src/main/java/frc/robot/commands/Shooter/CloseHopperPiston.java @@ -0,0 +1,40 @@ +package frc.robot.commands.Shooter; + +import edu.wpi.first.wpilibj.command.Command; +import frc.robot.Constants; +import frc.robot.Robot; +// import frc.robot.subsystems.Shooter; + +//in theory this just chanes whether the hopper piston is retracted or not and then stops +public class CloseHopperPiston extends Command{ + private boolean retracted; + public CloseHopperPiston(){ + super("CloseHopperPiston"); + requires(Robot.hopper); + } + + @Override + protected void initialize(){ + retracted = Robot.hopper.getRetracted(); + } + + @Override + protected void execute() { + Robot.hopper.setRetracted(); + } + + @Override + + protected boolean isFinished(){ + return true; + } + @Override + protected void end() { + + } + + @Override + + protected void interrupted(){ + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/Shooter/LongShot.java b/src/main/java/frc/robot/commands/Shooter/LongShot.java new file mode 100644 index 0000000..9e1551b --- /dev/null +++ b/src/main/java/frc/robot/commands/Shooter/LongShot.java @@ -0,0 +1,60 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc.robot.commands.Shooter; + +import edu.wpi.first.wpilibj.command.Command; +import frc.robot.Constants; +import frc.robot.Robot; + +public class LongShot extends Command { + public LongShot() { + super("LongShot"); + requires(Robot.shooter); + + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + Robot.shooter.setShooterPID(-2791); + //-3100 good for comp bot + } + + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + return true; + } + // if(Robot.shooter.checkWheelSpeed_Long() == true) { + // return true; + // } + + // return false; + // } + + // Called once after isFinished returns true + @Override + protected void end() { + // Robot.shooter.setShooter(0); + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + } +} diff --git a/src/main/java/frc/robot/commands/StopShooter.java b/src/main/java/frc/robot/commands/Shooter/LongShotHood.java similarity index 77% rename from src/main/java/frc/robot/commands/StopShooter.java rename to src/main/java/frc/robot/commands/Shooter/LongShotHood.java index 714ce9c..c1529f8 100644 --- a/src/main/java/frc/robot/commands/StopShooter.java +++ b/src/main/java/frc/robot/commands/Shooter/LongShotHood.java @@ -5,15 +5,17 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc.robot.commands; +package frc.robot.commands.Shooter; import edu.wpi.first.wpilibj.command.Command; +import frc.robot.Constants; import frc.robot.Robot; -public class StopShooter extends Command { - public StopShooter() { - super("StopShooter"); +public class LongShotHood extends Command { + public LongShotHood() { + super("LongShotHood"); requires(Robot.shooter); + // Use requires() here to declare subsystem dependencies // eg. requires(chassis); } @@ -21,24 +23,30 @@ public StopShooter() { // Called just before this Command runs the first time @Override protected void initialize() { + } // Called repeatedly when this Command is scheduled to run @Override protected void execute() { - Robot.shooter.setShooter(0); + Robot.shooter.setHood1(true); + } // Make this return true when this Command no longer needs to run execute() @Override protected boolean isFinished() { - return false; - } + return true; + // if(Robot.shooter.getHood1() == false) { + // return true; + // } + // return false; + } // Called once after isFinished returns true @Override protected void end() { - Robot.shooter.setShooter(0); + // Robot.shooter.setHood1(0); } // Called when another command which requires one or more of the same diff --git a/src/main/java/frc/robot/commands/MoveShooter.java b/src/main/java/frc/robot/commands/Shooter/MoveShooter.java similarity index 97% rename from src/main/java/frc/robot/commands/MoveShooter.java rename to src/main/java/frc/robot/commands/Shooter/MoveShooter.java index d479b14..d9b31a2 100644 --- a/src/main/java/frc/robot/commands/MoveShooter.java +++ b/src/main/java/frc/robot/commands/Shooter/MoveShooter.java @@ -5,7 +5,7 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc.robot.commands; +package frc.robot.commands.Shooter; import edu.wpi.first.wpilibj.command.Command; import frc.robot.Constants; diff --git a/src/main/java/frc/robot/commands/Shooter/MoveShooterPassive.java b/src/main/java/frc/robot/commands/Shooter/MoveShooterPassive.java new file mode 100644 index 0000000..a72c83f --- /dev/null +++ b/src/main/java/frc/robot/commands/Shooter/MoveShooterPassive.java @@ -0,0 +1,52 @@ +// /*----------------------------------------------------------------------------*/ +// /* Copyright (c) 2018 FIRST. All Rights Reserved. */ +// /* Open Source Software - may be modified and shared by FRC teams. The code */ +// /* must be accompanied by the FIRST BSD license file in the root directory of */ +// /* the project. */ +// /*----------------------------------------------------------------------------*/ + +// package frc.robot.commands; + +// import edu.wpi.first.wpilibj.command.Command; +// import frc.robot.Constants; +// import frc.robot.Robot; + +// public class MoveShooterPassive extends Command { +// public MoveShooterPassive() { +// super("MoveShooterPassive"); +// requires(Robot.shooter); + +// // Use requires() here to declare subsystem dependencies +// // eg. requires(chassis); +// } + +// // Called just before this Command runs the first time +// @Override +// protected void initialize() { + +// } + +// // Called repeatedly when this Command is scheduled to run +// @Override +// protected void execute() { +// Robot.shooter.setShooter(Constants.SHOOTER_OUTPUT_PASSIVE); +// } + +// // Make this return true when this Command no longer needs to run execute() +// @Override +// protected boolean isFinished() { +// return false; +// } + +// // Called once after isFinished returns true +// @Override +// protected void end() { +// Robot.shooter.setShooter(0); +// } + +// // Called when another command which requires one or more of the same +// // subsystems is scheduled to run +// @Override +// protected void interrupted() { +// } +// } diff --git a/src/main/java/frc/robot/commands/Shooter/OpenHopperPiston.java b/src/main/java/frc/robot/commands/Shooter/OpenHopperPiston.java new file mode 100644 index 0000000..6e7fe8e --- /dev/null +++ b/src/main/java/frc/robot/commands/Shooter/OpenHopperPiston.java @@ -0,0 +1,38 @@ +package frc.robot.commands.Shooter; + +import edu.wpi.first.wpilibj.command.Command; +import frc.robot.Constants; +import frc.robot.Robot; +// import frc.robot.subsystems.Shooter; + +//in theory this just chanes whether the hopper piston is retracted or not and then stops +public class OpenHopperPiston extends Command{ + private boolean retracted; + public OpenHopperPiston(){ + super("OpenHopperPiston"); + requires(Robot.hopper); + } + + @Override + protected void initialize(){ + } + + @Override + protected void execute() { + Robot.hopper.setExtended(); + } + + @Override + protected boolean isFinished(){ + return true; + } + @Override + protected void end() { + + } + + @Override + + protected void interrupted(){ + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/Shooter/Shoot.java b/src/main/java/frc/robot/commands/Shooter/Shoot.java new file mode 100644 index 0000000..29706dc --- /dev/null +++ b/src/main/java/frc/robot/commands/Shooter/Shoot.java @@ -0,0 +1,51 @@ +// package frc.robot.commands.Shooter; + +// import edu.wpi.first.wpilibj.command.Command; +// import frc.robot.Constants; +// import frc.robot.Robot; +// import frc.robot.commands.MoveHopper; +// import frc.robot.subsystems.Shooter; + +// public class Shoot extends Command{ +// private double wheelSpeed; +// private boolean goodSpeed; +// public Shoot(){ +// super("Shoot"); +// requires(Robot.shooter); +// requires(Robot.hopper); +// } + +// @Override + +// protected void initialize(){ + +// } + +// @Override + +// protected void execute() { +// Robot.shooter.setHood1(false); //change to true??? +// //new CheckWheelSpeed(); +// new MoveHopper(); +// Robot.hopper.setRetracted(Robot.hopper.isRetracted()); //define isRetracted as true or false??? + +// } + +// @Override + +// protected boolean isFinished(){ +// return false; +// } +// @Override + +// protected void end() { +// Robot.shooter.setShooter(0); +// Robot.hopper.setHopper(0); +// Robot.hopper.setRetracted(Robot.hopper.isRetracted()); //if yes above, change here as well +// } + +// @Override + +// protected void interrupted(){ +// } +// } \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/Shooter/StopShooter.java b/src/main/java/frc/robot/commands/Shooter/StopShooter.java new file mode 100644 index 0000000..fbde93e --- /dev/null +++ b/src/main/java/frc/robot/commands/Shooter/StopShooter.java @@ -0,0 +1,37 @@ +package frc.robot.commands.Shooter; + +import edu.wpi.first.wpilibj.command.Command; +import frc.robot.Constants; +import frc.robot.Robot; +import frc.robot.subsystems.Shooter; + +//in theory this just chanes whether the hopper piston is retracted or not and then stops +public class StopShooter extends Command{ + private boolean retracted; + public StopShooter(){ + super("StopShooter"); + requires(Robot.shooter); + } + + @Override + protected void initialize(){ + } + + @Override + protected void execute() { + Robot.shooter.setShooter(0); + } + + @Override + protected boolean isFinished(){ + return true; + } + @Override + protected void end() { + } + + @Override + + protected void interrupted(){ + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/Shooter/WallShot.java b/src/main/java/frc/robot/commands/Shooter/WallShot.java new file mode 100644 index 0000000..f160958 --- /dev/null +++ b/src/main/java/frc/robot/commands/Shooter/WallShot.java @@ -0,0 +1,58 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc.robot.commands.Shooter; + +import edu.wpi.first.wpilibj.command.Command; +import frc.robot.Constants; +import frc.robot.Robot; + +public class WallShot extends Command { + public WallShot() { + super("WallShot"); + requires(Robot.shooter); + + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + + Robot.shooter.setShooterPID(-1300); //Utilize PIDs for this (and Longshot as well) + + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + // if(Robot.shooter.checkWheelSpeed_Wall()) { + // return true; + // } + // return false; + return true; + } + + // Called once after isFinished returns true + @Override + protected void end() { + // Robot.shooter.setShooter(0); + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + } +} diff --git a/src/main/java/frc/robot/commands/MoveShooterPassive.java b/src/main/java/frc/robot/commands/Shooter/WallShotHood.java similarity index 80% rename from src/main/java/frc/robot/commands/MoveShooterPassive.java rename to src/main/java/frc/robot/commands/Shooter/WallShotHood.java index d7eb505..7cfdd1e 100644 --- a/src/main/java/frc/robot/commands/MoveShooterPassive.java +++ b/src/main/java/frc/robot/commands/Shooter/WallShotHood.java @@ -5,15 +5,15 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc.robot.commands; +package frc.robot.commands.Shooter; import edu.wpi.first.wpilibj.command.Command; import frc.robot.Constants; import frc.robot.Robot; -public class MoveShooterPassive extends Command { - public MoveShooterPassive() { - super("MoveShooterPassive"); +public class WallShotHood extends Command { + public WallShotHood() { + super("WallShotHood"); requires(Robot.shooter); // Use requires() here to declare subsystem dependencies @@ -29,19 +29,23 @@ protected void initialize() { // Called repeatedly when this Command is scheduled to run @Override protected void execute() { - Robot.shooter.setShooter(Constants.SHOOTER_OUTPUT_PASSIVE); + Robot.shooter.setHood1(false); } // Make this return true when this Command no longer needs to run execute() @Override protected boolean isFinished() { - return false; + // if(Robot.shooter.getHood1()) { + // return true; + // } + // return false; + return true; } // Called once after isFinished returns true @Override protected void end() { - Robot.shooter.setShooter(0); + // Robot.shooter.setHood1(0); } // Called when another command which requires one or more of the same diff --git a/src/main/java/frc/robot/commands/StopDrivetrain.java b/src/main/java/frc/robot/commands/StopDrivetrain.java index 5c430a6..f4d0066 100644 --- a/src/main/java/frc/robot/commands/StopDrivetrain.java +++ b/src/main/java/frc/robot/commands/StopDrivetrain.java @@ -8,7 +8,6 @@ package frc.robot.commands; import edu.wpi.first.wpilibj.command.Command; -import frc.robot.Constants; import frc.robot.Robot; public class StopDrivetrain extends Command { @@ -27,7 +26,7 @@ protected void initialize() { // Called repeatedly when this Command is scheduled to run @Override protected void execute() { - Robot.drivetrain.setMotors(0); + Robot.drivetrain.setMotors(0,0); } // Make this return true when this Command no longer needs to run execute() @@ -39,7 +38,7 @@ protected boolean isFinished() { // Called once after isFinished returns true @Override protected void end() { - Robot.drivetrain.setMotors(0); + Robot.drivetrain.setMotors(0,0); } // Called when another command which requires one or more of the same diff --git a/src/main/java/frc/robot/commands/setCameraOne.java b/src/main/java/frc/robot/commands/setCameraOne.java index 9ddbd09..a98c7a3 100644 --- a/src/main/java/frc/robot/commands/setCameraOne.java +++ b/src/main/java/frc/robot/commands/setCameraOne.java @@ -8,6 +8,9 @@ package frc.robot.commands; import edu.wpi.first.wpilibj.command.Command; +import frc.robot.Robot; +// import frc.robot.subsystems.Camera; +import frc.robot.util.Camera_Switch.CameraSwitch; public class setCameraOne extends Command { public setCameraOne() { @@ -23,12 +26,13 @@ protected void initialize() { // Called repeatedly when this Command is scheduled to run @Override protected void execute() { + Robot.Cam_switch.select(CameraSwitch.kcamera1); } // Make this return true when this Command no longer needs to run execute() @Override protected boolean isFinished() { - return false; + return true; } // Called once after isFinished returns true @@ -41,4 +45,6 @@ protected void end() { @Override protected void interrupted() { } + + } diff --git a/src/main/java/frc/robot/commands/StopElevator.java b/src/main/java/frc/robot/commands/setCameraThree.java similarity index 84% rename from src/main/java/frc/robot/commands/StopElevator.java rename to src/main/java/frc/robot/commands/setCameraThree.java index b7581f1..a0cff0b 100644 --- a/src/main/java/frc/robot/commands/StopElevator.java +++ b/src/main/java/frc/robot/commands/setCameraThree.java @@ -9,11 +9,11 @@ import edu.wpi.first.wpilibj.command.Command; import frc.robot.Robot; +//import frc.robot.subsystems.Camera; +import frc.robot.util.Camera_Switch.CameraSwitch; -public class StopElevator extends Command { - public StopElevator() { - super("StopElevator"); - requires(Robot.elevator); +public class setCameraThree extends Command { + public setCameraThree() { // Use requires() here to declare subsystem dependencies // eg. requires(chassis); } @@ -26,19 +26,18 @@ protected void initialize() { // Called repeatedly when this Command is scheduled to run @Override protected void execute() { - Robot.elevator.setElevator(0); + Robot.Cam_switch.select(CameraSwitch.kcamera3); } // Make this return true when this Command no longer needs to run execute() @Override protected boolean isFinished() { - return false; + return true; } // Called once after isFinished returns true @Override protected void end() { - Robot.elevator.setElevator(0); } // Called when another command which requires one or more of the same diff --git a/src/main/java/frc/robot/commands/setCameraTwo.java b/src/main/java/frc/robot/commands/setCameraTwo.java index cff84d2..078b2ad 100644 --- a/src/main/java/frc/robot/commands/setCameraTwo.java +++ b/src/main/java/frc/robot/commands/setCameraTwo.java @@ -8,6 +8,9 @@ package frc.robot.commands; import edu.wpi.first.wpilibj.command.Command; +import frc.robot.Robot; +//import frc.robot.subsystems.Camera; +import frc.robot.util.Camera_Switch.CameraSwitch; public class setCameraTwo extends Command { public setCameraTwo() { @@ -23,12 +26,13 @@ protected void initialize() { // Called repeatedly when this Command is scheduled to run @Override protected void execute() { + Robot.Cam_switch.select(CameraSwitch.kcamera2); } // Make this return true when this Command no longer needs to run execute() @Override protected boolean isFinished() { - return false; + return true; } // Called once after isFinished returns true diff --git a/src/main/java/frc/robot/commands/SetPanelMech.java b/src/main/java/frc/robot/commands/tempTest/testExtendClimber.java similarity index 51% rename from src/main/java/frc/robot/commands/SetPanelMech.java rename to src/main/java/frc/robot/commands/tempTest/testExtendClimber.java index cd8318b..cd502f3 100644 --- a/src/main/java/frc/robot/commands/SetPanelMech.java +++ b/src/main/java/frc/robot/commands/tempTest/testExtendClimber.java @@ -1,20 +1,14 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc.robot.commands; +package frc.robot.commands.tempTest; import edu.wpi.first.wpilibj.command.Command; import frc.robot.Constants; import frc.robot.Robot; -public class SetPanelMech extends Command { - public SetPanelMech() { - super("SetPanelMech"); - requires(Robot.panelMech); +public class testExtendClimber extends Command { + public testExtendClimber() { + super("testExtendClimber"); + requires(Robot.manipulator); + requires(Robot.hopper); //Use requires() here to declare subsystem dependencies //eg. requires(chassis); } @@ -28,20 +22,19 @@ protected void initialize() { //Called repeatedly when this Command is scheduled to run @Override protected void execute() { - Robot.panelMech.setPanelMech(0); - // Robot.panelMech.setManipulator(0); + Robot.climber.setExtended(); + } //Make this return true when this Command no longer needs to run execute() @Override protected boolean isFinished() { - return false; + return true; } //Called once after isFinished returns true @Override protected void end() { - Robot.panelMech.setPanelMech(0); } //Called when another command which requires one or more of the same diff --git a/src/main/java/frc/robot/commands/tempTest/testManipulator.java b/src/main/java/frc/robot/commands/tempTest/testManipulator.java new file mode 100644 index 0000000..44cb65b --- /dev/null +++ b/src/main/java/frc/robot/commands/tempTest/testManipulator.java @@ -0,0 +1,45 @@ +package frc.robot.commands.tempTest; + +import edu.wpi.first.wpilibj.command.Command; +import frc.robot.Constants; +import frc.robot.Robot; + +public class testManipulator extends Command { + public testManipulator() { + super("testManipulator"); + requires(Robot.manipulator); + } + + //Called just before this Command runs the first time + @Override + protected void initialize() { + + } + + //Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + Robot.manipulator.setExtended(); + + } + + //Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + return true; + } + + //Called once after isFinished returns true + @Override + protected void end() { + // Robot.manipulator.setManipulator(0); + } + + //Called when another command which requires one or more of the same + //subsystems is scheduled to run + + @Override + protected void interrupted() { + //Yes + } +} diff --git a/src/main/java/frc/robot/commands/tempTest/testRetractClimber.java b/src/main/java/frc/robot/commands/tempTest/testRetractClimber.java new file mode 100644 index 0000000..ad486c7 --- /dev/null +++ b/src/main/java/frc/robot/commands/tempTest/testRetractClimber.java @@ -0,0 +1,44 @@ +package frc.robot.commands.tempTest; + +import edu.wpi.first.wpilibj.command.Command; +import frc.robot.Constants; +import frc.robot.Robot; + +public class testRetractClimber extends Command { + public testRetractClimber() { + super("testRetractClimber"); + requires(Robot.climber); + } + + //Called just before this Command runs the first time + @Override + protected void initialize() { + + } + + //Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + Robot.climber.setRetracted(); + + } + + //Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + return true; + } + + //Called once after isFinished returns true + @Override + protected void end() { + } + + //Called when another command which requires one or more of the same + //subsystems is scheduled to run + + @Override + protected void interrupted() { + //Yes + } +} diff --git a/src/main/java/frc/robot/commands/tempTest/testStopManipulator.java b/src/main/java/frc/robot/commands/tempTest/testStopManipulator.java new file mode 100644 index 0000000..2bdce40 --- /dev/null +++ b/src/main/java/frc/robot/commands/tempTest/testStopManipulator.java @@ -0,0 +1,46 @@ +package frc.robot.commands.tempTest; + +import edu.wpi.first.wpilibj.command.Command; +import frc.robot.Constants; +import frc.robot.Robot; + +public class testStopManipulator extends Command { + public testStopManipulator() { + super("testStopManipulator"); + requires(Robot.manipulator); + //Use requires() here to declare subsystem dependencies + //eg. requires(chassis); + } + + //Called just before this Command runs the first time + @Override + protected void initialize() { + + } + + //Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + Robot.manipulator.setRetracted(); + } + + //Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + return true; + } + + //Called once after isFinished returns true + @Override + protected void end() { + // Robot.manipulator.setManipulator(0); + } + + //Called when another command which requires one or more of the same + //subsystems is scheduled to run + + @Override + protected void interrupted() { + //Yes + } +} diff --git a/src/main/java/frc/robot/subsystems/Camera.java b/src/main/java/frc/robot/subsystems/Camera.java new file mode 100644 index 0000000..88f14a4 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Camera.java @@ -0,0 +1,22 @@ +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj.command.Subsystem; +import frc.robot.util.Camera_Switch.*; + +public class Camera extends Subsystem{ + public CameraSwitch cam_switch; + public Camera() { + + //Initialize Cameron Switch + cam_switch = new CameraSwitch(0, 1); + } + public void initDefaultCommand() { + + } + public void setCameraNum(int camNum) { + cam_switch.select(camNum); + } + + public void debug (){} + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java index 2a08628..0f576b8 100644 --- a/src/main/java/frc/robot/subsystems/Climber.java +++ b/src/main/java/frc/robot/subsystems/Climber.java @@ -9,19 +9,20 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.command.Subsystem; - +import edu.wpi.first.wpilibj.interfaces.Gyro; import frc.robot.Constants; import frc.robot.RobotMap; import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMax.IdleMode; import com.revrobotics.CANSparkMaxLowLevel.MotorType; import edu.wpi.first.wpilibj.Solenoid; - +import edu.wpi.first.wpilibj.buttons.Button; import edu.wpi.first.wpilibj.GyroBase; -import frc.robot.commands.*; +import edu.wpi.first.wpilibj.Joystick; public class Climber extends Subsystem { private CANSparkMax winch_Neo; - private CANSparkMax selfClimb_Neo; + // private CANSparkMax selfClimb_Neo; public static double bottom_penetrometer; public static double top_penetrometer; @@ -29,86 +30,85 @@ public class Climber extends Subsystem { private Solenoid Extender; - private GyroBase climbGyro; + public GyroBase climbGyro; public double angle; - + private Gyro gyro; + + public static Joystick driverStick; + private Button driverA, driverB, driverY, driverX; + public Climber(){ winch_Neo = new CANSparkMax(RobotMap.WINCH_NEO, MotorType.kBrushless); Extender = new Solenoid(RobotMap.kPCM, RobotMap.CLIMB_SOLENOID); - } - // selfClimb_Neo= new CANSparkMax(RobotMap.SELFCLIMB_NEO, MotorType.kBrushless); - // climbGyro = new GyroBase(){ - - // @Override - // public void close() throws Exception { - // // TODO Auto-generated method stub - - // } - - // @Override - // public void reset() { - // // TODO Auto-generated method stub - - // } - - // @Override - // public double getRate() { - // // TODO Auto-generated method stub - // return 0; - // } - - // @Override - // public double getAngle() { - // // TODO Auto-generated method stub - // return 0; - // } - - // @Override - // public void calibrate() { - // // TODO Auto-generated method stub - - // } - // }; - // } + // selfClimb_Neo= new CANSparkMax(RobotMap.SELFCLIMB_NEO, MotorType.kBrushless); + driverStick = new Joystick(0); + winch_Neo.setIdleMode(IdleMode.kBrake); + + } + public void initDefaultCommand() { - // setDefaultCommand(new DefaultExtension()); + // setDefaultCommand(new DefaultExtension()); } - - public void setPinExtender(boolean extended){ - Extender.set(extended); - } + public void setPinExtender(boolean extend){ + Extender.set(extend); + } + + public void setRetracted(){ + Extender.set(false); + } - public boolean getPinExtender(){ - return Extender.get(); - } - public void setWinchOutput(double outputWinch){ - winch_Neo.set(outputWinch); - } + public void setExtended(){ + Extender.set(true); + } + public void setPinExtenderByButton(){ + // if(driverY.get()){ + // Extender.set(true); + // } + // else if(driverB.get()){ + // Extender.set(false); + // } + } - // public void calibrateGyro(){ - // climbGyro.calibrate(); - // } + public boolean getPinExtender(){ + return Extender.get(); + } + + public void setWinchOutput(double outputWinch){ + winch_Neo.set(outputWinch); + } - // public void setSelfClimbOutput(double outputSelf){ - // selfClimb_Neo.set(outputSelf); - // } + public void setWinchOutputByButton(){ + // if (driverA.get()){ + // winch_Neo.set(1); + // } + // else if (driverX.get()){ + // winch_Neo.set(-1); + // } + // else { + // winch_Neo.set(0); + // } + } - // public double getGyroAngle() { - // return climbGyro.getAngle(); - // } - - public void debug(){ - SmartDashboard.putBoolean("Pin Extender Status - ", getPinExtender()); - // SmartDashboard.putNumber("Gyro angle", climbGyro.getAngle()); - } + public void calibrateGyro(){ + climbGyro.calibrate(); + } + + public void setSelfClimbOutput(double outputSelf){ + // selfClimb_Neo.set(outputSelf); + } - + public double getGyroAngle() { + return climbGyro.getAngle(); + } - + public void debug(){ + SmartDashboard.putBoolean("Pin Extender Status - ", getPinExtender()); + //SmartDashboard.putNumber("Gyro angle", climbGyro.getAngle()); + } + // public void setSelfClimbOutput(double turn) { + // } } - - - \ No newline at end of file + \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index a49afaa..1bdbd8f 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -1,35 +1,231 @@ package frc.robot.subsystems; import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMax.IdleMode; import com.revrobotics.CANSparkMaxLowLevel.MotorType; -import com.ctre.phoenix.motorcontrol.ControlMode; -import com.ctre.phoenix.motorcontrol.can.TalonSRX; +import com.revrobotics.CANEncoder; +import com.revrobotics.AlternateEncoderType; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.command.Subsystem; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.Constants; +import frc.robot.Robot; import frc.robot.RobotMap; import frc.robot.util.Util; + public class Drivetrain extends Subsystem { - public CANSparkMax drivetrain_neo; + private CANSparkMax leftLeader; + private CANSparkMax rightLeader; + private CANSparkMax [] leftFollowers; + private CANSparkMax [] rightFollowers; + + private static final AlternateEncoderType kAltEncType = AlternateEncoderType.kQuadrature; + private CANEncoder m_leftAlternateEncoder; + private CANEncoder m_rightAlternateEncoder; + + public Timer driveTime; + + + public Drivetrain() { - drivetrain_neo = new CANSparkMax(RobotMap.DRIVETRAIN_NEO, MotorType.kBrushless); - drivetrain_neo.setOpenLoopRampRate(Constants.kNeoRampTime); + + // Init Left Leader + leftLeader = new CANSparkMax(RobotMap.kLeftLeader, MotorType.kBrushless); + leftLeader.setOpenLoopRampRate(Constants.kNeoRampTime); + leftLeader.setSmartCurrentLimit(60); + + + // Init Right Leader + rightLeader = new CANSparkMax(RobotMap.kRightLeader, MotorType.kBrushless); + rightLeader.setOpenLoopRampRate(Constants.kNeoRampTime); + rightLeader.setSmartCurrentLimit(60); + + // encoders + m_leftAlternateEncoder = leftLeader.getAlternateEncoder(kAltEncType, 1024); + m_rightAlternateEncoder = rightLeader.getAlternateEncoder(kAltEncType, 1024); + + // init left followers + leftFollowers = new CANSparkMax[RobotMap.kLeftFollowers.length]; + for(int i = 0; i < leftFollowers.length; ++i) { + leftFollowers[i] = new CANSparkMax(RobotMap.kLeftFollowers[i], MotorType.kBrushless); + leftFollowers[i].setOpenLoopRampRate(Constants.kNeoRampTime); + leftFollowers[i].follow(leftLeader); + } + + //init right followers + rightFollowers = new CANSparkMax[RobotMap.kRightFollowers.length]; + for(int i = 0; i < rightFollowers.length; ++i) { + rightFollowers[i] = new CANSparkMax(RobotMap.kRightFollowers[i], MotorType.kBrushless); + rightFollowers[i].setOpenLoopRampRate(Constants.kNeoRampTime); + rightFollowers[i].follow(rightLeader); + } + + driveTime = new Timer(); + setBrakeMode(true); } - public void initDefaultCommand() { + public void initDefaultCommand() {} + + + + public void setMotors(double left, double right) { + leftLeader.set(-left); + rightLeader.set(right); + } + + + public double getLeftMotor(){ + return m_leftAlternateEncoder.getVelocity(); + } + + + public double getRightMotor(){ + return m_rightAlternateEncoder.getVelocity(); + } + + + public double getRightPosition(){ + return m_leftAlternateEncoder.getPosition(); + } + + public double getLeftPosition(){ + return m_leftAlternateEncoder.getPosition(); + } + + + public double getLeftCPR(){ + return m_leftAlternateEncoder.getCountsPerRevolution(); + } + + public double getRightCPR(){ + return m_leftAlternateEncoder.getCountsPerRevolution(); + } + public double getAverageDistance(){ + return Util.average(getDistanceLeft(), getDistanceRight()); + } + + public double getDistanceLeft(){ + return Math.PI*Constants.wheelDiameterInches*(getLeftCPR()/1024)/12; // returns distance in feet + } + + public double getDistanceRight(){ + return Math.PI*Constants.wheelDiameterInches*(getLeftCPR()/1024)/12; // returns distance in feet + } + + public void setBrakeMode(boolean isbrake) { + IdleMode mode = isbrake ? IdleMode.kBrake : IdleMode.kCoast; + leftLeader.setIdleMode(mode); + rightLeader.setIdleMode(mode); + for(int i = 0; i < leftFollowers.length; ++i) { + leftFollowers[i].setIdleMode(mode); + } + + for(int i = 0; i < rightFollowers.length; ++i) { + rightFollowers[i].setIdleMode(mode); + } + + } + public void stopCompletely() { + leftLeader.set(0); + rightLeader.set(0); + } + + + public void BackupAndShoot(){ + SmartDashboard.putString("AUTO_LOADED", "BackupAndShoot"); + //driveTime.start(); + if(driveTime.get() < 1.85){ + setMotors(-0.5,-0.5); + Robot.shooter.setShooterPID(-3200); + } + if (driveTime.get() >= 1.85){ + Robot.shooter.setShooterPID(-3200); + Robot.shooter.setHood1(true); + Robot.hopper.setExtended(); + setMotors(0, 0); + if(driveTime.get() > 6.45){ + Robot.hopper.setHopper(Constants.HOPPER_AUTO_HORIZONTAL, Constants.HOPPER_AUTO_VERTICAL); + } + } + } + public void BackAndShoot2(){ + SmartDashboard.putString("AUTO_LOADED", "BackAndShoot2"); + //driveTime.start(); + if(driveTime.get() < 1.5){ + setMotors(-0.5,-0.5); + Robot.shooter.setShooterPID(-2900); } + if (driveTime.get() >= 1.5){ + Robot.shooter.setShooterPID(-2900); + Robot.shooter.setHood1(true); + Robot.hopper.setExtended(); + setMotors(0, 0); + if(driveTime.get() > 6.3){ + Robot.hopper.setHopper(Constants.HOPPER_AUTO_HORIZONTAL, Constants.HOPPER_AUTO_VERTICAL); + } + + } + - public void setMotors(final double left) { - drivetrain_neo.set(left); + + } + public void BackAndShoot3(){ + SmartDashboard.putString("AUTO_LOADED", "BackAndShoot3"); + //driveTime.start(); + if(driveTime.get() < 1.5){ + setMotors(-0.5,-0.5); + Robot.shooter.setShooterPID(-2900); + } + if (driveTime.get() >= 1.5){ + Robot.shooter.setShooterPID(-2900); + Robot.shooter.setHood1(true); + Robot.hopper.setExtended(); + setMotors(0, 0); + if(driveTime.get() > 6.3){ + Robot.hopper.setHopper(Constants.HOPPER_AUTO_HORIZONTAL, Constants.HOPPER_AUTO_VERTICAL); + } + } } + + + + public void driveBackAndShoot_angled(){ + SmartDashboard.putString("AUTO_LOADED", "DriveBackAndShoot_angled"); + + if(driveTime.get() < 2.25){ + setMotors(-0.5,-0.5); + Robot.shooter.setShooterPID(-2900); + } + if (driveTime.get() >= 2.25){ + setMotors(0, 0); + Robot.shooter.setShooterPID(-2900); + Robot.shooter.setHood1(true); + Robot.hopper.setExtended(); + if(driveTime.get() > 6.3){ + Robot.hopper.setHopper(Constants.HOPPER_AUTO_HORIZONTAL, Constants.HOPPER_AUTO_VERTICAL); + } + + } + + } + + public void debug() { + SmartDashboard.putNumber("Get Left Velocity", getLeftMotor()); + SmartDashboard.putNumber("Get Right Velocity", getRightMotor()); + SmartDashboard.putNumber("Get Right Position", getRightPosition()); + SmartDashboard.putNumber("Get Left Position", getLeftPosition()); + SmartDashboard.putNumber("Get Right CPR", getRightCPR()); + SmartDashboard.putNumber("Get Left CPR", getLeftCPR()); + SmartDashboard.putNumber("Drivetimer", driveTime.get()); } + } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index f882f0d..9d375e6 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -6,22 +6,13 @@ /*----------------------------------------------------------------------------*/ package frc.robot.subsystems; -import com.revrobotics.CANSparkMax; -import com.revrobotics.CANSparkMaxLowLevel.MotorType; import com.ctre.phoenix.motorcontrol.ControlMode; import com.ctre.phoenix.motorcontrol.can.TalonSRX; - - import edu.wpi.first.wpilibj.command.Subsystem; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - -import frc.robot.Constants; import frc.robot.RobotMap; -import frc.robot.util.Util; -import edu.wpi.first.wpilibj.command.Subsystem; - /** * Add your docs here. */ diff --git a/src/main/java/frc/robot/subsystems/Hopper.java b/src/main/java/frc/robot/subsystems/Hopper.java index 0e845d9..1e60c99 100644 --- a/src/main/java/frc/robot/subsystems/Hopper.java +++ b/src/main/java/frc/robot/subsystems/Hopper.java @@ -7,34 +7,142 @@ package frc.robot.subsystems; -import com.ctre.phoenix.motorcontrol.ControlMode; -import com.ctre.phoenix.motorcontrol.can.TalonSRX; +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMax.IdleMode; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; + +import frc.robot.Constants; +import edu.wpi.first.wpilibj.Solenoid; import edu.wpi.first.wpilibj.command.Subsystem; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.RobotMap; - +import frc.robot.util.IrSensor; +import edu.wpi.first.wpilibj.Counter; +import edu.wpi.first.wpilibj.DigitalInput;; /** * Add your docs here. */ public class Hopper extends Subsystem { - public TalonSRX hopper_talon; - + public CANSparkMax hopper_horizontal; + public CANSparkMax hopper_vertical; + public Solenoid hopper_stopper; + public IrSensor entrySensor, upperSensor; + public DigitalInput beamBreak1 = new DigitalInput(9);//Switched from Limit switch to bream break + public DigitalInput beamBreak2 = new DigitalInput(8); + public Counter counter= new Counter(beamBreak1); + public Hopper(){ - hopper_talon = new TalonSRX(RobotMap.HOPPER_TALON); + hopper_horizontal = new CANSparkMax(RobotMap.HORIZONTAL_HOPPER, MotorType.kBrushless); + hopper_vertical = new CANSparkMax(RobotMap.VERTICAL_HOPPER, MotorType.kBrushless); + hopper_stopper = new Solenoid(RobotMap.kPCM, RobotMap.HOPPER_SOLENOID); + entrySensor = new IrSensor(RobotMap.kPDP); + upperSensor = new IrSensor(2); + + hopper_horizontal.setIdleMode(IdleMode.kBrake); + hopper_vertical.setIdleMode(IdleMode.kBrake); + + hopper_horizontal.setSmartCurrentLimit(20); + hopper_vertical.setSmartCurrentLimit(20); + + counter.clearDownSource(); + } + + public void setHopper(final double output, final double vOutput){ + hopper_horizontal.set(output); + hopper_vertical.set(-vOutput); + } + // public double getHopperVoltage() { + // return hopper_neo.ge + // } + public boolean isRetracted() { + return !hopper_stopper.get(); + } + + public void setExtended() { + hopper_stopper.set(true); + } + + public void setRetracted() { + hopper_stopper.set(false); + } + + public boolean getRetracted() { + return hopper_stopper.get(); + } + + public int getIRSensor() { + return entrySensor.getValue(); } - public void setHopper(final double output){ - hopper_talon.set(ControlMode.PercentOutput, output); + public double getHorizontalCurrent() { + return hopper_horizontal.getOutputCurrent(); } - public double getHopperVoltage() { - return hopper_talon.getMotorOutputVoltage(); + + public double getVerticalCurrent() { + return hopper_vertical.getOutputCurrent(); + } + + public boolean checkJammed(){ + if(getHorizontalCurrent()>Constants.jamCurrent) + return true; + else + return false; + } + + public boolean isBall(){ + if(Constants.BALL_VALUE < getIRSensor()){ + return true; } + else{ + return false; + } + } + public boolean isSwitchSet() { + return counter.get() > 0; + } + public boolean isUpperSensorTripped() { + return 745 <= upperSensor.getValue(); + } - public void debug(){ - SmartDashboard.putNumber("Hopper Voltage - ", getHopperVoltage()); + public void loadingWithIR(){ + if(Constants.BALL_VALUE < getIRSensor()){ + + if (isUpperSensorTripped()) { + setHopper( + Constants.HOPPER_LOADING_HORIZONTAL_OUTPUT, + 0); + } else { + setHopper( + Constants.HOPPER_LOADING_HORIZONTAL_OUTPUT, + Constants.HOPPER_LOADING_VERTICAL_OUTPUT); + } + } + + else{ + setHopper(0, 0); + } + } + + public boolean isHopperFull(){ + if (counter.get()==5){ + return true; + } + return false; + } + public void debug(){ + //SmartDashboard.putNumber("Hopper Voltage - ", getHopperVoltage()); + SmartDashboard.putNumber("Entry IR Value", entrySensor.getValue()); + SmartDashboard.putBoolean("Got Balls?", isBall()); + SmartDashboard.putNumber("Horizontal Current", getHorizontalCurrent()); + SmartDashboard.putNumber("Vertical Current", getVerticalCurrent()); + SmartDashboard.putBoolean("Hopper Stopper Out", isRetracted()); + SmartDashboard.putNumber("Upper IR Value", upperSensor.getValue()); + SmartDashboard.putBoolean("Jammed", checkJammed()); + SmartDashboard.putNumber("Counter Value", counter.get()); + SmartDashboard.putBoolean("Beam Break", beamBreak1.get()); //Switched from Limit switch to bream break } @Override protected void initDefaultCommand() { diff --git a/src/main/java/frc/robot/subsystems/Limelight.java b/src/main/java/frc/robot/subsystems/Limelight.java new file mode 100644 index 0000000..f1e0588 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Limelight.java @@ -0,0 +1,290 @@ +package frc.robot.subsystems; + +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.command.Subsystem; + +public class Limelight extends Subsystem { + + private NetworkTable getTable() { + return NetworkTableInstance.getDefault().getTable("limelight"); + } + + private NetworkTableEntry getEntry(String entryName) { + return getTable().getEntry(entryName); + } + + private double getValue(String entryName) { + return getEntry(entryName).getDouble(0); + } + + private void setValue(String entryName, double value) { + getEntry(entryName).setNumber(value); + } + + /** @return true if the Limelight has any valid targets */ + public boolean hasValidTargets() { + return getValue("tv") == 1; + } + + public double getTargetValue() { + return getValue("tv"); + } + + /** @return horizontal offset from crosshair to target (-27 degrees to 27 degrees) */ + public double getHorizontalOffset() { + return getValue("tx"); + } + + /** @return vertical offset from crosshair to target (-20.5 degrees to 20.5 degrees) */ + public double getVerticalOffset() { + return getValue("ty"); + } + + /** @return target area (0% of image to 100% of image) */ + public double getTargetArea() { + return getValue("ta"); + } + + /** @return skew or rotation (-90 degrees to 0 degrees) */ + public double getSkewOrRotation() { + return getValue("ts"); + } + + /** @return pipeline's latency contribution (ms); add 11ms for image capture */ + public double getLatency() { + return getValue("tl"); + } + + /** @return sidelength of shortest side of the fitted bounding box (pixels) */ + public double getShortSideLength() { + return getValue("tshort"); + } + + /** @return sidelength of longest side of the fitted bounding box (pixels) */ + public double getLongSideLength() { + return getValue("tlong"); + } + + /** @return Horizontal sidelength of the rough bounding box (0 - 320 pixels) */ + public double getHorizontalSidelength() { + return getValue("thor"); + } + + /** @return Vertical sidelength of the rough bounding box (0 - 320 pixels) */ + public double getVerticalSidelength() { + return getValue("tvert"); + } + + /** @return True active pipeline index of the camera (0 .. 9) */ + public double getPipeline() { + return getValue("getpipe"); + } + + /** + * @return Results of a 3D position solution, 6 numbers: Translation (x,y,y) + * Rotation(pitch,yaw,roll) + */ + public Double[] getCamtran() { + return getEntry("camtran").getDoubleArray(new Double[] {}); + } + + public enum LedMode { + PIPELINE(0), + FORCE_OFF(1), + FORCE_BLINK(2), + FORCE_ON(3), + UNKNOWN(-1); + public double value; + + LedMode(double value) { + this.value = value; + } + } + + /** @return The current LED mode set on the Limelight */ + public LedMode getLedMode() { + double mode = getValue("ledMode"); + if (mode == 0) { + return LedMode.PIPELINE; // Uses the LED mode set in the pipeliine + } else if (mode == 1) { + return LedMode.FORCE_OFF; + } else if (mode == 2) { + return LedMode.FORCE_BLINK; + } else if (mode == 3) { + return LedMode.FORCE_ON; + } else { + System.out.println("[Limelight] UNKNOWN LED MODE -- " + mode); + return LedMode.UNKNOWN; + } + } + + /** @param mode The LED Mode to set on the Limelight */ + public void setLedMode(LedMode mode) { + if (mode != LedMode.UNKNOWN) { + setValue("ledMode", mode.value); + } + } + + public enum CamMode { + VISION_CAM(0), + DRIVER_CAM(1), + UNKNOWN(-1); + public double value; + + CamMode(double value) { + this.value = value; + } + } + + /** @return The current LED mode set on the Limelight */ + public CamMode getCamMode() { + double mode = getValue("camMode"); + if (mode == 0) { + return CamMode.VISION_CAM; + } else if (mode == 1) { + return CamMode.DRIVER_CAM; + } else { + System.out.println("[Limelight] UNKNOWN CAMERA MODE -- " + mode); + return CamMode.UNKNOWN; + } + } + + /** @param mode The LED Mode to set on the Limelight */ + public void setCamMode(CamMode mode) { + if (mode != CamMode.UNKNOWN) { + setValue("camMode", mode.value); + } + } + + public enum Pipeline { + PIPELINE0(0), + PIPELINE1(1), + PIPELINE2(2), + PIPELINE3(3), + PIPELINE4(4), + PIPELINE5(5), + PIPELINE6(6), + PIPELINE7(7), + PIPELINE8(8), + PIPELINE9(9), + UNKNOWN(-1); + + public double value; + + Pipeline(double value) { + this.value = value; + } + } + + /** @return The current LED mode set on the Limelight */ + public Pipeline getCurrentPipeline() { + double mode = getValue("pipeline"); + if (mode == 0) { + return Pipeline.PIPELINE0; + } else if (mode == 1) { + return Pipeline.PIPELINE1; + } else if (mode == 2) { + return Pipeline.PIPELINE2; + } else if (mode == 3) { + return Pipeline.PIPELINE3; + } else if (mode == 4) { + return Pipeline.PIPELINE4; + } else if (mode == 5) { + return Pipeline.PIPELINE5; + } else if (mode == 6) { + return Pipeline.PIPELINE6; + } else if (mode == 7) { + return Pipeline.PIPELINE7; + } else if (mode == 8) { + return Pipeline.PIPELINE8; + } else if (mode == 9) { + return Pipeline.PIPELINE9; + } else { + System.out.println("[Limelight] UNKNOWN Pipeline -- " + mode); + return Pipeline.UNKNOWN; + } + } + + /** @param mode The LED Mode to set on the Limelight */ + public void setPipeline(Pipeline mode) { + if (mode != Pipeline.UNKNOWN) { + setValue("pipeline", mode.value); + } + } + + public enum StreamMode { + STANDARD(0), + PIP_MAIN(1), + PIP_SECONDARY(2), + UNKNOWN(-1); + + public double value; + + StreamMode(double value) { + this.value = value; + } + } + + /** @return The current LED mode set on the Limelight */ + public StreamMode getCurrentStreamMode() { + double mode = getValue("stream"); + if (mode == 0) { + return StreamMode.STANDARD; // Side-by-side streams if a webcam is attached to Limelight + } else if (mode == 1) { + return StreamMode + .PIP_MAIN; // The secondary camera stream is placed in the lower-right corner of the + // primary camera stream + } else if (mode == 2) { + return StreamMode.PIP_SECONDARY; + } else { + System.out.println("[Limelight] UNKNOWN StreamMode -- " + mode); + return StreamMode.UNKNOWN; + } + } + + /** @param mode The LED Mode to set on the Limelight */ + public void setStreamMode(StreamMode mode) { + if (mode != StreamMode.UNKNOWN) { + setValue("stream", mode.value); + } + } + + public enum SnapshotMode { + OFF(0), + TWO_PER_SECOND(1), + UNKNOWN(-1); + + public double value; + + SnapshotMode(double value) { + this.value = value; + } + } + + /** @return The current LED mode set on the Limelight */ + public SnapshotMode getCurrentSnapShotMode() { + double mode = getValue("snapshot"); + if (mode == 0) { + return SnapshotMode.OFF; + } else if (mode == 1) { + return SnapshotMode.TWO_PER_SECOND; + } else { + System.out.println("[Limelight] UNKNOWN SnapshotMode -- " + mode); + return SnapshotMode.UNKNOWN; + } + } + + /** @param mode The LED Mode to set on the Limelight */ + public void setSnapshotMode(SnapshotMode mode) { + if (mode != SnapshotMode.UNKNOWN) { + setValue("snapshot", mode.value); + } + } + + @Override + protected void initDefaultCommand() { + // TODO Auto-generated method stub + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Manipulator.java b/src/main/java/frc/robot/subsystems/Manipulator.java new file mode 100644 index 0000000..b476af7 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Manipulator.java @@ -0,0 +1,49 @@ +package frc.robot.subsystems; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; + +import edu.wpi.first.wpilibj.command.Subsystem; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.Solenoid; + +import frc.robot.Constants; +import frc.robot.RobotMap; + +public class Manipulator extends Subsystem { + + private CANSparkMax manipulator_neo; + private Solenoid extender; + + public Manipulator() { + manipulator_neo = new CANSparkMax(RobotMap.MANIPULATOR_NEO, MotorType.kBrushless); + manipulator_neo.setOpenLoopRampRate(Constants.kNeoRampTime); + extender = new Solenoid(RobotMap.kPCM, RobotMap.MANIPULATOR_SOLENOID); + } + + public void initDefaultCommand() { + //TODO: Find out what goes in here + } + + public void setManipulator(final double velocity) { + manipulator_neo.set(velocity); + } + public boolean isRetracted() { + //I think extender.get() returns false when it's extended + return !extender.get(); + } + public void setExtended() { + extender.set(true); + } + public void setRetracted() { + extender.set(false); + } + public boolean getRetracted() { + return !extender.get(); + } + + public void debug() { + SmartDashboard.putBoolean("Manipulator Extender Solenoid", extender.get()); + SmartDashboard.putNumber("Manipulator current", manipulator_neo.getOutputCurrent()); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/PanelMech.java b/src/main/java/frc/robot/subsystems/PanelMech.java index a73acf2..d56cdc2 100644 --- a/src/main/java/frc/robot/subsystems/PanelMech.java +++ b/src/main/java/frc/robot/subsystems/PanelMech.java @@ -1,25 +1,11 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ package frc.robot.subsystems; - import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkMaxLowLevel.MotorType; -import com.ctre.phoenix.motorcontrol.ControlMode; -import com.ctre.phoenix.motorcontrol.can.TalonSRX; - import edu.wpi.first.wpilibj.Solenoid; import edu.wpi.first.wpilibj.command.Subsystem; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - import frc.robot.Constants; import frc.robot.RobotMap; -// import frc.robot.commands.MoveShooter; -// import frc.robot.commands.MoveShooterPassive; -import frc.robot.util.Util; + /** * Add your docs here. @@ -38,8 +24,12 @@ public PanelMech(){ public void setPanelMech(final double velocity) { panelMech_motor.set(velocity); } - public void extendPanelMech(boolean extend) { - panelMech_soleinoid.set(extend); + public void extendPanelMech() { + panelMech_soleinoid.set(true); + } + + public void retractPanelMech(){ + panelMech_soleinoid.set(false); } @Override protected void initDefaultCommand() { diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 6f3ef7c..61274d6 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -7,41 +7,108 @@ package frc.robot.subsystems; import com.revrobotics.CANSparkMax; +import com.revrobotics.ControlType; import com.revrobotics.CANSparkMaxLowLevel.MotorType; -import com.ctre.phoenix.motorcontrol.ControlMode; -import com.ctre.phoenix.motorcontrol.can.TalonSRX; +import edu.wpi.first.wpilibj.Solenoid; import edu.wpi.first.wpilibj.command.Subsystem; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.Constants; -import frc.robot.RobotMap; -import frc.robot.commands.MoveShooter; -import frc.robot.commands.MoveShooterPassive; -import frc.robot.util.Util; +import frc.robot.Robot; +import frc.robot.RobotMap; +// import frc.robot.commands.MoveShooterPassive; /** * Add your docs here. */ public class Shooter extends Subsystem { - private CANSparkMax shooter_neo; - private MoveShooterPassive defaultCommand; + + public CANSparkMax shooter_leader; + private CANSparkMax shooter_follower; + private Solenoid hood_1; + // private MoveShooterPassive defaultCommand; public Shooter(){ - shooter_neo = new CANSparkMax(RobotMap.SHOOTER_NEO, MotorType.kBrushless); - shooter_neo.setOpenLoopRampRate(Constants.kNeoRampTime); + shooter_leader = new CANSparkMax(RobotMap.kShooterLeft, MotorType.kBrushless); + shooter_follower = new CANSparkMax(RobotMap.kShooterRight, MotorType.kBrushless); + shooter_leader.setOpenLoopRampRate(Constants.kNeoRampTime); + shooter_follower.setOpenLoopRampRate(Constants.kNeoRampTime); + shooter_follower.follow(shooter_leader, true); + hood_1 = new Solenoid(RobotMap.kPCM, RobotMap.HOOD_SOLENOID); + + shooter_leader.getPIDController().setP(Constants.ShooterTrenchkP); + shooter_leader.getPIDController().setFF(Constants.ShooterTrenchkFF); + shooter_leader.getPIDController().setD(Constants.ShooterTrenchkD); + shooter_leader.getPIDController().setOutputRange(-1, 1); + shooter_leader.setSmartCurrentLimit(75); + + shooter_leader.enableVoltageCompensation(11.9); + SmartDashboard.putNumber("Shooter Current", 0); + } + public double idealVelocity(double angle, double dist, double height){ + double gravityInches = Constants.kGravity*12; + angle = Math.toRadians(angle); + double velocity = Math.sqrt( (gravityInches*Math.pow(dist,2)) / (2*Math.pow(Math.cos(angle),2) * (dist*Math.tan(angle)-height))); //speed in inches/second + return velocity; } + public double applyDrag(double velocity){ + velocity*=Constants.kDrag; + return velocity; + } + public double applyMagnus(double velocity){ + velocity/=Constants.kMagnus; + return velocity; + } + public double velocityToRPM(double velocity){ + double shooterRotationsPerSecond = velocity/(.5*Constants.ShooterDiameter); //rotation per second + double shooterRPM = shooterRotationsPerSecond*60; + double motorRPM = shooterRPM/Constants.ShooterGearing; + return motorRPM; + } public void setShooter(final double output){ - shooter_neo.set(output); + shooter_leader.set(output); + } + public void setShooterPID(final double setpoint){ + if (setpoint == 0) { + setShooter(setpoint); + } else { + shooter_leader.getPIDController().setReference(setpoint, ControlType.kVelocity); + } } public double getShooterVelocity(){ - return shooter_neo.getEncoder().getVelocity(); + return shooter_leader.getEncoder().getVelocity(); + } + + public double getShooterVoltage(){ + return shooter_leader.getBusVoltage(); + } + + public double getShooterConversionFactor(){ + return shooter_leader.getEncoder().getVelocityConversionFactor(); + } + public double getShooterVelocity2(){ + double velocity = getShooterVoltage() * getShooterConversionFactor(); + return velocity; + } + public boolean isShooterVelocityCorrect(){ + if(getShooterVelocity() != Constants.SHOOTER_VELOCITY){ + return false; + } + return true; } public double getShooter(){ - return shooter_neo.getEncoder().getCountsPerRevolution(); + return shooter_leader.getEncoder().getCountsPerRevolution(); + } + public void setHood1(boolean extended) { + hood_1.set(extended); + } + + public boolean getHood1(){ + return hood_1.get(); } @Override protected void initDefaultCommand() { @@ -50,9 +117,29 @@ protected void initDefaultCommand() { // defaultCommand.start(); } + public boolean checkWheelSpeed_Wall(){ + if(Robot.shooter.getShooterVelocity() >= Constants.SHOOTER_OUTPUT_WALL - .01 && Robot.shooter.getShooterVelocity() <= Constants.SHOOTER_OUTPUT_WALL + .01) { + return true; + } + else{ + return false; + } + } + public boolean checkWheelSpeed_Long() { + if(Robot.shooter.getShooterVelocity() >= Constants.SHOOTER_OUTPUT_LONG - .01 && Robot.shooter.getShooterVelocity() <= Constants.SHOOTER_OUTPUT_LONG + .01){ + return true; + } + else{ + return false; + } + } public void debug() { SmartDashboard.putNumber("Shooter Neo Velocity -", getShooterVelocity()); SmartDashboard.putNumber("Shooter Neo CPR -", getShooter()); + SmartDashboard.putBoolean("Hood Position Down", !getHood1()); + SmartDashboard.putNumber("Voltage", getShooterVoltage()); + SmartDashboard.putNumber("Calculated Velcoity", getShooterVelocity2()); + SmartDashboard.putNumber("Shooter Current", shooter_leader.getOutputCurrent()); } } diff --git a/src/main/java/frc/robot/util/Camera_Switch/CameraSwitch.java b/src/main/java/frc/robot/util/Camera_Switch/CameraSwitch.java new file mode 100644 index 0000000..affc97e --- /dev/null +++ b/src/main/java/frc/robot/util/Camera_Switch/CameraSwitch.java @@ -0,0 +1,107 @@ +/************************************* +* @author Chris Lane | FRC 2791 | 2020 +* +* API for a multiplexer camera switch. +* +* The switch should plug into the relay +* port on the RoboRIO given in the +* constructor. Each relay value +* corresponds to a pin on the port being +* driven hi or lo (1/0). +* +* This API essentially utilizes the +* relay port as two DI/O ports right +* next to each other +* kforward = pin1 high/pin 2 low, +* kReverse = pin 1 low/pin 2 high, +* kON = pin 1 high/pin2 high, +* kOff = pin 1 low/pin 2 low. +* +**************************************/ +package frc.robot.util.Camera_Switch; + +import edu.wpi.first.wpilibj.Relay; +import edu.wpi.first.wpilibj.Relay.Value; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + + + +public class CameraSwitch implements RelayPortDevice{ + + public static final int kcamera1 = 1; + public static final int kcamera2 = 2; + public static final int kcamera3 = 3; + public static final int kcamera4 = 4; + + + + public static Relay camrelay1; + public static Relay camrelay2; + + + + private int cameraState; + public CameraSwitch(int port1, int port2){ + + camrelay1 = new Relay(port1); + camrelay2 = new Relay(port2); + + }//Constructor for a CameraSwitch on a single relay port + + public void select(int camSelected){ + /* enter the constant strings for the switch into this method to select cameras + constants for cameras are- "kcamera(camera#)" (these can be changed in the switch statment) + This switch has four possible ports. */ + cameraState = camSelected; + switch (camSelected) { + case kcamera1 : + camrelay1.set(Value.kReverse); + camrelay2.set(Value.kOff); + break; + case kcamera2 : + camrelay1.set(Value.kForward); + camrelay2.set(Value.kOff); + + break; + case kcamera3 : + camrelay2.set(Value.kReverse); + camrelay1.set(Value.kOff); + case kcamera4 : + camrelay2.set(Value.kForward); + camrelay1.set(Value.kOff); + break; + default: + camrelay1.set(Value.kReverse); + camrelay2.set(Value.kOff); + System.err.print("Camera not properly selected, setting to case 1,Camera1"); + break; + }//This switch statement is what actually writes to the relay port + + }//Use this method to select the desired camera to connect to the RoboRIO + + public void rawSetRelay(Relay.Value kvalue){ + + camrelay1.set(kvalue); + camrelay2.set(kvalue); + + }//Use this method to set the relay port to a regular relay value + + public void setDirection(Relay.Direction direction){ + + camrelay1.setDirection(direction); + camrelay2.setDirection(direction); + + }//Use this method to set valid directions for the local relay used in this class + + public void setLocalRelay(Relay.Value klocalValue) { + + //unused function + System.err.println("setLocalRelay(Relay.Value klocalValue); method not utilized in CameraSwitch.java"); + + }//DO NOT USE THIS METHOD + public void debug() { + SmartDashboard.putNumber("Camera Number: ", cameraState); + } + + } +//end of file----------------------------------------------------------------------- diff --git a/src/main/java/frc/robot/util/Camera_Switch/RelayPortDevice.java b/src/main/java/frc/robot/util/Camera_Switch/RelayPortDevice.java new file mode 100644 index 0000000..e4c1c08 --- /dev/null +++ b/src/main/java/frc/robot/util/Camera_Switch/RelayPortDevice.java @@ -0,0 +1,27 @@ +/**************************************** +* @author Chris Lane | FRC 2791 | 2020 +* +* Interface for a relay port device. +* +* This interface is for interfacing with +* devices that connect to the relay ports +* on the RoboRIO that are not regular IFI +* SPIKE or similar relays. +* +*****************************************/ + +package frc.robot.util.Camera_Switch; +import edu.wpi.first.wpilibj.Relay; + +public interface RelayPortDevice { + + +public void rawSetRelay(Relay.Value kValue);//Set the raw relay port + +public void setDirection(Relay.Direction direction);//Indirectly set relay port valid direction + +public void setLocalRelay(Relay.Value klocalValue);//Set the local relay values from a static field + + +} +//End of file----------------------------------------------------------------------- \ No newline at end of file diff --git a/src/main/java/frc/robot/util/IMU/Constants/LSM9DS1_Accelerometer_Range.java b/src/main/java/frc/robot/util/IMU/Constants/LSM9DS1_Accelerometer_Range.java new file mode 100644 index 0000000..3b71537 --- /dev/null +++ b/src/main/java/frc/robot/util/IMU/Constants/LSM9DS1_Accelerometer_Range.java @@ -0,0 +1,7 @@ +package frc.robot.util.IMU.Constants; +public enum LSM9DS1_Accelerometer_Range { + LSM9DS1_ACCELEROMETER_2G, + LSM9DS1_ACCELEROMETER_4G, + LSM9DS1_ACCELEROMETER_8G, + LSM9DS1_ACCELEROMETER_16G +} diff --git a/src/main/java/frc/robot/util/IMU/Constants/LSM9DS1_Constants.java b/src/main/java/frc/robot/util/IMU/Constants/LSM9DS1_Constants.java new file mode 100644 index 0000000..3252e80 --- /dev/null +++ b/src/main/java/frc/robot/util/IMU/Constants/LSM9DS1_Constants.java @@ -0,0 +1,41 @@ +package frc.robot.util.IMU.Constants; + +public final class LSM9DS1_Constants { + + public static final float EARTH_GRAVITY = 9.80665f; + public static final float DPS_TO_RADS = 0.017453293f; + + public static final byte LSM9DS1_M_ADDR = (byte)0x1E; + public static final byte LSM9DS1_XG_ADDR = (byte)0x6B; + + public static final byte LSM9DS1_REGISTER_WHO_AM_I_XG = 0x0F; + public static final byte LSM9DS1_REGISTER_CTRL_REG1_G = 0x10; + public static final byte LSM9DS1_REGISTER_CTRL_REG2_G = 0x11; + public static final byte LSM9DS1_REGISTER_CTRL_REG3_G = 0x12; + public static final byte LSM9DS1_REGISTER_STATUS_REG = 0x17; + public static final byte LSM9DS1_REGISTER_OUT_X_L_G = 0x18; + public static final byte LSM9DS1_REGISTER_OUT_X_H_G = 0x19; + public static final byte LSM9DS1_REGISTER_OUT_Y_L_G = 0x1A; + public static final byte LSM9DS1_REGISTER_OUT_Y_H_G = 0x1B; + public static final byte LSM9DS1_REGISTER_OUT_Z_L_G = 0x1C; + public static final byte LSM9DS1_REGISTER_OUT_Z_H_G = 0x1D; + public static final byte LSM9DS1_REGISTER_CTRL_REG4 = 0x1E; + public static final byte LSM9DS1_REGISTER_CTRL_REG5_XL = 0x1F; + public static final byte LSM9DS1_REGISTER_CTRL_REG6_XL = 0x20; + public static final byte LSM9DS1_REGISTER_CTRL_REG7_XL = 0x21; + public static final byte LSM9DS1_REGISTER_CTRL_REG8 = 0x22; + public static final byte LSM9DS1_REGISTER_CTRL_REG9 = 0x23; + public static final byte LSM9DS1_REGISTER_CTRL_REG10 = 0x24; + + public static final byte LSM9DS1_REGISTER_TEMP_OUT_L = 0x05; + public static final byte LSM9DS1_REGISTER_TEMP_OUT_H = 0x06; + public static final byte LSM9DS1_REGISTER_OUT_X_L_XL = 0x28; + public static final byte LSM9DS1_REGISTER_OUT_X_H_XL = 0x29; + public static final byte LSM9DS1_REGISTER_OUT_Y_L_XL = 0x2A; + public static final byte LSM9DS1_REGISTER_OUT_Y_H_XL = 0x2B; + public static final byte LSM9DS1_REGISTER_OUT_Z_L_XL = 0x2C; + public static final byte LSM9DS1_REGISTER_OUT_Z_H_XL = 0x2D; + + private LSM9DS1_Constants() { } + +} diff --git a/src/main/java/frc/robot/util/IMU/Constants/LSM9DS1_Gyroscope_Scale.java b/src/main/java/frc/robot/util/IMU/Constants/LSM9DS1_Gyroscope_Scale.java new file mode 100644 index 0000000..f641748 --- /dev/null +++ b/src/main/java/frc/robot/util/IMU/Constants/LSM9DS1_Gyroscope_Scale.java @@ -0,0 +1,7 @@ +package frc.robot.util.IMU.Constants; + +public enum LSM9DS1_Gyroscope_Scale { + LSM9DS1_GYROSCOPE_245DPS, + LSM9DS1_GYROSCOPE_500DPS, + LSM9DS1_GYROSCOPE_2000DPS +} diff --git a/src/main/java/frc/robot/util/IMU/Constants/LSM9DS1_Magnetometer_Gain.java b/src/main/java/frc/robot/util/IMU/Constants/LSM9DS1_Magnetometer_Gain.java new file mode 100644 index 0000000..0045929 --- /dev/null +++ b/src/main/java/frc/robot/util/IMU/Constants/LSM9DS1_Magnetometer_Gain.java @@ -0,0 +1,8 @@ +package frc.robot.util.IMU.Constants; + +public enum LSM9DS1_Magnetometer_Gain { + LSM9DS1_MAGNETOMETER_4GAUSS, + LSM9DS1_MAGNETOMETER_8GAUSS, + LSM9DS1_MAGNETOMETER_12GAUSS, + LSM9DS1_MAGNETOMETER_16GAUSS +} diff --git a/src/main/java/frc/robot/util/IMU/LSM9DS1.java b/src/main/java/frc/robot/util/IMU/LSM9DS1.java new file mode 100644 index 0000000..389ef4e --- /dev/null +++ b/src/main/java/frc/robot/util/IMU/LSM9DS1.java @@ -0,0 +1,369 @@ +// package frc.robot.util.IMU; + +// import com.pi4j.io.i2c.I2CBus; +// import com.pi4j.io.i2c.I2CDevice; +// import com.pi4j.io.i2c.I2CFactory; + +// import edu.wpi.first.wpilibj.I2C; +// import edu.wpi.first.wpilibj.I2C.Port; + +// import java.io.IOException; +// import java.text.DecimalFormat; +// import frc.robot.util.IMU.Constants.*; + +// public class LSM9DS1 { + +// private int temperature = 0; +// private long deltaTime = 0; +// private double rollXLFilteredOld = 0.0d; +// private double pitchXLFilteredOld = 0.0d; +// private double roll = 0.0d; +// private double pitch = 0.0d; +// private double yaw = 0.0d; +// private I2C bus; +// private I2CDevice magnetometer; +// private I2CDevice accelerometerGyro; +// private AccelerometerData accelerometerData; +// private GyroscopeData gyroscopeData; +// private MagnetometerData magnetometerData; + +// /** +// * Creates an instance using the default settings. (2G, 245DPS, 4GAUSS) +// */ +// public LSM9DS1() { +// this.setup(LSM9DS1_Accelerometer_Range.LSM9DS1_ACCELEROMETER_2G, LSM9DS1_Gyroscope_Scale.LSM9DS1_GYROSCOPE_245DPS, LSM9DS1_Magnetometer_Gain.LSM9DS1_MAGNETOMETER_4GAUSS); +// } + +// /** +// * Creates an instance using the specified settings. +// * +// * @param accelerometerRange AccelerometerRange that will be used. +// * @param gyroscopeScale GyroscopeScale that will be used. +// * @param magnetometerRange MagnetometerRange that will be used. +// */ +// public LSM9DS1(LSM9DS1_Accelerometer_Range accelerometerRange, LSM9DS1_Gyroscope_Scale gyroscopeScale, LSM9DS1_Magnetometer_Gain magnetometerRange) { +// this.setup(accelerometerRange, gyroscopeScale, magnetometerRange); +// } + +// private void setup(LSM9DS1_Accelerometer_Range accelerometerRange, LSM9DS1_Gyroscope_Scale gyroscopeRange, LSM9DS1_Magnetometer_Gain magnetometerRange) { +// try { +// this.bus = I2CFactory.getInstance(I2CBus.BUS_1); +// this.magnetometer = this.bus.getDevice(LSM9DS1_M_ADDR); +// this.accelerometerGyro = this.bus.getDevice(LSM9DS1_XG_ADDR); +// this.accelerometerData = new AccelerometerData(); +// this.gyroscopeData = new GyroscopeData(); +// this.magnetometerData = new MagnetometerData(); +// this.begin(accelerometerRange, gyroscopeRange, magnetometerRange); +// } catch (I2CFactory.UnsupportedBusNumberException | IOException ex) { +// ex.printStackTrace(); +// } +// } + +// private void begin(LSM9DS1_Accelerometer_Range accelerometerRange, LSM9DS1_Gyroscope_Scale gyroscopeRange, LSM9DS1_Magnetometer_Gain magnetometerRange) { +// // soft reset & reboot accelerometer/gyroscope +// this.writeRegister(true, LSM9DS1_REGISTER_CTRL_REG8, (byte) 0x05); +// try { // TODO: might be unnecessary +// Thread.sleep(10); +// } catch (InterruptedException ex) { +// ex.printStackTrace(); +// } +// int id = readRegister(true, LSM9DS1_REGISTER_WHO_AM_I_XG); +// if (id != 104) { +// System.out.println("Error identifying the device!"); +// return; +// } +// // TODO: enable temperature sensor +// // this.write8(true, LSM9DS1_REGISTER_CTRL_REG10, (byte)0xF4); + +// // enable gyro continuous +// this.writeRegister(true, LSM9DS1_REGISTER_CTRL_REG4, (byte) 0x38); + +// // enable the accelerometer continuously +// this.writeRegister(true, LSM9DS1_REGISTER_CTRL_REG5_XL, (byte) 0x38); +// this.writeRegister(true, LSM9DS1_REGISTER_CTRL_REG6_XL, (byte) 0xC0); +// // activate accelerometer high resolution mode +// this.writeRegister(true, LSM9DS1_REGISTER_CTRL_REG7_XL, (byte) 0x80); + +// // set default ranges for the various sensors +// this.setupAccelerometer(accelerometerRange); +// this.setupGyroscope(gyroscopeRange); +// this.setupMagnetometer(magnetometerRange); +// } + +// private void setupAccelerometer(LSM9DS1_Accelerometer_Range range) { +// int reg = readRegister(true, LSM9DS1_REGISTER_CTRL_REG6_XL); +// reg &= ~(0b00011000); +// byte rangeData = 0b00; +// if (range == LSM9DS1_Accelerometer_Range.LSM9DS1_ACCELEROMETER_2G) { +// this.accelerometerData.setAccelerometerMgLsb(0.061f); +// } else if (range == LSM9DS1_Accelerometer_Range.LSM9DS1_ACCELEROMETER_4G) { +// rangeData = (0b10 << 3); +// this.accelerometerData.setAccelerometerMgLsb(0.122f); +// } else if (range == LSM9DS1_Accelerometer_Range.LSM9DS1_ACCELEROMETER_8G) { +// rangeData = (0b11 << 3); +// this.accelerometerData.setAccelerometerMgLsb(0.244f); +// } else if (range == LSM9DS1_Accelerometer_Range.LSM9DS1_ACCELEROMETER_16G) { +// rangeData = (0b01 << 3); +// this.accelerometerData.setAccelerometerMgLsb(0.732f); +// } +// reg |= rangeData; +// writeRegister(true, LSM9DS1_REGISTER_CTRL_REG6_XL, (byte)reg); +// } + +// private void setupGyroscope(LSM9DS1_Gyroscope_Scale range) { +// int reg = readRegister(true, LSM9DS1_REGISTER_CTRL_REG1_G); +// reg &= ~(0b00110000); +// byte rangeData = 0b00; +// if (range == LSM9DS1_Gyroscope_Scale.LSM9DS1_GYROSCOPE_245DPS) { +// this.gyroscopeData.setGyroDpsDigit(0.00875f); +// } else if (range == LSM9DS1_Gyroscope_Scale.LSM9DS1_GYROSCOPE_500DPS) { +// rangeData = (0b01 << 4); +// this.gyroscopeData.setGyroDpsDigit(0.01750f); +// } else if (range == LSM9DS1_Gyroscope_Scale.LSM9DS1_GYROSCOPE_2000DPS) { +// rangeData = (0b11 << 4); +// this.gyroscopeData.setGyroDpsDigit(0.07000f); +// } +// reg |= rangeData; +// writeRegister(true, LSM9DS1_REGISTER_CTRL_REG1_G, (byte)reg); +// } + +// private void setupMagnetometer(LSM9DS1_Magnetometer_Gain range) { +// // TODO +// } + +// private void readAccelerometerOrGyroscope(boolean readAccelerometer) { +// byte reg; +// if (readAccelerometer) { +// reg = (byte)(0x80 | LSM9DS1_REGISTER_OUT_X_L_XL); +// } else { +// reg = (byte)(0x80 | LSM9DS1_REGISTER_OUT_X_L_G); +// } +// byte[] buffer = this.readBuffer(true, reg, (byte)6); + +// int xAxis = (((int) buffer[1]) << 8) | (buffer[0] & 0xFF); +// int yAxis = (((int) buffer[3]) << 8) | (buffer[2] & 0xFF); +// int zAxis = (((int) buffer[5]) << 8) | (buffer[4] & 0xFF); + +// if (readAccelerometer) { +// this.accelerometerData.x = xAxis; +// this.accelerometerData.y = yAxis; +// this.accelerometerData.z = zAxis; +// } else { +// this.gyroscopeData.x = xAxis; +// this.gyroscopeData.y = yAxis; +// this.gyroscopeData.z = zAxis; +// } +// } + +// private void readMagnetometer() { +// // TODO +// } + +// private void readTemperature() { +// byte reg = (byte)(0x80 | LSM9DS1_REGISTER_TEMP_OUT_L); +// byte[] buffer = this.readBuffer(true, reg, (byte)2); +// this.temperature = (((int) buffer[1] << 8) | (buffer[0] & 0xFF)); +// } + +// private void writeRegister(boolean writerAccelerometerGyroscope, byte register, byte value) { +// try { +// if (writerAccelerometerGyroscope) { +// this.accelerometerGyro.write(register, value); +// } else { +// this.magnetometer.write(register, value); +// } +// } catch (IOException ex) { +// ex.printStackTrace(); +// } +// } + +// private byte readRegister(boolean readAccelerometerGyroscope, byte register) { +// byte[] value = this.readBuffer(readAccelerometerGyroscope, register, (byte)1); +// return value[0]; +// } + +// private byte[] readBuffer(boolean readAccelerometerGyro, byte register, byte length) { +// byte[] buffer = new byte[length]; +// try { +// if (readAccelerometerGyro) { +// this.accelerometerGyro.write(register); +// this.accelerometerGyro.read(buffer, 0, length); +// } else { +// // TODO Magnetometer read +// } +// } catch (IOException ex) { +// ex.printStackTrace(); +// } +// return buffer; +// } + +// /** +// * Reads and stores all data from the IMU. Also calculates Euler angles. +// */ +// public void read() { +// this.readAccelerometerOrGyroscope(true); +// this.readAccelerometerOrGyroscope(false); +// this.readMagnetometer(); +// // this.readTemperature(); +// this.calculateEulerAngles(); +// } + +// /** +// * @return Returns the last saved data from the accelerometer. +// */ +// public float[] getAccelerometerData() { +// float[] accelerometerData = new float[3]; +// accelerometerData[0] = this.accelerometerData.getX(); +// accelerometerData[1] = this.accelerometerData.getY(); +// accelerometerData[2] = this.accelerometerData.getZ(); +// return accelerometerData; +// } + +// /** +// * @return Returns the last saved data from the gyroscope. +// */ +// public float[] getGyroscopeData() { +// float[] gyroscopeData = new float[3]; +// gyroscopeData[0] = this.gyroscopeData.getX(); +// gyroscopeData[1] = this.gyroscopeData.getY(); +// gyroscopeData[2] = this.gyroscopeData.getZ(); +// return gyroscopeData; +// } + +// /** +// * @return Returns the last saved data from the magnetometer. +// */ +// public float[] getMagnetometerData() { +// float[] magnetometerData = new float[3]; +// magnetometerData[0] = this.magnetometerData.getX(); +// magnetometerData[1] = this.magnetometerData.getY(); +// magnetometerData[2] = this.magnetometerData.getZ(); +// return magnetometerData; +// } + +// /** +// * @return Returns the last saved raw data from the temperature sensor. +// */ +// public int getTemperature() { +// return this.temperature; +// } + +// /** +// * @return Returns the last calculated pitch. +// */ +// public double getPitch() { +// return this.pitch; +// } + +// /** +// * @return Returns the last calculated roll. +// */ +// public double getRoll() { +// return this.roll; +// } + +// /** +// * @return Returns the last calculated yaw. +// */ +// public double getYaw() { +// return this.yaw; +// } + +// private void calculateEulerAngles() { +// // Measured angle by the accelerometer +// double rollXLMeasured = Math.atan2(this.accelerometerData.getX() / EARTH_GRAVITY, this.accelerometerData.getZ() / EARTH_GRAVITY) / 2 / Math.PI * 360; +// double pitchXLMeasured = Math.atan2(this.accelerometerData.getY() / EARTH_GRAVITY, this.accelerometerData.getZ() / EARTH_GRAVITY) / 2 / Math.PI * 360; + +// // Adding a low pass filter +// double rollXLFiltered = 0.9f * rollXLFilteredOld + 0.1f * rollXLMeasured; +// double pitchXLFiltered = 0.9f * pitchXLFilteredOld + 0.1f * pitchXLMeasured; +// this.rollXLFilteredOld = rollXLFiltered; +// this.pitchXLFilteredOld = pitchXLFiltered; + +// // Calculating deltaTime +// long time = System.nanoTime(); +// int difference = (int) ((time - this.deltaTime) / 1000000000); +// this.deltaTime = time; + +// // Adding a complementary filter +// this.roll = 0.95f * (this.roll + this.gyroscopeData.getY() * difference) + 0.05f * rollXLMeasured; +// this.pitch = 0.95f * (this.pitch - this.gyroscopeData.getX() * difference) + 0.05f * pitchXLMeasured; +// } + +// /** +// * Closes the currently used I2C bus. +// * +// * @throws IOException Throws when the I2C bus could not be closed. +// */ +// public void close() throws IOException { +// this.bus.close(); +// } + +// private static class AccelerometerData { +// private float x = 0.0f; +// private float y = 0.0f; +// private float z = 0.0f; +// private float accelerometerMgLsb = 0.0f; + +// public void setAccelerometerMgLsb(float accelerometerMgLsb) { +// this.accelerometerMgLsb = accelerometerMgLsb; +// } + +// public float getX() { +// return (x * accelerometerMgLsb / 1000 * EARTH_GRAVITY); +// } + +// public float getY() { +// return (y * accelerometerMgLsb / 1000 * EARTH_GRAVITY); +// } + +// public float getZ() { +// return (z * accelerometerMgLsb / 1000 * EARTH_GRAVITY); +// } + +// } + +// private static class GyroscopeData { +// private float x = 0.0f; +// private float y = 0.0f; +// private float z = 0.0f; +// private float gyroDpsDigit = 0.0f; + + +// public void setGyroDpsDigit(float gyroDpsDigit) { +// this.gyroDpsDigit = gyroDpsDigit; +// } + +// public float getX() { +// return (x * gyroDpsDigit * DPS_TO_RADS); +// } + +// public float getY() { +// return (y * gyroDpsDigit * DPS_TO_RADS); +// } + +// public float getZ() { +// return (z * gyroDpsDigit * DPS_TO_RADS); +// } + +// } + +// private static class MagnetometerData { +// private float x = 0.0f; +// private float y = 0.0f; +// private float z = 0.0f; + +// public float getX() { +// return x; +// } + +// public float getY() { +// return y; +// } + +// public float getZ() { +// return z; +// } + +// } +// }