#pragma region VEXcode Generated Robot Configuration
// Make sure all required headers are included.
#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>
#include <math.h>
#include <string.h>
#include "vex.h"
using namespace vex;
// Brain should be defined by default
brain Brain;
// START V5 MACROS
#define waitUntil(condition) \
do { \
wait(5, msec); \
} while (!(condition))
#define repeat(iterations) \
for (int iterator = 0; iterator < iterations; iterator++)
// END V5 MACROS
// Robot configuration code.
controller Controller1 = controller(primary);
motor halfright = motor(PORT14, ratio18_1, true);
motor fullleft = motor(PORT17, ratio18_1, false);
motor halfleft = motor(PORT18, ratio18_1, true);
digital_out newmadkick = digital_out(Brain.ThreeWirePort.A);
digital_out newmadkick2 = digital_out(Brain.ThreeWirePort.B);
rotation rotate = rotation(PORT1, false);
digital_out Leftists = digital_out(Brain.ThreeWirePort.C);
digital_out Rightists = digital_out(Brain.ThreeWirePort.D);
motor LeftA = motor(PORT19, ratio6_1, false);
motor LeftB = motor(PORT9, ratio6_1, false);
motor RightA = motor(PORT12, ratio6_1, false);
motor RightB = motor(PORT13, ratio6_1, false);
// Helper to make playing sounds from the V5 in VEXcode easier and
// keeps the code cleaner by making it clear what is happening.
void playVexcodeSound(const char *soundName) {
printf("VEXPlaySound:%s\n", soundName);
wait(5, msec);
}
// define variable for remote controller enable/disable
bool RemoteControlCodeEnabled = true;
#pragma endregion VEXcode Generated Robot Configuration
// ----------------------------------------------------------------------------
//
// Project: Reformed code
// Author:
// Created: 10/23/2023
// Configuration: guh
//
// ----------------------------------------------------------------------------
// Include the V5 Library
#include <vex.h>
// Allows for easier use of the VEX Library
using namespace vex;
using namespace std;
// DECLARE VARIALBES:
//pre-auton
void preAutonomous(void) {
// actions to do when the program starts
Brain.Screen.clearScreen();
Brain.Screen.print("pre auton code");
wait(1, seconds);
}
double kP = 0.06;
double kI = 0.0;
double kD = 0.0;
double TurnkP = 0.0;
double TurnkI = 0.0;
double TurnkD = 0.0;
int DesVal;
int DesTurnVal;
int error;
int previError = 0;
int derivative;
int totalError = 0;
int Turnerror;
int TurnpreviError = 0;
int Turnderivative;
int TurntotalError = 0;
bool MiH = false; //Resets the Drive sensors when true
bool enabledrivepid = true;
double LeftTotalDivided = (LeftB.position(degrees)+LeftA.position(degrees))/2;
double RightTotalDivided = (RightB.position(degrees)+RightA.position(degrees))/2;
int DrivePid() {
while (enabledrivepid) {
if (MiH) {
MiH = false;
LeftTotalDivided = 0;
RightTotalDivided = 0;
}
double AvgPos = (RightTotalDivided + LeftTotalDivided) / 2;
error = DesVal - AvgPos; // Correct the error calculation
derivative = error - previError;
totalError += error;
double LatMotorPower = error * kP + derivative * kD + totalError * kI;
double TurnDiff = RightTotalDivided - LeftTotalDivided;
Turnerror = DesTurnVal - TurnDiff; // Correct the Turnerror calculation
Turnderivative = Turnerror - TurnpreviError;
TurntotalError += Turnerror;
double THISISPOWER = Turnerror * TurnkP + Turnderivative * TurnkD + TurntotalError * TurnkI;
// Debugging statements
Brain.Screen.printAt(10, 20, "AvgPos: %.2f", AvgPos);
Brain.Screen.printAt(10, 40, "Error: %.2f", error);
Brain.Screen.printAt(10, 60, "TurnDiff: %.2f", TurnDiff);
Brain.Screen.printAt(10, 80, "TurnError: %.2f", Turnerror);
LeftA.spin(forward, LatMotorPower + THISISPOWER, voltageUnits::volt);
LeftB.spin(forward, LatMotorPower + THISISPOWER, voltageUnits::volt);
RightA.spin(reverse, LatMotorPower - THISISPOWER, voltageUnits::volt);
RightB.spin(reverse, LatMotorPower - THISISPOWER, voltageUnits::volt);
previError = error;
TurnpreviError = Turnerror;
vex::task::sleep(20); // Adjust sleep duration if needed
}
// Stop the motors when the PID loop is disabled
LeftA.stop();
LeftB.stop();
RightA.stop();
RightB.stop();
return 1;
}
void autonomous(void) {
vex::task think2xmorethanyoubuild(DrivePid);
MiH = true;
Brain.Screen.clearScreen();
Brain.Screen.print("autonomous code");
// place automonous code here
DesVal = 1;
vex::task::sleep(2000);
MiH = true;
DesVal = 100;
DesTurnVal = 600;
}
void userControl(void) {
Brain.Screen.clearScreen();
// place driver control in this while loop
while (true) {
wait(20, msec);
//enabledrivepid = false;
//driveTrain();
LeftA.spin(forward, Controller1.Axis3.position() + Controller1.Axis1.position(), pct);
LeftB.spin(forward, Controller1.Axis3.position() + Controller1.Axis1.position(), pct);
RightB.spin(reverse, Controller1.Axis3.position() - Controller1.Axis1.position(), pct);
RightA.spin(reverse , Controller1.Axis3.position() - Controller1.Axis1.position(), pct);
//intake
}
}
int main() {
// create competition instance
competition Competition;
// Set up callbacks for autonomous and driver control periods.
Competition.autonomous(autonomous);
Competition.drivercontrol(userControl);
//vex::task think2xmorethanyoubuild(DrivePid);
// Run the pre-autonomous function.
preAutonomous();
// Prevent main from exiting with an infinite loop.
while (true) {
wait(100, msec);
}
}
Are there any clear issues?