When posting code, I recommend that you use [code] and the beginning and [/code] at the end, since, while using ``` works just fine, it does format a bit better if you use the code blocks, so it looks like this:
void pre_auton(void) {
// Initializing Robot Configuration. DO NOT REMOVE!
vexcodeInit();
// All activities that occur before the competition starts
// Example: clearing encoders, setting servo positions, ...
}
//Tune Here
double kp = 0.0005;
double ki = 0.0;
double kd = 0.0;
double turnkp = 0.0;
double turnki = 0.0;
double turnkd = 0.0;
//Autonomous settings
int desiredValue = 200;
int desiredturnValue = 0;
int error; //sensor value - disired value : positional value
int previouserror = 0; // position 20 milliseconds ago
int derivitive; // difference between error and previous error : Speed
int totalerror = 0; // totalerror = totalerror + error
int turnerror; //sensor value - disired value : positional value
int turnpreviouserror = 0; // position 20 milliseconds ago
int turnderivitive; // difference between error and previous error : Speed
int turntotalerror = 0; // totalerror = totalerror + error
bool resetdrivesensor = false;
// variabels motified for use
bool enabledrivepid = true;
int drivepid(){
while(enabledrivepid){
if (resetdrivesensor) {
resetdrivesensor = false;
LeftMotor.setPosition(0, degrees);
RightMotor.setPosition(0, degrees);
}
//get the position of both motors
int Leftmotorposition = LeftMotor.position(degrees);
int Rightmotorposition = RightMotor.position(degrees);
/////////////////////////////////////////////////////
// lateral movement pid
////////////////////////////////////////////////////////////////////////////////////////////
// mean of the two variables
int averageposition = Leftmotorposition + Rightmotorposition/2;
error = averageposition - desiredValue;
// derivitive
derivitive = error - previouserror;
//Intigral
//totalerror = error;
double lateralmotorPower = (error * kp + derivitive + kd + totalerror + ki) / 12.0;
//////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////
// turning movement pid
////////////////////////////////////////////////////////////////////////////////////////////
int turnDifference = Leftmotorposition - Rightmotorposition;
turnerror = turnDifference - desiredturnValue;
// derivitive
turnderivitive = turnerror - turnpreviouserror;
//Intigral
//turntotalerror = turnerror;
double turnmotorPower = (turnerror * turnkp + turnderivitive + turnkd + turntotalerror + turnki) / 12.0;
//////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////
LeftMotor.spin(forward, lateralmotorPower + turnmotorPower, voltageUnits::volt);
RightMotor.spin(forward, lateralmotorPower - turnmotorPower, voltageUnits::volt);
//code
previouserror = error;
turnpreviouserror = turnerror;
vex::task::sleep(20);
}
return 1;
}void autonomous(void) {
vex::task Tea(drivepid);
resetdrivesensor = true;
desiredValue = 300;
desiredturnValue = 600;
vex::task::sleep(100);
resetdrivesensor = true;
desiredValue = 300;
desiredturnValue = 300;
}Personally, I’ve never actually used drivetrains, so I can’t really speak to the effectiveness of them, but if you want multiple to spin outside of a drivetrain you can just assign seperate motor commands and have them run at the same time, such as spinning for a certain amount of time, like this:
motor_1.spin(fwd);
motor_2.spin(fwd);
motor_3.spin(fwd);
motor_4.spin(fwd);
sleep(1);
motor_1.stop();
motor_2.stop();
motor_3.stop();
motor_4.stop();There’s other ways to do it with spinFor and whatnot, but this is a good starting point.
I would suggest looking at this thread for some additional PID resources that might be able to help you now, and in the future.