Hi, my teammate is currently trying to write PID code for our mechenum drive, but can’t get it to run and is having difficulty troubleshooting it. I tried to help him, but coding isn’t really my forte. Any suggestions would be great, thanks!
// ---- START VEXCODE CONFIGURED DEVICES ----
// Robot Configuration:
// [Name] [Type] [Port(s)]
// FL motor 1
// FR motor 10
// BL motor 20
// BR motor 11
// Controller1 controller
// Flywheel motor_group 3, 4
// indexer digital_out A
// Inertial6 inertial 6
// ---- END VEXCODE CONFIGURED DEVICES ----
#include "vex.h"
#include <cmath>
using namespace vex;
competition Competition;
// Stops all motors
void myblockfunction_Stop_you_repire();
// Turns Robot to certain degree going left
void myblockfunction_inertial_left_turn_degree(double myblockfunction_inertial_left_turn_degree__degree);
// Turns Robot to certain degree going right
void myblockfunction_inertial_right_turn_degree(double myblockfunction_inertial_right_turn_degree__degree);
float myVariable, travel_distance, end_rotation;
// Stops all motors
void myblockfunction_Stop_you_repire() {
FL.stop();
FR.stop();
BL.stop();
BR.stop();
}
void myblockfunction_Go_you_repire() {
FL.spin(forward);
FR.spin(forward);
BL.spin(forward);
BR.spin(forward);
}
void myblockfunction_Back_you_repire() {
FL.spin(reverse);
FR.spin(reverse);
BL.spin(reverse);
BR.spin(reverse);
}
// Turns Robot to certain degree going left
void myblockfunction_inertial_left_turn_degree(double myblockfunction_inertial_left_turn_degree__degree) {
while ((Inertial6.heading(degrees) > myblockfunction_inertial_left_turn_degree__degree)) {
// turn left
FL.spin(reverse);
FR.spin(forward);
BL.spin(forward);
BR.spin(reverse);
wait(5, msec);
}
myblockfunction_Stop_you_repire();
}
// Turns Robot to certain degree going right
void myblockfunction_inertial_right_turn_degree(double myblockfunction_inertial_right_turn_degree__degree) {
while ((Inertial6.heading(degrees) < myblockfunction_inertial_right_turn_degree__degree)) {
// turn right
FL.spin(forward);
FR.spin(reverse);
BL.spin(reverse);
BR.spin(forward);
wait(5, msec);
}
myblockfunction_Stop_you_repire();
}
void pre_auton(void){
vexcodeInit();
}
//Settings
double kP =0.000001;
double kI =0.0;
double kD =0.000002;
//Autonomous settings
int desiredValue = 200;
int error; //SensorValue - DesireValue = Positional Value
int preError = 0; //Position 20 msec ago
int derivative; //error - preError = Speed
int totalError = 0; //totalError = total Error + error
bool resetDriveSensors=false;
//Varriables modified for use
bool enableDrivePID = true;
int drivePID(){
while(enableDrivePID){
// resets all motors
if(resetDriveSensors){
resetDriveSensors=false;
BL.setPosition(0,degrees);
BR.setPosition(0,degrees);
FL.setPosition(0,degrees);
FR.setPosition(0,degrees);
}
//get postion of all motors
int BLPostion=BL.position(degrees);
int BrPostion=BR.position(degrees);
int FLPostion=FR.position(degrees);
int FRPostion=FR.position(degrees);
//Get avarage of all motors
int avaragePostion =(BLPostion + BrPostion + FLPostion + FRPostion)/4;
//Potential
error = avaragePostion - desiredValue;
//Derivative
derivative = error - preError;
//Velocity -> Position -> Absement/Integral
totalError += error;
int forwardMotorPower = (error * kP + derivative * kD + totalError *kI);
double turnImportance = 0.5;
while (1){
//int abs;
double turnVal = Controller1.Axis1.position(percent);
double forwardVal = Controller1.Axis3.position(percent);
int turnVolts = turnVal * 0.12;
double forwardVolts = forwardVal * 0.12 * (1 - (std:: abs (turnVolts)/12.0) * turnImportance);
FL.spin(forward,forwardMotorPower+ forwardVolts, voltageUnits::volt);
FR.spin(reverse,forwardMotorPower+ forwardVolts, voltageUnits::volt);
BL.spin(forward,forwardMotorPower+ forwardVolts, voltageUnits::volt);
BR.spin(reverse,forwardMotorPower+ forwardVolts, voltageUnits::volt);
}
//code
resetDriveSensors=true;
desiredValue = 30;
resetDriveSensors=true;
desiredValue = -40;
resetDriveSensors=true;
desiredValue = 20;
resetDriveSensors=true;
desiredValue = -10;
preError = error;
vex::task::sleep(20);
}
return 1;
}
void autonomous(void) {
vex::task soy(drivePID);
}
// Driver Control Start
int driver_drivercontrol_0() {
enableDrivePID = false;
Flywheel.setVelocity(100.0, percent);
FL.spin(forward);
FR.spin(forward);
BR.spin(forward);
BL.spin(forward);
while (true) {
FR.setVelocity(((Controller1.Axis2.position() - Controller1.Axis1.position()) - Controller1.Axis4.position()), percent);
FL.setVelocity(((Controller1.Axis2.position() + Controller1.Axis1.position()) + Controller1.Axis4.position()), percent);
BR.setVelocity(((Controller1.Axis2.position() - Controller1.Axis1.position()) + Controller1.Axis4.position()), percent);
BL.setVelocity(((Controller1.Axis2.position() + Controller1.Axis1.position()) - Controller1.Axis4.position()), percent);
wait(5, msec);
}
return 0;
}
// Automaticlly callibrates sensors
int whenStarted1() {
Inertial6.startCalibration();
while (Inertial6.isCalibrating()) { task::sleep(50); }
Inertial6.setHeading(0.0, degrees);
return 0;
}
// Moves indexer forward
void onevent_Controller1ButtonR2_pressed_0() {
indexer.set(false);
}
// Moves indexer back
void onevent_Controller1ButtonR1_pressed_0() {
indexer.set(true);
}
void VEXcode_driver_task() {
// Start the driver control tasks....
vex::task drive0(driver_drivercontrol_0);
while(Competition.isDriverControl() && Competition.isEnabled()) {this_thread::sleep_for(10);}
drive0.stop();
return;
}
void VEXcode_auton_task() {
// Start the auton control tasks....
vex::task autonomous;
while(Competition.isAutonomous() && Competition.isEnabled()) {this_thread::sleep_for(10);}
autonomous.stop();
return;
}
int main() {
vex::competition::bStopTasksBetweenModes = false;
Competition.autonomous(VEXcode_auton_task);
Competition.drivercontrol(VEXcode_driver_task);
// register event handlers
Controller1.ButtonR2.pressed(onevent_Controller1ButtonR2_pressed_0);
Controller1.ButtonR1.pressed(onevent_Controller1ButtonR1_pressed_0);
wait(15, msec);
whenStarted1();
}
this. and therefore never recalculate error, forwardMotorPower etc.
and this, they are.
and unrelated, this was obviously blocks code at some point and you have the unfortunate baggage that comes with blocks to C++ conversion. You should probably rewrite the competition control part, no point starting a task, that starts another task etc. and cleanup all the crazy variables such as
Don’t know about the PID but please rename your functions. Having “myblockfunction_back_You_repire” is just bad for readability. just make it “moveBackFunc” or something, or come up with it yourself