forked from kbilla/VEX-PID-Odometry
-
Notifications
You must be signed in to change notification settings - Fork 0
Expand file tree
/
Copy pathPIDTEST.v5cpp
More file actions
1 lines (1 loc) · 6.24 KB
/
PIDTEST.v5cpp
File metadata and controls
1 lines (1 loc) · 6.24 KB
1
{"mode":"Text","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 FL = motor(PORT14, ratio18_1, false);\n\nmotor BL = motor(PORT18, ratio18_1, false);\n\nmotor FR = motor(PORT13, ratio18_1, true);\n\nmotor BR = motor(PORT20, ratio18_1, true);\n\n\n\n#pragma endregion VEXcode Generated Robot Configuration\n\n// Include the V5 Library\n#include \"vex.h\"\n\n#include \"cmath\"\n\n// Allows for easier use of the VEX Library\nusing namespace vex;\n\n//wheels\ndouble gearRatio = 84.0/60.0;\ndouble wheelDiameter = 4.125; //inches\ndouble wheelCircumfrence = M_PI*wheelDiameter;\n\n/////////////////////////////////\n// PID Constants and Variables //\n/////////////////////////////////\n\n// constants\ndouble driveYKP = .8;\ndouble driveYKI = 0;\ndouble driveYKD = 0;\n\n// variables\ndouble currentYValue = 0;\ndouble desiredYValue = 0;\ndouble errorY = 0;\ndouble lastYError = 0; // position 20ms ago\ndouble totalYError = 0;\ndouble derivativeY;\n\n/////////////////////\n// Movement Inputs //\n/////////////////////\n\ndouble y = 0;\n\n/////////////////\n// PID Outputs //\n/////////////////\n\ndouble driveYPIDOutput = 0;\ndouble turnPIDOutput = 0;\n\n/*--------------------------*/\n/* */\n/* Threads/Tasks */\n/* */\n/*--------------------------*/\n\n// PID boolean (allows turning PID and Odometry on/off)\nbool enableDrivePIDODO = false;\nbool enablePrinting = false;\n\n// PID control for drivetrain in X direction\nint drivePIDODO(){\n while(enableDrivePIDODO){\n\n \n ////////////////////\n // Y Movement PID //\n ////////////////////\n\n // update Y direction motor position\n currentYValue = (FL.position(degrees) + BL.position(degrees) + FR.position(degrees) + BR.position(degrees))/4; \n\n // P - Proportional: Adjusts speed of motor to error, leaves \"offset\" (does not perfectly reach destination)...\n // ... and \"noise\" (over/undershooting the destination, oscilates until reaching destination)\n errorY = desiredYValue - currentYValue;\n\n // I - Integral: Removes offset\n totalYError = totalYError + errorY;\n\n // D - Derivative: Removes noise\n derivativeY = lastYError-errorY;\n\n // OUTPUT: How fast the motors should move\n driveYPIDOutput = (driveYKP * errorY) + (driveYKI * totalYError) + (driveYKD * derivativeY);\n\n ////////////////////////////////////////////////////////////////////////////////////////////////////////////////\n\n ///////////////////////\n // Movement Commands //\n ///////////////////////\n\n FL.spin(forward, driveYPIDOutput, voltageUnits::volt);\n BL.spin(forward, driveYPIDOutput, voltageUnits::volt);\n FR.spin(forward, driveYPIDOutput, voltageUnits::volt);\n BR.spin(forward, driveYPIDOutput, voltageUnits::volt);\n \n Brain.Screen.clearScreen();\n Brain.Screen.setCursor(1,1);\n Brain.Screen.print(errorY);\n Brain.Screen.newLine();\n Brain.Screen.print(FR.position(degrees)*gearRatio);\n Brain.Screen.newLine();\n Brain.Screen.print(FL.position(degrees)*gearRatio);\n Brain.Screen.newLine();\n Brain.Screen.print(BR.position(degrees)*gearRatio);\n Brain.Screen.newLine();\n Brain.Screen.print(BL.position(degrees)*gearRatio);\n Brain.Screen.newLine();\n\n vex::task::sleep(20);\n\n lastYError = errorY;\n }\n return 1;\n}\n\nvoid preAutonomous(void) {\n // actions to do when the program starts\n Brain.Screen.clearScreen();\n Brain.Screen.print(\"pre auton code\");\n wait(1, seconds);\n FR.setPosition(0,degrees);\n BR.setPosition(0,degrees);\n FL.setPosition(0,degrees);\n BL.setPosition(0,degrees);\n}\n\nvoid autonomous(void) {\n Brain.Screen.clearScreen();\n Brain.Screen.print(\"autonomous code\");\n // place automonous code here\n enableDrivePIDODO = true;\n enablePrinting = true;\n vex::task irish(drivePIDODO);\n desiredYValue = 360.0/gearRatio;\n}\n\nvoid userControl(void) {\n Brain.Screen.clearScreen();\n enableDrivePIDODO = false;\n enablePrinting = false;\n // place driver control in this while loop\n while (true) {\n wait(20, msec);\n }\n}\n\nint main() {\n // create competition instance\n competition Competition;\n\n // Set up callbacks for autonomous and driver control periods.\n Competition.autonomous(autonomous);\n Competition.drivercontrol(userControl);\n\n // Run the pre-autonomous function.\n preAutonomous();\n\n // Prevent main from exiting with an infinite loop.\n while (true) {\n wait(100, msec);\n }\n}","textLanguage":"cpp","rconfig":[{"port":[14],"name":"FL","customName":true,"deviceType":"Motor","deviceClass":"motor","setting":{"reversed":"false","fwd":"forward","rev":"reverse","gear":"ratio18_1"},"triportSourcePort":22},{"port":[18],"name":"BL","customName":true,"deviceType":"Motor","deviceClass":"motor","setting":{"reversed":"false","fwd":"forward","rev":"reverse","gear":"ratio18_1"},"triportSourcePort":22},{"port":[13],"name":"FR","customName":true,"deviceType":"Motor","deviceClass":"motor","setting":{"reversed":"true","fwd":"forward","rev":"reverse","gear":"ratio18_1"},"triportSourcePort":22},{"port":[20],"name":"BR","customName":true,"deviceType":"Motor","deviceClass":"motor","setting":{"reversed":"true","fwd":"forward","rev":"reverse","gear":"ratio18_1"},"triportSourcePort":22}],"slot":5,"platform":"V5","sdkVersion":"20211210.18.00.00","appVersion":"2.3.0","fileFormat":"1.0.1","icon":"","targetBrainGen":"First","target":"Physical"}