Format your code using three ` before and after like this.
If you are going to try to convert motor ticks into inches use the actual PI instead of 3.14, it creates unecessary error.
To use pi put this at the top of the file you are doing conversions.
#define _USE_MATH_DEFINES
#include <cmath>
now instead of using 3.14 you can use ( M_PI * 4.125 )
EDIT: Be sure to use data type double instead of int, since PI is irrational.
/ *----------------------------------------------------------------------------* /
/* <em>/
/</em> PID Control Test code <em>/
/</em> <em>/
/</em> ---------------------------------------------------------------------------- <em>/
// ---- START VEXCODE CONFIGURED DEVICES ----
// Robot Configuration:
// [Name] [Type] [Port(s)]
// LeftDriveSide motor_group 1, 2
// RightDriveSide motor_group 3, 4
// Controller1 controller
// ---- END VEXCODE CONFIGURED DEVICES ----
#include “vex.h”
using namespace vex;
// A global instance of competition
competition Competition;
// define your global instances of motors and other devices here
/</em> --------------------------------------------------------------------------- <em>/
/</em> Pre-Autonomous Functions <em>/
/</em> <em>/
/</em> You may want to perform some actions before the competition starts. <em>/
/</em> Do them in the following function. You must return from this function <em>/
/</em> or the autonomous and usercontrol tasks will not be started. This <em>/
/</em> function is only called once after the V5 has been powered on and <em>/
/</em> not every time that the robot is disabled. <em>/
/</em> ---------------------------------------------------------------------------*/
void pre_auton(void) {
// Initializing Robot Configuration. DO NOT REMOVE!
vexcodeInit();
LeftDriveSide.setPosition(0,degrees);
RightDriveSide.setPosition(0,degrees);
// All activities that occur before the competition starts
// Example: clearing encoders, setting servo positions, …
}
//settings
double kP = 0.0;
double kI = 0.0;//these are constants that never change
double kD = 0.0;//modify these numbers if needed
double turnkP = 0.0;
double turnkI = 0.0;//these are constants that never change
double turnkD = 0.0;//modify these numbers if needed
//Autonomous Settings
int inches;
inches/12.9525 *360 = degrees;
int desiredValue = inches;//this is the amount of degrees that you desire – change if needed
int desiredTurnValue = 0;
int error;// currentSensorValue - desiredValue, positional value -> speed -> acceleration ->jerk
int prevError = 0; //position 20 miliseconds ago
int derivative; //error - prevError = speed, calculates the speed needed to reach the end value/target without overshooting -
//if it is going too fast, the derivative will have a negative effect and slow it down and if too slow, it will speed it up (positive effect)
int totalError = 0; //totalError = totalError + error
int turnError;// currentSensorValue - desiredValue, positional value -> speed -> acceleration ->jerk
int turnPrevError = 0; //position 20 miliseconds ago
int turnDerivative; //error - prevError = speed, calculates the speed needed to reach the end value/target without overshooting -
//if it is going too fast, the derivative will have a negative effect and slow it down and if too slow, it will speed it up (positive effect)
int turnTotalError = 0; //totalError = totalError + error
bool resetDriveSensors = false;
//variables modified for use
bool enableDrivePID = true;//currently when it is true, that means it is enabled
int drivePID(){
while (enableDrivePID){
if (resetDriveSensors){
resetDriveSensors = false;
LeftDriveSide.setPosition(0,degrees);
RightDriveSide.setPosition(0,degrees);
}
//get the position of both sides
int LeftDriveSidePosition = LeftDriveSide.position(degrees);
int RightDriveSidePosition = RightDriveSide.position(degrees);
//////////////////////////////////////////////////////////////////
//drive PID control
///////////////////////////////////////////////////////////////////
//get average of these 2 drive sides(motors)
int averagePosition = (LeftDriveSidePosition + RightDriveSidePosition)/2;
//Potential
error = averagePosition - desiredValue;
//Derivative
derivative = error - prevError;
//integral
//velocity -> position -> absement(postion * time) -- if pos doesnt change, then we need the absement to increase
//totalError += error;
double DriveMotorPower = error * kP + derivative * kD; // + totalError * kI /12.0 you dont have to have it be divided by 12.0
//////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////
//turn PID control
////////////////////////////////////////////////////////////////////////////////
//get average of these 2 drive sides(motors)
int turnDifference = LeftDriveSidePosition - RightDriveSidePosition;
//Potential
turnError = turnDifference - desiredTurnValue;
//Derivative
turnDerivative = turnError - turnPrevError;
//integral
//velocity -> position -> absement(postion * time) -- if pos doesnt change, then we need the absement to increase
//turnTotalError += turnError;
double TurnMotorPower = turnError * turnkP + turnDerivative * turnkD; // + turnTotalError * turnkI /12.0 you dont have to have it be divided by 12.0
///////////////////////////////////////////////////////////////////////////////////
LeftDriveSide.spin(forward, DriveMotorPower + TurnMotorPower, voltageUnits::volt);
RightDriveSide.spin(forward, DriveMotorPower + TurnMotorPower, voltageUnits::volt);
prevError = error;
turnPrevError = turnError;
vex::task::sleep(20);
}
return 1;
}
/ *---------------------------------------------------------------------------* /
/* <em>/
/</em> Autonomous Task <em>/
/</em> <em>/
/</em> This task is used to control your robot during the autonomous phase of <em>/
/</em> a VEX Competition. <em>/
/</em> <em>/
/</em> You must modify the code to add your own robot specific commands here. <em>/
/</em> ---------------------------------------------------------------------------*/
void autonomous(void) {
vex::task DrivePIDTask(drivePID);
resetDriveSensors = true;
desiredValue = 10;//this number is the number of inches the wheels will move
desiredTurnValue = 360;//the desired turn value to turn the robot – try negative turn values and see if it will turn the other direction
vex::task::sleep(1000);
resetDriveSensors = true;
desiredValue = 720;
desiredTurnValue = -360;
vex::task::sleep(1000);
}
/ *---------------------------------------------------------------------------* /
/* <em>/
/</em> User Control Task <em>/
/</em> <em>/
/</em> This task is used to control your robot during the user control phase of <em>/
/</em> a VEX Competition. <em>/
/</em> <em>/
/</em> You must modify the code to add your own robot specific commands here. <em>/
/</em> ---------------------------------------------------------------------------*/
void usercontrol(void) {
enableDrivePID = false;
// User control code here, inside the loop
while (1) {
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);
}
}
