Hey guys, now that worlds is cancelled, along with school and sports, I have a bit of free time. I’m not an incredibly experienced programmer, as you are about to see, but I have been trying to teach myself more. My team had a great robot for tower takeover, but the main flaw was that our autonomous programs were not always consistent, and we have not been using a PID. This year we just used the rotatefor command in vexcode, using the internal encoder to measure. However, as I said, this was not as reliable as I wanted. Now that I have no reason to work on my TT robot, I am doing some CAD work and trying to learn PID to utilize for the next game. I looked at @Connor’s PID tutorial, which helped a lot, but I am trying to do something a bit different, because copying and pasting code is no fun. When I run the code below, it runs the rotations, but is not escaping somewhere because I cannot run driver control after the autonomous runs, and I cannot run that same function again afterwards. I was wondering how I could make it so it runs the function, then can run other functions for the rest of autonomous, then can run driver control. I just have 1 function for driver control as a test, this is just for testing the PID system. Any other tips for PID would help as well, I am still trying to learn. Thanks.
Here is the code:
/----------------------------------------------------------------------------/
/* /
/ Module: main.cpp /
/ Author: VEX /
/ Created: Thu Sep 26 2019 /
/ Description: Clawbot Competition Template /
/ /
/----------------------------------------------------------------------------*/
// ---- START VEXCODE CONFIGURED DEVICES ----
// Robot Configuration:
// [Name] [Type] [Port(s)]
// Controller1 controller
// Drivetrain drivetrain 1, 10, D
// ClawMotor motor 3
// ArmMotor motor 8
// ---- 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
/---------------------------------------------------------------------------/
/* 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, …
}
//settings
double integral=0;
double previouserror=7200;
double Kp = 0.18;//somewhat tuned-.18
double Ki = 0.000000000001;
double Kd = 0.05;
int sensorvalue=0;
bool enablePID=true;
int drivePID(){
while(sensorvalue<7200)
{
double error = 7200 - sensorvalue;
integral = integral + error;
if(error==0)
{
integral=0;
}
double derivative = error-previouserror;
previouserror=error;
double speed = (Kp * error)+(Ki * integral)+(Kd * derivative);
ArmMotor.setVelocity(speed, percent);
ArmMotor.spin(forward);
sensorvalue=ArmMotor.position(degrees);
}
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 bill(drivePID);
vex::task::sleep(2000);
vex::task billl(drivePID);
}
/---------------------------------------------------------------------------/
/* /
/ 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
enablePID=false;
while (1) {
if (Controller1.ButtonA.pressing()){
ArmMotor.spinFor(forward, 90, degrees);
}
else
{
}
// 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);
}
}