/*----------------------------------------------------------------------------*/
/* */
/* Module: main.cpp */
/* Author: C:\Users\Andy Lentini */
/* Created: Thu Nov 07 2019 */
/* Description: V5 project */
/* */
/*----------------------------------------------------------------------------*/
// ---- START VEXCODE CONFIGURED DEVICES ----
// Robot Configuration:
// [Name] [Type] [Port(s)]
// BumperA bumper A
// ---- END VEXCODE CONFIGURED DEVICES ----
#include "vex.h"
#include <cmath>
using namespace vex;
using namespace std;
motor LF_Drive=motor(PORT12, gearSetting::ratio18_1, true);
motor LB_Drive =motor(PORT20, gearSetting::ratio18_1, true);
motor RB_Drive =motor(PORT1, gearSetting::ratio18_1, false);
motor RF_Drive =motor(PORT6, gearSetting::ratio18_1, false);
motor Lift =motor(PORT2, gearSetting::ratio36_1, false);
motor Tray =motor(PORT5, gearSetting::ratio18_1, true);
motor Right_Intake =motor(PORT10, gearSetting::ratio36_1, true);
motor Left_Intake =motor(PORT8, gearSetting::ratio36_1, true);
bumper LiftBumper = bumper(Brain.ThreeWirePort.A);
controller Controller1;
void DriveTerrainControl() {
if (Controller1.Axis3.value() > 10) {
LF_Drive.spin(directionType::fwd, Controller1.Axis3.value(),percentUnits::pct);
LB_Drive.spin(directionType::fwd, Controller1.Axis3.value(),percentUnits::pct);
} else if (Controller1.Axis3.value() < -10) {
LF_Drive.spin(directionType::rev, std::abs(Controller1.Axis3.value()),percentUnits::pct);
LB_Drive.spin(directionType::rev, std::abs(Controller1.Axis3.value()),percentUnits::pct);
} else {
LF_Drive.stop(brakeType::coast);
LB_Drive.stop(brakeType::coast);
}
if (Controller1.Axis2.value() > 10) {
RF_Drive.spin(directionType::fwd, Controller1.Axis2.value(),percentUnits::pct);
RB_Drive.spin(directionType::fwd, Controller1.Axis2.value(),percentUnits::pct);
} else if (Controller1.Axis2.value() < -10) {
RF_Drive.spin(directionType::rev, std::abs(Controller1.Axis2.value()),percentUnits::pct);
RB_Drive.spin(directionType::rev, std::abs(Controller1.Axis2.value()),percentUnits::pct);
} else {
RF_Drive.stop(brakeType::coast);
RB_Drive.stop(brakeType::coast);
}
}
void DriveControl() {
// Drive Control
DriveTerrainControl();
// Brake hold system
if (Controller1.ButtonA.pressing()) {
LF_Drive.stop(brakeType::hold);
LB_Drive.stop(brakeType::hold);
RF_Drive.stop(brakeType::hold);
RB_Drive.stop(brakeType::hold);
} else {
DriveTerrainControl();
}
// Set Speed at half
if (Controller1.ButtonX.pressing()) {
if (Controller1.Axis3.value() > 10) {
LF_Drive.spin(directionType::fwd,(Controller1.Axis3.value() * abs(50) / 100),percentUnits::pct);
LB_Drive.spin(directionType::fwd,(Controller1.Axis3.value() * abs(50) / 100),percentUnits::pct);
}
else if (Controller1.Axis3.value() < -10) {
LF_Drive.spin(directionType::rev,(std::abs(Controller1.Axis3.value()) * abs(50) / 100),percentUnits::pct);
LB_Drive.spin(directionType::rev,(std::abs(Controller1.Axis3.value()) * abs(50) / 100),percentUnits::pct);
}
else {
LF_Drive.stop(brakeType::coast);
LB_Drive.stop(brakeType::coast);
}
if (Controller1.Axis2.value() > 10) {
RF_Drive.spin(directionType::fwd,(Controller1.Axis2.value() * abs(50) / 100),percentUnits::pct);
RB_Drive.spin(directionType::fwd,(Controller1.Axis2.value() * abs(50) / 100),percentUnits::pct);
}
else if (Controller1.Axis2.value() < -10) {
RF_Drive.spin(directionType::rev,(std::abs(Controller1.Axis2.value()) * abs(50) / 100),percentUnits::pct);
RB_Drive.spin(directionType::rev,(std::abs(Controller1.Axis2.value()) * abs(50) / 100),percentUnits::pct);
}
else {
RF_Drive.stop(brakeType::coast);
RB_Drive.stop(brakeType::coast);
}
} else {
DriveTerrainControl();
}
// Tray control
if (Controller1.ButtonR1.pressing()) {
Tray.spin(directionType::fwd, 50, velocityUnits::pct);
}
else if (Controller1.ButtonR2.pressing()) {
Tray.spin(directionType::rev, 100, velocityUnits::pct);
}
else {
Tray.stop(brakeType::hold);
}
if(Controller1.ButtonB.pressing()){
// Lifts the Lift and Tray[]
if (Controller1.Axis3.value() > 10){
Lift.spin(directionType::fwd, Controller1.Axis3.value(), percentUnits::pct);
}
else if(Controller1.Axis3.value() < -10){
Lift.spin(directionType::rev, std::abs(Controller1.Axis3.value()), percentUnits::pct);
}
else{
Lift.stop(brakeType::hold);
}
// Changes Drive to Arcade Drive
if (Controller1.Axis2.value() > 10 & Controller1.Axis1.value() > 10) {
LF_Drive.spin(directionType::fwd,(Controller1.Axis2.value() + Controller1.Axis1.value()) / 2,percentUnits::pct);
LB_Drive.spin(directionType::fwd,(Controller1.Axis2.value() + Controller1.Axis1.value()) / 2,percentUnits::pct);
RF_Drive.spin(directionType::fwd,(Controller1.Axis2.value() + Controller1.Axis1.value()) / 2,percentUnits::pct);
RB_Drive.spin(directionType::fwd,(Controller1.Axis2.value() + Controller1.Axis1.value()) / 2,percentUnits::pct);
}
else if (Controller1.Axis2.value() < -10 &Controller1.Axis1.value() < -10) {
LF_Drive.spin(directionType::fwd,std::abs((Controller1.Axis2.value() + Controller1.Axis1.value()) / 2),percentUnits::pct);
LB_Drive.spin(directionType::fwd,std::abs((Controller1.Axis2.value() + Controller1.Axis1.value()) / 2),percentUnits::pct);
RF_Drive.spin(directionType::fwd,std::abs((Controller1.Axis2.value() + Controller1.Axis1.value()) / 2),percentUnits::pct);
RB_Drive.spin(directionType::fwd,std::abs((Controller1.Axis2.value() + Controller1.Axis1.value()) / 2),percentUnits::pct);
}
else {
RF_Drive.stop(brakeType::coast);
RB_Drive.stop(brakeType::coast);
LF_Drive.stop(brakeType::coast);
LB_Drive.stop(brakeType::coast);
}
}
else{
DriveTerrainControl();
Lift.stop(brakeType::hold);
}
// Intake Control
if (Controller1.ButtonL1.pressing()) {
Right_Intake.spin(directionType::fwd, 100, pct);
Left_Intake.spin(directionType::rev, 100, pct);
}
else if (Controller1.ButtonL2.pressing()) {
Right_Intake.spin(directionType::rev, 100, pct);
Left_Intake.spin(directionType::fwd, 100, pct);
}
else {
Right_Intake.stop(brakeType::hold);
Left_Intake.stop(brakeType::hold);
}
}
int main() {
// Initializing Robot Configuration. DO NOT REMOVE!
vexcodeInit();
while (1) {
// Brain.Screen.print("Driver Control Started");
// Controller1.Screen.print("Driver Control Started");
// Controller1.rumble(".-.-");
while (true) {
DriveControl();
}
}
}