we are trying PID for the first time and we followed a youtube tutorial by Connor 1814D and our robot will not move for whatever reason, any help or sources would be very useful
here is the code:
/----------------------------------------------------------------------------/
/* /
/ Module: main.cpp /
/ Author: VEX /
/ Created: Thu Sep 26 2019 /
/ Description: Competition Template /
/ /
/----------------------------------------------------------------------------*/
// ---- START VEXCODE CONFIGURED DEVICES ----
// ---- END VEXCODE CONFIGURED DEVICES ----
#include “vex.h”
using namespace vex;
vex::motor BLeftMotor = vex::motor( vex::PORT8,true );
vex::motor FLeftMotor = vex::motor( vex::PORT16,true );
vex::motor BRightMotor = vex::motor( vex::PORT2,true );
vex::motor FRightMotor = vex::motor( vex::PORT11,true );
vex::motor IntakeR = vex::motor( vex::PORT20,true );
vex::motor IntakeL = vex::motor( vex::PORT17,true );
vex::motor TopBoi = vex::motor( vex::PORT19,true );
vex::motor BallCarry = vex::motor( vex::PORT12,true );
vex::motor Scorer = vex::motor( vex::PORT4,true );
//vex::limit ClickyBoi =vex::limit( Brain.ThreeWirePort.A);
vex::controller Controller = vex::controller();
// 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. /
/---------------------------------------------------------------------------*/
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, …
}
//PID
//////////////////////////////////////////////
//Constants
double kP = 0.00001;
double kI = 0;
double kD = 0;
double TurnkP = 0;
double TurnkI = 0;
double TurnkD = 0;
//AutonStoof
///////////////////////////////
int desiredValue = 300;
int desiredTurnValue = 0;
int error; //SensorValue - DesireValue : Position
int prevError = 0; //Position 20 milliseconds ago
int derivative; //error - prevError : Speed
int totalError = 0; //totalError = totalError + error
int Turnerror; //SensorValue - DesireValue : Position
int TurnprevError = 0; //Position 20 milliseconds ago
int Turnderivative; //error - prevError : Speed
int TurntotalError = 0; //totalError = totalError + error
bool resetDriveSensors = false;
//Varibles
//////////////////////////////////////
int drivePID(){
if (resetDriveSensors) {
resetDriveSensors = true;
BLeftMotor.setPosition(0, degrees);
BRightMotor.setPosition(0, degrees);
FLeftMotor.setPosition(0, degrees);
FRightMotor.setPosition(0, degrees);
}
//Get the position of both motors
int BleftMotorPosition = BLeftMotor.position(degrees);
int BrightMotorPosition = BRightMotor.position(degrees);
int FleftMotorPosition = FLeftMotor.position(degrees);
int FrightMotorPosition = FRightMotor.position(degrees);
///////////////////////////////////////////////
// Lateral movement PID
/////////////////////////////////////////////////////
//average of 4 motors
int averagePostion = (BleftMotorPosition + BrightMotorPosition + FleftMotorPosition + FrightMotorPosition );
//Potential
error = averagePostion - desiredValue;
//Derivative
derivative = error - prevError;
//Intengral
totalError += error;
double LateralMotorPower = (error * kP + derivative * kD + totalError * kI);
double TurnMotorPower = (Turnerror * TurnkP + Turnderivative * TurnkD + TurntotalError * TurnkI);
BLeftMotor.spin(forward, LateralMotorPower + TurnMotorPower, voltageUnits::volt);
BRightMotor.spin(forward, LateralMotorPower - TurnMotorPower, voltageUnits::volt);
FLeftMotor.spin(forward, LateralMotorPower + TurnMotorPower, voltageUnits::volt);
FRightMotor.spin(forward, LateralMotorPower - TurnMotorPower, voltageUnits::volt);
///////////////////////////////////////////////
// Turning movement PID
/////////////////////////////////////////////////////
int TurnaveragePostion = (BleftMotorPosition + BrightMotorPosition + FleftMotorPosition + FrightMotorPosition );
//Potential
Turnerror = TurnaveragePostion - desiredTurnValue;
//Derivative
Turnderivative = Turnerror - TurnprevError;
//Intengral
TurntotalError += Turnerror;
BLeftMotor.spin(forward, LateralMotorPower + TurnMotorPower, voltageUnits::volt);
BRightMotor.spin(forward, LateralMotorPower - TurnMotorPower, voltageUnits::volt);
FLeftMotor.spin(forward, LateralMotorPower + TurnMotorPower, voltageUnits::volt);
FRightMotor.spin(forward, LateralMotorPower - TurnMotorPower, voltageUnits::volt);
//code
prevError = error;
TurnprevError = Turnerror;
vex::task::sleep(1);
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 = 300;
desiredTurnValue = 600;
vex::task::sleep(3);
resetDriveSensors = true;
desiredValue = 300;
desiredTurnValue = 300;
}
/---------------------------------------------------------------------------/
/* /
/ 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) {
while (1) {
// This is the main execution loop for the user control program.
// Each time through the loop your program should update motor + servo
// values based on feedback from the joysticks.
// ........................................................................
// Insert user code here. This is where you use the joystick values to
// update your motors, etc.
// ........................................................................
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);
}
}