-
Notifications
You must be signed in to change notification settings - Fork 0
Expand file tree
/
Copy pathMachineControlProject.v5cpp
More file actions
1 lines (1 loc) · 11.5 KB
/
Copy pathMachineControlProject.v5cpp
File metadata and controls
1 lines (1 loc) · 11.5 KB
1
{"mode":"Text","hardwareTarget":"brain","textContent":"#pragma region VEXcode Generated Robot Configuration\n// Make sure all required headers are included.\n#include <stdio.h>\n#include <stdlib.h>\n#include <stdbool.h>\n#include <math.h>\n#include <string.h>\n\n\n#include \"vex.h\"\n\nusing namespace vex;\n\n// Brain should be defined by default\nbrain Brain;\n\n\n// START V5 MACROS\n#define waitUntil(condition) \\\n do { \\\n wait(5, msec); \\\n } while (!(condition))\n\n#define repeat(iterations) \\\n for (int iterator = 0; iterator < iterations; iterator++)\n// END V5 MACROS\n\n\n// Robot configuration code.\nmotor elevatorMotor = motor(PORT10, ratio18_1, false);\n\ndistance elevatorDistance = distance(PORT2);\nmotor floorOneDoor = motor(PORT11, ratio18_1, false);\n\nmotor floorTwoDoor = motor(PORT12, ratio18_1, false);\n\nmotor floorThreeDoor = motor(PORT13, ratio18_1, false);\n\nbumper floorOneBumper = bumper(Brain.ThreeWirePort.A);\nbumper floorTwoBumper = bumper(Brain.ThreeWirePort.B);\nbumper floorThreeBumper = bumper(Brain.ThreeWirePort.C);\nled floorThreeLED = led(Brain.ThreeWirePort.H);\nlimit floorOneLS = limit(Brain.ThreeWirePort.D);\nlimit floorTwoLS = limit(Brain.ThreeWirePort.E);\nlimit floorThreeLS = limit(Brain.ThreeWirePort.F);\nled floorTwoLED = led(Brain.ThreeWirePort.G);\n\n\n// generating and setting random seed\nvoid initializeRandomSeed(){\n int systemTime = Brain.Timer.systemHighResolution();\n double batteryCurrent = Brain.Battery.current();\n double batteryVoltage = Brain.Battery.voltage(voltageUnits::mV);\n\n // Combine these values into a single integer\n int seed = int(batteryVoltage + batteryCurrent * 100) + systemTime;\n\n // Set the seed\n srand(seed);\n}\n\n\n\nvoid vexcodeInit() {\n\n //Initializing random seed.\n initializeRandomSeed(); \n}\n\n\n// Helper to make playing sounds from the V5 in VEXcode easier and\n// keeps the code cleaner by making it clear what is happening.\nvoid playVexcodeSound(const char *soundName) {\n printf(\"VEXPlaySound:%s\\n\", soundName);\n wait(5, msec);\n}\n\n#pragma endregion VEXcode Generated Robot Configuration\n\n/*----------------------------------------------------------------------------*/\n/* */\n/* Module: main.cpp */\n/* Author: {Abdullah Khaled} */\n/* Created: {3/3/2025} */\n/* Description: Code for elevator project for */\n/* FISD PLTW Machine Control Project */\n/*----------------------------------------------------------------------------*/\n\n// Include the V5 Library\n#include \"vex.h\"\n \n// Allows for easier use of the VEX Library\nusing namespace vex;\n\n/* Calculations for elevator motor speed using P controller */\ndouble calculateElevatorSpeed(double kP, double curr, double setpoint) {\n return (setpoint - curr) * kP;\n}\n\n// MathUtil functions\n\n/* Caps the given value a at the given max b */\ndouble max(double a, double b) {\n if (a > b) {\n return b;\n } else {\n return a;\n }\n}\n\n/* Returns the absolute value of a*/\ndouble abs(double a) {\n if (a > 0) {\n return a;\n } else {\n return -a;\n }\n}\n\n/* Returns the distance of the elevator's distance sensor \n in millimeters*/\ndouble getDistance() {\n return elevatorDistance.objectDistance(mm);\n}\n\n/* Non-functional code that should get the closest rounded floor\n based on the distance sensed by the elevator distance sensor */\ndouble getClosestFloor() {\n return (round((getDistance() - 22) / 55) + 1);\n}\n\n/* Set the elevator speed in rpm based on the distance away from the setpoint\n With a max speed of 5000 rpm */\nint goToHeight(double floorDist) {\n double kP = 3.0;\n elevatorMotor.setVelocity(\n max(\n calculateElevatorSpeed(kP, getDistance(), floorDist),\n 5000\n ),\n rpm\n );\n elevatorMotor.spin(reverse);\n return 0;\n}\n\n/* Open the door of the floor based on the floor number passed in */\nint openDoor(int floor) {\n floorOneDoor.setVelocity(50, percent);\n floorTwoDoor.setVelocity(50, percent);\n floorThreeDoor.setVelocity(50, percent);\n\n if (floor == 1) {\n floorOneDoor.spinToPosition(60, degrees);\n return 1;\n }\n if (floor == 2) {\n floorTwoDoor.spinToPosition(60, degrees);\n return 2;\n }\n if (floor == 3) {\n floorThreeDoor.spinToPosition(60, degrees);\n return 3;\n }\n return 0;\n}\n\n/*Helper function to toggle the LEDs on or off (if they work T_T) */\nint toggleLEDs(bool toggle) {\n if (toggle) {\n floorTwoLED.on();\n floorThreeLED.on();\n } else {\n floorTwoLED.off();\n floorThreeLED.off();\n }\n return 0;\n}\n\n/* Fills the screen with color c one line below text is printed out*/\nint fillScreen(color c) {\n Brain.Screen.setPenColor(c);\n Brain.Screen.setFillColor(c);\n Brain.Screen.drawRectangle(1, 30, 1000, 1000);\n return 0;\n}\n\n// During maintenance mode, print to screen, open doors, and turn on LEDs\nint maintenanceMode() {\n\n //Open all doors\n floorOneDoor.spinToPosition(60, degrees);\n floorTwoDoor.spinToPosition(60, degrees);\n floorThreeDoor.spinToPosition(60, degrees);\n\n Brain.Screen.clearScreen();\n\n //State maintenance mode on screen\n Brain.Screen.setCursor(1, 5);\n Brain.Screen.setFillColor(red);\n Brain.Screen.setPenColor(black);\n Brain.Screen.print(\"MAINTENANCE MODE\");\n Brain.Screen.setPenWidth(10);\n \n //Fill the screen with alternating red and yellow as a stand-in for LEDs\n while (floorOneLS.pressing() && floorTwoLS.pressing() && floorThreeLS.pressing()) {\n toggleLEDs(true);\n // Brain.Screen.newLine();\n fillScreen(red);\n wait(.5, seconds);\n fillScreen(yellow);\n wait(.5, seconds);\n }\n //Once maintenance mode over, refill the screen with black\n fillScreen(black);\n return 0;\n}\n\nint main() {\n // Initializing Robot Configuration. DO NOT REMOVE!\n vexcodeInit();\n // Begin project code\n\n // After 10 seconds (converted to ms) floor one downtime will kick in\n Brain.Timer.clear();\n int kUnusedTime = 3 * 1000;\n bool maintenanceLocked = false;\n\n // Store constants for floors and set the floor to floor one\n double kFloorTolerance = 5.0;\n double kFloorOneDist = 22.0;\n double kFloorTwoDist = 75.0;\n double kFloorThreeDist = 130.0;\n double distanceSetpoint = kFloorOneDist;\n double currentFloor = 1;\n int floorReachedFloor = 0;\n color printColor = orange;\n\n // Initialize doors\n floorOneDoor.setPosition(0, degrees);\n floorTwoDoor.setPosition(0, degrees);\n floorThreeDoor.setPosition(0, degrees);\n\n // Initialize screen settings\n Brain.Screen.setCursor(1, 1);\n Brain.Screen.setFont(mono30);\n Brain.Screen.setPenColor(white);\n\n while (true) {\n // If all three limit switches are pressed, toggle on maintenance mode\n if (floorOneLS.pressing() && floorTwoLS.pressing() && floorThreeLS.pressing()) {\n if (!maintenanceLocked) {\n maintenanceLocked = true;\n maintenanceMode();\n }\n } else {\n Brain.Screen.setPenColor(black);\n toggleLEDs(false);\n maintenanceLocked = false;\n Brain.Screen.clearScreen();\n\n // Logic for floor selection (Floor 1-3)\n if (floorOneBumper.pressing() || floorOneLS.pressing()) {\n Brain.Timer.clear();\n currentFloor = 1;\n distanceSetpoint = kFloorOneDist;\n }\n if (floorTwoBumper.pressing() || floorTwoLS.pressing()) {\n Brain.Timer.clear();\n currentFloor = 2;\n distanceSetpoint = kFloorTwoDist;\n }\n if (floorThreeBumper.pressing() || floorThreeLS.pressing()) {\n Brain.Timer.clear();\n currentFloor = 3;\n distanceSetpoint = kFloorThreeDist;\n }\n\n // Logic for floor one inactivity default (kUnusedTime = 10 seconds)\n if (Brain.Timer.time(msec) >= kUnusedTime) {\n currentFloor = 1;\n distanceSetpoint = kFloorOneDist;\n }\n\n goToHeight(distanceSetpoint);\n\n // Telemetry for tuning P gain\n // Brain.Screen.print(distanceSetpoint);\n // Brain.Screen.newLine();\n // Brain.Screen.print(getDistance());\n\n // Telemetry for displaying the current floor\n Brain.Screen.setCursor(1, 10);\n Brain.Screen.print(\"Floor: \");\n Brain.Screen.setCursor(1, 17);\n Brain.Screen.print(floorReachedFloor);\n fillScreen(printColor);\n \n // Logic for opening doors (must be within tolerances)\n if (abs(getDistance() - distanceSetpoint) <= kFloorTolerance) {\n floorReachedFloor = openDoor(currentFloor);\n // floorReachedFloor = currentFloor;\n Brain.Screen.newLine();\n Brain.Screen.print(\"FLOOR REACHED\");\n printColor = green;\n } else {\n floorOneDoor.spinToPosition(0, degrees);\n floorTwoDoor.spinToPosition(0, degrees);\n floorThreeDoor.spinToPosition(0, degrees);\n printColor = orange;\n }\n }\n }\n}","textLanguage":"cpp","robotConfig":[{"port":[10],"name":"elevatorMotor","customName":true,"deviceType":"Motor","deviceClass":"motor","setting":{"reversed":"false","fwd":"forward","rev":"reverse","gear":"ratio18_1"},"triportSourcePort":22},{"port":[2],"name":"elevatorDistance","customName":true,"deviceType":"Distance","deviceClass":"distance","setting":{},"triportSourcePort":22},{"port":[11],"name":"floorOneDoor","customName":true,"deviceType":"Motor","deviceClass":"motor","setting":{"reversed":"false","fwd":"forward","rev":"reverse","gear":"ratio18_1"},"triportSourcePort":22},{"port":[12],"name":"floorTwoDoor","customName":true,"deviceType":"Motor","deviceClass":"motor","setting":{"reversed":"false","fwd":"forward","rev":"reverse","gear":"ratio18_1"},"triportSourcePort":22},{"port":[13],"name":"floorThreeDoor","customName":true,"deviceType":"Motor","deviceClass":"motor","setting":{"reversed":"false","fwd":"forward","rev":"reverse","gear":"ratio18_1"},"triportSourcePort":22},{"port":[1],"name":"floorOneBumper","customName":true,"deviceType":"Bumper","deviceClass":"bumper","setting":{},"triportSourcePort":22},{"port":[2],"name":"floorTwoBumper","customName":true,"deviceType":"Bumper","deviceClass":"bumper","setting":{},"triportSourcePort":22},{"port":[3],"name":"floorThreeBumper","customName":true,"deviceType":"Bumper","deviceClass":"bumper","setting":{},"triportSourcePort":22},{"port":[8],"name":"floorThreeLED","customName":true,"deviceType":"LED","deviceClass":"led","setting":{},"triportSourcePort":22},{"port":[4],"name":"floorOneLS","customName":true,"deviceType":"LimitSwitch","deviceClass":"limit","setting":{},"triportSourcePort":22},{"port":[5],"name":"floorTwoLS","customName":true,"deviceType":"LimitSwitch","deviceClass":"limit","setting":{},"triportSourcePort":22},{"port":[6],"name":"floorThreeLS","customName":true,"deviceType":"LimitSwitch","deviceClass":"limit","setting":{},"triportSourcePort":22},{"port":[7],"name":"floorTwoLED","customName":true,"deviceType":"LED","deviceClass":"led","setting":{},"triportSourcePort":22}],"slot":0,"platform":"V5","sdkVersion":"20240802.15.00.00","appVersion":"4.0.8","fileFormat":"2.0.0","targetBrainGen":"First","v5Sounds":[{"name":"game over","url":"static/sounds/mixkit-arcade-retro-game-over-213.wav"}],"v5SoundsEnabled":false}