Hi everyone,
I recently decided to write a PID for my robot, but there are a few issues with it. Before people ask, I have checked the search bar, and I have also read the George Gillard guide to PIDs. Further, I have watched the 30-minute PID video on YouTube. I do not quite know what the issue is although I am aware that there are problems. Currently the Robot will not drive at all. I am trying to control the forward motion with the internal motor encoders and the turning with the inertial sensor. If anyone can help fix the issues it would be really helpful.
#include "vex.h"
using namespace vex;
// A global instance of competition
competition Competition;
float myVariable, convspin;
float yourVariable, flyspin;
float ourVariable, index1;
void Lstrafe2(){
Rback.spin(reverse);
Rfront.spin(forward);
Lback.spin(forward);
Lfront.spin(reverse);}
void Rstrafe2(){
Rback.spin(forward);
Rfront.spin(reverse);
Lback.spin(reverse);
Lfront.spin(forward);
}
/////////////////////////////////Conveyor
void ConveyorButton() {
if (convspin == 0.0) {
Conveyor.spin(forward);
convspin = 1.0;
}
else {
Conveyor.stop();
convspin = 0.0;
}
}
int whenStarted1() {
convspin = 0.0;
return 0;
}
////////////////////////////////Flywheel
void FlyButton() {
if (flyspin == 0.0) {
Flywheel.spin(forward);
flyspin = 1.0;
}
else {
Flywheel.stop();
flyspin = 0.0;
}
}
int whenStarted2() {
flyspin = 0.0;
return 0;
}
///////////////////////////////////indexer
void IndexButton() {
if (index1 == 0.0) {
Indexer.set(true);
index1 = 1.0;
}
else {
Indexer.set(false);
index1 = 0.0;
}
}
int whenStarted3() {
index1 = 0.0;
return 0;
}
//All other voids
// Strafe Left
void Lstrafe(int numTurns){
Rback.spinFor(reverse,numTurns,turns,false);
Rfront.spinFor(forward,numTurns,turns,false);
Lback.spinFor(forward,numTurns,turns,false);
Lfront.spinFor(reverse,numTurns,turns);}
void Rstrafe(int numTurns){
Rback.spinFor(forward,numTurns,turns,false);
Rfront.spinFor(reverse,numTurns,turns,false);
Lback.spinFor(reverse,numTurns,turns,false);
Lfront.spinFor(forward,numTurns,turns);}
void pre_auton(void) {
// Initializing Robot Configuration. DO NOT REMOVE!
vexcodeInit();
}
int distanceError;
int angleError;
int desiredValue;
int desiredTurnValue;
int error; // Sensor value - desired value: position
// variables modified for use
bool enableTurnPID = true;
bool enableDrivePID = true;
bool resetDriveSensors = false;
int drivePID(){
while (enableDrivePID){
if (resetDriveSensors){
resetDriveSensors = false;
Lside.setPosition(0,turns);
Rside.setPosition(0,turns);
}
int forwardDistanceController;
int angleController;
forwardDistanceController = desiredValue;
angleController = desiredTurnValue;
int LsidePosition = Lside.position(turns);
int RsidePosition = Lside.position(turns);
int averagePosition = (LsidePosition + RsidePosition)/2;
int inertialAngle = Inertial.heading();
//Potential
distanceError = averagePosition - desiredValue;
angleError = inertialAngle - desiredTurnValue;
Lside.spinFor(forward,error,turns,false);
Rside.spinFor(forward,error,turns);
double straightOut = forwardDistanceController - (distanceError);
double angleOut = angleController - (desiredTurnValue);
int leftPower;
int rightPower;
leftPower = straightOut + angleOut;
rightPower = straightOut - angleOut;
Lside.setVelocity(leftPower,percent);
Rside.setVelocity(rightPower,percent);
vex::task::sleep(20);
}
return(1);
}
// Autonomous code
void autonomous(void) {
enableTurnPID = true;
vex::task start(drivePID);
desiredValue = 30;
desiredTurnValue = 0;
Drivetrain.stop();
while(1<2){
Controller1.Screen.clearScreen();
Controller1.Screen.print(error);
}
}
///////////////////DRIVER CONTROL
void driver(){
enableDrivePID = false;
enableTurnPID = false;
Controller2.Screen.clearScreen();
while(1<2){
Eye.setLight(ledState::on);
Eye.brightness(100);
Drivetrain.setStopping(hold);
// Prints the temperature of the Drivetrain
Controller1.Screen.clearLine(3);
Controller1.Screen.setCursor(3, 1);
Controller1.Screen.print(Drivetrain.temperature(percent));
// Prints the temperature of the Flywheel
Controller2.Screen.clearLine(2);
Controller2.Screen.setCursor(2, 1);
Controller2.Screen.print(Flywheel.temperature(percent));
// Prints the temperature of the Conveyor
Controller2.Screen.clearLine(3);
Controller2.Screen.setCursor(3, 1);
Controller2.Screen.print(Conveyor.temperature(percent));
//Drivetrain Control
Rfront.setVelocity((Controller1.Axis2.position() - (Controller1.Axis1.position() + Controller1.Axis4.position()/2)), percent);
Lfront.setVelocity((Controller1.Axis2.position() + (Controller1.Axis1.position() + Controller1.Axis4.position()/2)), percent);
Rback.setVelocity((Controller1.Axis2.position() + (Controller1.Axis1.position() - Controller1.Axis4.position()/2)), percent);
Lback.setVelocity((Controller1.Axis2.position() - (Controller1.Axis1.position() - Controller1.Axis4.position()/2)), percent);
Rfront.spin(forward);
Lfront.spin(forward);
Rback.spin(forward);
Lback.spin(forward);
// Conveyor
Conveyor.setStopping(brake);
Conveyor.setVelocity(100,percent);
if ((Controller1.ButtonB.pressing)()||(Controller2.ButtonB.pressing)()){
Conveyor.spin(reverse);
}
// Flywheel
Flywheel.setStopping(coast);
Flywheel.setVelocity(100,percent);
//Endgame
if (((Controller1.ButtonLeft.pressing)()&&(Controller1.ButtonA.pressing)())||
((Controller2.ButtonLeft.pressing)()&&(Controller2.ButtonA.pressing)())){
Endgame.set(true);
}
else{
Endgame.set(false);
}
//Flicker
if ((Controller1.ButtonR2.pressing)()||(Controller2.ButtonR2.pressing)()){
Flicker.set(true);
}
else{
Flicker.set(false);
}
}
//Eye Control
if (Eye.color()==red){
Conveyor.stop();
}
wait(0.02,seconds);
}
int main() {
Controller1.ButtonR1.pressed(ConveyorButton);
Controller2.ButtonR1.pressed(ConveyorButton);
Controller1.ButtonL1.pressed(FlyButton);
Controller2.ButtonL1.pressed(FlyButton);
Controller1.ButtonL2.pressed(IndexButton);
Controller2.ButtonL2.pressed(IndexButton);
// Initializing Robot Configuration. DO NOT REMOVE!
vexcodeInit();
Competition.autonomous(autonomous);
Competition.drivercontrol(driver);
whenStarted1();
whenStarted2();
whenStarted3();
}