Basically the robot just moves forward without stopping even as I adjust the values. Heres the most recent code.
#include "vex.h"
using namespace vex;
// A global instance of competition
competition Competition;
// define your global instances of motors and other devices here
/*---------------------------------------------------------------------------*/
/* Pre-Autonomous Functions */
/* */
/* You may want to perform some actions before the competition starts. */
/* Do them in the following function. You must return from this function */
/* or the autonomous and usercontrol tasks will not be started. This */
/* function is only called once after the V5 has been powered on and */
/* not every time that the robot is disabled. */
/*---------------------------------------------------------------------------*/
// "when Controller1 ButtonDown released" hat block
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, ...
}
int nullifiedIntegral = 0;
//Settings ADJUST THESE VALUES FOR OCELATING
double kP=0.1;
double kI=0.1;
double kD=0.5;
double turnkP=0.1;
double turnkI=0.1;
double turnkD=0.1;
int maxIntegral = 300;
int integralBound = 20;
int turnMaxIntegral = 300;
int error; //Sensor Value - Desired Value. Positional Value
int prevError=0; // Position 20 milli seconds ago
int derivative; // error - prevError = Speed
int totalError=0; //totalError = totalError + error
int turnError; //Sensor Value - Desired Value. Positional Value
int turnPrevError=0; // Position 20 milli seconds ago
int turnDerivative; // error - prevError = Speed
int turnTotalError=0; //totalError = totalError + error
//Autonomous Settings
int desiredValue = 40;
int desiredTurnValue = 0;
double signnum_c(double x) {
if (x > 0.0) return 1.0;
if (x < 0.0) return -1.0;
return x;
}
//Variables Modified for Use
bool enabledrivePID = true;
bool resetDriveSensors = false;
int drivePID(){
while(enabledrivePID){
if(resetDriveSensors) {
resetDriveSensors = false;
FrontLeftMotor.setPosition(0.0,degrees);
FrontRightMotor.setPosition(0.0,degrees);
BackLeftMotor.setPosition(0.0,degrees);
BackRightMotor.setPosition(0.0,degrees);
}
//////////////////////////////////////
/////Lateral Movement PID
//////////////////////////////////////
//Position of both motors
int leftmotorposition = FrontLeftMotor.position(degrees);
int rightmotorposition = FrontRightMotor.position(degrees);
int brightmotorposition = BackRightMotor.position(degrees);
int bleftmotorposition = BackLeftMotor.position(degrees);
//Average of both motors
int averagePosition = (leftmotorposition+rightmotorposition+brightmotorposition+bleftmotorposition)/4;
//Potential
error = averagePosition-desiredValue;
//Derivative
derivative = error-prevError;
//Integral
if(abs(error) < integralBound){
totalError+=error;
} else {
totalError = 0;
}
//totalError += error;
totalError = abs(totalError) > maxIntegral ? signnum_c(totalError) * maxIntegral : totalError;
double lateralMotorPower = error * kP + derivative * kD + totalError *kI;
/////////////////////////////////////////////////////////////
//////////////////////////////////////
/////Turning Movement PID
//////////////////////////////////////
//Average of both motors
int turnDifference = (leftmotorposition+bleftmotorposition)-(rightmotorposition+brightmotorposition);
//Potential
turnError = turnDifference-desiredTurnValue;
//Derivative
turnDerivative = turnError-turnPrevError;
//Integral
if(abs(turnError) < integralBound){
turnTotalError+=turnError;
} else {
turnTotalError = 0;
}
//totalError += error;
turnTotalError = abs(turnTotalError) > turnMaxIntegral ? signnum_c(turnTotalError) * turnMaxIntegral : turnTotalError;
double turnMotorPower = turnError * turnkP + turnDerivative * turnkD + turnTotalError * turnkI;
//////////////////////////////////////
FrontLeftMotor.spin(forward,lateralMotorPower + turnMotorPower, voltageUnits::volt);
FrontRightMotor.spin(forward,lateralMotorPower - turnMotorPower, voltageUnits::volt);
BackRightMotor.spin(forward,lateralMotorPower - turnMotorPower, voltageUnits::volt);
BackLeftMotor.spin(forward,lateralMotorPower - turnMotorPower, voltageUnits::volt);
//code
prevError=error;
turnPrevError=turnError;
vex::task::sleep(20);
}
return 1;
}
/*---------------------------------------------------------------------------*/
/* */
/* Autonomous Task */
/* */
/* This task is used to control your robot during the autonomous phase of */
/* a VEX Competition. */
/* */
/* You must modify the code to add your own robot specific commands here. */
/*---------------------------------------------------------------------------*/
void autonomous(void) {
vex::task drive(drivePID);
resetDriveSensors = true;
desiredValue = 100;
desiredTurnValue = 90;
wait(20, msec);
vex::task::stop(drive);
resetDriveSensors = true;
}
/*---------------------------------------------------------------------------*/
/* */
/* User Control Task */
/* */
/* This task is used to control your robot during the user control phase of */
/* a VEX Competition. */
/* */
/* You must modify the code to add your own robot specific commands here. */
/*---------------------------------------------------------------------------*/
void usercontrol(void) {
// User control code here, inside the loop
while (1) {
enabledrivePID = false;
wait(20, msec); // Sleep the task for a short amount of time to
// prevent wasted resources.
}
}
//
// Main will set up the competition functions and callbacks.
//
int main() {
// Set up callbacks for autonomous and driver control periods.
Competition.autonomous(autonomous);
Competition.drivercontrol(usercontrol);
// Run the pre-autonomous function.
pre_auton();
// Prevent main from exiting with an infinite loop.
while (true) {
wait(100, msec);
}
}
edit: mods fixed code tags