using namespace vex;
// A global instance of competition
competition Competition;
// define your global instances of motors and other devices here
extern motor LeftMotor;
extern motor RightMotor;
extern drivetrain Drivetrain ;
extern controller Controller1 ;
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, ...
}
// Settings
double kP = 0.0; // Error
double kI = 0.0; // Last, Increase Precision
double kD = 0.0; // To Fast or Too Slow
double turnkP = 0.0 ;
double turnkI = 0.0;
double turnkD = 0.0;
// Auton Settings
int desiredValue = 200;
int desiredTurnValue = 0;
int error = 0; // SensorValue - DesireValue : Positional Value
int previouserror = 0; // Position 20 miliseconds ago
int derivative = 0; // error - previouserror : Speed
int totalerror= 0; // totalerror = totalerror + error
int turnerror; // SensorValue - DesireValue : Positional Value
int turnpreviouserror = 0; // Position 20 miliseconds ago
int turnderivative; // error - previouserror : Speed
int turntotalerror= 0; // totalerror = totalerror + error
bool resetDriveSensors = false;
// Variation modified for use
bool enableDrivePID = true;
int drivePID(){
while(enableDrivePID){
if (resetDriveSensors) {
resetDriveSensors = false;
LeftMotor.setPosition(0,degrees);
RightMotor.setPosition(0,degrees);
}
// Get Position of Both Motors
int leftMotorPosition = LeftMotor.position(degrees);
int rightMotorPosition = RightMotor.position(degrees);
//////////////////////////////////////////////////////////////////////////
//Lateral Movement PID
//////////////////////////////////////////////////////////////////////////
// Average Of the Motors
int averagePosition = (leftMotorPosition + rightMotorPosition)/2;
// Potential
error = averagePosition - desiredValue;
// Derivative
derivative = error - previouserror;
// Integral : Velocity -> Position -> Absenent ( Position x Time )
totalerror += error;
double lateralMotorPower = (error * kP + derivative * kD + totalerror *kI);
////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////
//Turning movement PID
//////////////////////////////////////////////////
// Average Of the Motors
int turnDifference = leftMotorPosition - rightMotorPosition;
// Potential
turnerror = turnDifference - desiredTurnValue;
// Derivative
turnderivative = turnerror - turnpreviouserror;
// Integral : Velocity -> Position -> Absenent ( Position x Time )
turntotalerror += turnerror;
double turnMotorPower = (turnerror * turnkP + turnderivative * turnkD + turntotalerror * turnkI);
///////////////////////////////////////////////////////////////////////
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;
}
/*---------------------------------------------------------------------------*/
/* */
/* 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 whynot(drivePID);
desiredValue = 300;
desiredTurnValue = 600;
vex::task::sleep(1000);
resetDriveSensors = true;
desiredValue = 300;
desiredTurnValue = 600;
}
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);
}
}
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) {
task whynot(drivePID);
resetDriveSensors = true;
desiredValue = 300;
desiredTurnValue = 600;
vex::task::sleep(1000);
resetDriveSensors = true;
desiredValue = 300;
desiredTurnValue = 600;
}
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.
}
}
First of all, what problems are you facing? What does the code do now?
Second, it really helps to read code when it is in [code] ... [/code]
tags.
Third, I feel like guessing the problem. If the code does nothing, then I might have an idea why. Since PID multiplies each element by its k value, you might want to set the k values to something other than 0 to test. Otherwise, you will likely get no result, with the motors not moving.
By merely pasting your code @Micah_7157X you are causing the system to be confused as it’s trying to identify what you are doing. Please use [code]
and [/code]
to make it a more readable format.
The code does nothing…
using namespace vex;
// A global instance of competition
competition Competition;
// define your global instances of motors and other devices here
extern motor LeftMotor;
extern motor RightMotor;
extern drivetrain Drivetrain ;
extern controller Controller1 ;
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, …
}
// Settings
double kP = 0.0; // Error
double kI = 0.0; // Last, Increase Precision
double kD = 0.0; // To Fast or Too Slow
double turnkP = 0.0 ;
double turnkI = 0.0;
double turnkD = 0.0;
// Auton Settings
int desiredValue = 200;
int desiredTurnValue = 0;
int error = 0; // SensorValue - DesireValue : Positional Value
int previouserror = 0; // Position 20 miliseconds ago
int derivative = 0; // error - previouserror : Speed
int totalerror= 0; // totalerror = totalerror + error
int turnerror; // SensorValue - DesireValue : Positional Value
int turnpreviouserror = 0; // Position 20 miliseconds ago
int turnderivative; // error - previouserror : Speed
int turntotalerror= 0; // totalerror = totalerror + error
bool resetDriveSensors = false;
// Variation modified for use
bool enableDrivePID = true;
int drivePID(){
while(enableDrivePID){
if (resetDriveSensors) {
resetDriveSensors = false;
LeftMotor.setPosition(0,degrees);
RightMotor.setPosition(0,degrees);
}
// Get Position of Both Motors
int leftMotorPosition = LeftMotor.position(degrees);
int rightMotorPosition = RightMotor.position(degrees);
//////////////////////////////////////////////////////////////////////////
//Lateral Movement PID
//////////////////////////////////////////////////////////////////////////
// Average Of the Motors
int averagePosition = (leftMotorPosition + rightMotorPosition)/2;
// Potential
error = averagePosition - desiredValue;
// Derivative
derivative = error - previouserror;
// Integral : Velocity -> Position -> Absenent ( Position x Time )
totalerror += error;
double lateralMotorPower = (error * kP + derivative * kD + totalerror *kI);
////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////
//Turning movement PID
//////////////////////////////////////////////////
// Average Of the Motors
int turnDifference = leftMotorPosition - rightMotorPosition;
// Potential
turnerror = turnDifference - desiredTurnValue;
// Derivative
turnderivative = turnerror - turnpreviouserror;
// Integral : Velocity -> Position -> Absenent ( Position x Time )
turntotalerror += turnerror;
double turnMotorPower = (turnerror * turnkP + turnderivative * turnkD + turntotalerror * turnkI);
///////////////////////////////////////////////////////////////////////
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;
}
/---------------------------------------------------------------------------/
/* /
/ 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 whynot(drivePID);
desiredValue = 300;
desiredTurnValue = 600;
vex::task::sleep(1000);
resetDriveSensors = true;
desiredValue = 300;
desiredTurnValue = 600;
}
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);
}
}
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) {
task whynot(drivePID);
resetDriveSensors = true;
desiredValue = 300;
desiredTurnValue = 600;
vex::task::sleep(1000);
resetDriveSensors = true;
desiredValue = 300;
desiredTurnValue = 600;
}
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.
}
}```
nevermind now it just doesnt stop once it reaches driver control
Your ki, kp, and kd values are all 0. Those values need to be tuned in order to actually make your robot move.
Look here for some critics by @Coded.
I saw the turnMotorPower issues yesterday and thought I responded but I did not hit replay. So I canceled it and linked to his post in the other thread in case you did not see it.
This topic was automatically closed 365 days after the last reply. New replies are no longer allowed.