Add intake and drive controls for stacking

master
Cole Deck
parent 9c717e5456
commit 8a75f10c2f

Binary file not shown.

Binary file not shown.

File diff suppressed because it is too large Load Diff

Binary file not shown.

@ -10,12 +10,13 @@
// ---- START VEXCODE CONFIGURED DEVICES ---- // ---- START VEXCODE CONFIGURED DEVICES ----
// Robot Configuration: // Robot Configuration:
// [Name] [Type] [Port(s)] // [Name] [Type] [Port(s)]
// Drivetrain drivetrain 10, 1 // Drivetrain drivetrain 1, 2
// LiftMotor motor 9 // LiftMotor motor 18
// TiltMotor motor 8 // LiftMotor2 motor 12
// TiltMotor motor 3
// Controller1 controller // Controller1 controller
// IntakeR motor 2 // IntakeR motor 17
// IntakeL motor 3 // IntakeL motor 8
// ---- END VEXCODE CONFIGURED DEVICES ---- // ---- END VEXCODE CONFIGURED DEVICES ----
#include "vex.h" #include "vex.h"
@ -231,7 +232,6 @@ void usercontrol(void) {
Controller1.Screen.print("Normal Driving "); Controller1.Screen.print("Normal Driving ");
bool recording = false; bool recording = false;
bool donerecording = false; bool donerecording = false;
//uint8_t * data = (uint8_t*)malloc(sizeof(uint8_t) * 5);
// User control code here, inside the loop // User control code here, inside the loop
while (1) { while (1) {
@ -435,18 +435,54 @@ void controllerBaseInput() {
drivetrainLeftSideSpeed = -powerLevel; drivetrainLeftSideSpeed = -powerLevel;
drivetrainRightSideSpeed = powerLevel; drivetrainRightSideSpeed = powerLevel;
} }
int IntakeSpeed = Controller1.Axis2.position();
if (IntakeSpeed < 5 && IntakeSpeed > -5) {
// check if the left motor has already been stopped
if (IntakeStop) {
// stop the left drive motor
IntakeL.stop();
IntakeR.stop();
// tell the code that the left motor has been stopped
IntakeStop = false;
}
} else {
// reset the toggle so that the deadband code knows to stop the left motor next time the input is in the deadband range
IntakeStop = true;
}
// only tell the left drive motor to spin if the values are not in the deadband range
if(speedin) {
Controller1.Screen.setCursor(2, 0);
//Controller1.Screen.clearLine();
Controller1.Screen.print("Fast intake");
} else {
Controller1.Screen.setCursor(2, 0);
//Controller1.Screen.clearLine();
Controller1.Screen.print("Slow intake");
IntakeSpeed /= 4;
}
if(Controller1.ButtonDown.pressing()) { if(Controller1.ButtonDown.pressing()) {
drivetrainLeftSideSpeed = -33; drivetrainLeftSideSpeed = -20;
drivetrainRightSideSpeed = -33; drivetrainRightSideSpeed = -20;
IntakeStop = true;
IntakeSpeed = -20;
} }
else if(Controller1.ButtonUp.pressing()) { else if(Controller1.ButtonUp.pressing()) {
drivetrainLeftSideSpeed = 33; drivetrainLeftSideSpeed = 20;
drivetrainRightSideSpeed = 33; drivetrainRightSideSpeed = 20;
IntakeStop = true;
IntakeSpeed = 20;
} }
else { else {
drivetrainLeftSideSpeed *= driveMultiplier; drivetrainLeftSideSpeed *= driveMultiplier;
drivetrainRightSideSpeed *= driveMultiplier; drivetrainRightSideSpeed *= driveMultiplier;
} }
if (IntakeStop) {
IntakeL.setVelocity(IntakeSpeed, percent);
IntakeR.setVelocity(IntakeSpeed, percent);
IntakeL.spin(forward);
IntakeR.spin(forward);
}
// check if the value is inside of the deadband range // check if the value is inside of the deadband range
if (drivetrainLeftSideSpeed < 5 && drivetrainLeftSideSpeed > -5) { if (drivetrainLeftSideSpeed < 5 && drivetrainLeftSideSpeed > -5) {
// check if the left motor has already been stopped // check if the left motor has already been stopped
@ -498,37 +534,7 @@ void controllerBaseInput() {
// set the toggle so that we don't constantly tell the motor to stop when the buttons are released // set the toggle so that we don't constantly tell the motor to stop when the buttons are released
Controller1LeftShoulderControlMotorsStopped = true; Controller1LeftShoulderControlMotorsStopped = true;
} }
int IntakeSpeed = Controller1.Axis2.position();
if (IntakeSpeed < 5 && IntakeSpeed > -5) {
// check if the left motor has already been stopped
if (IntakeStop) {
// stop the left drive motor
IntakeL.stop();
IntakeR.stop();
// tell the code that the left motor has been stopped
IntakeStop = false;
}
} else {
// reset the toggle so that the deadband code knows to stop the left motor next time the input is in the deadband range
IntakeStop = true;
}
// only tell the left drive motor to spin if the values are not in the deadband range
if(speedin) {
Controller1.Screen.setCursor(2, 0);
//Controller1.Screen.clearLine();
Controller1.Screen.print("Fast intake");
} else {
Controller1.Screen.setCursor(2, 0);
//Controller1.Screen.clearLine();
Controller1.Screen.print("Slow intake");
IntakeSpeed /= 4;
}
if (IntakeStop) {
IntakeL.setVelocity(IntakeSpeed, percent);
IntakeR.setVelocity(IntakeSpeed, percent);
IntakeL.spin(forward);
IntakeR.spin(forward);
}
// only tell the right drive motor to spin if the values are not in the deadband range // only tell the right drive motor to spin if the values are not in the deadband range
// check the ButtonR1/ButtonR2 status to control Intake // check the ButtonR1/ButtonR2 status to control Intake

Loading…
Cancel
Save