Hey everyone,
I’ve been trying to write a program for my team’s differential drive/mobile goal. For some reason this works on an old program but on here the mobile goal lift does not move at all. The motor inverting is exactly the same as the old program. Here is the code:
#include "vex.h"
#include <cmath>
using namespace vex;
competition Competition;
void wait(float mSec){
vex::task::sleep(mSec);
}
//-------------------------
//PID Coding
double kP = 0.1;
double kI = 0.1;
double kD = 0.1;
double turnkP = 0.1;
double turnkI = 0.1;
double turnkD = 0.1;
int desiredValue = 200;
int desiredTurnValue = 0;
int error;
int prevError = 0;
int derivative;
int totalError;
int turnError;
int turnPrevError = 0;
int turnDerivative;
int turnTotalError;
bool resetDriveSensors = false;
bool enableDrivePID = true;
int drivePID(){
while(enableDrivePID){
if (resetDriveSensors){
resetDriveSensors = false;
LeftFrontDrive.setPosition(0, degrees);
LeftMiddleDrive.setPosition(0, degrees);
LeftBackDrive.setPosition(0, degrees);
RightFrontDrive.setPosition(0, degrees);
RightMiddleDrive.setPosition(0, degrees);
RightBackDrive.setPosition(0, degrees);
}
int leftFrontPosition = LeftFrontDrive.position(degrees);
int leftMiddlePosition = LeftMiddleDrive.position(degrees);
int leftBackPosition = LeftBackDrive.position(degrees);
int rightFrontPosition = RightFrontDrive.position(degrees);
int rightMiddlePosition = RightMiddleDrive.position(degrees);
int rightBackPosition = RightBackDrive.position(degrees);
int averagePosition = (leftFrontPosition + rightFrontPosition + leftMiddlePosition + rightMiddlePosition + leftBackPosition + rightBackPosition)/6;
error = averagePosition - desiredValue;
derivative = error - prevError;
totalError += error;
double lateralMotorPower = error * kP + derivative * kD + totalError * kI;
int turnDifference = leftFrontPosition - rightFrontPosition;
turnError = turnDifference - desiredTurnValue;
turnDerivative = turnError - turnPrevError;
turnTotalError += turnError;
double turnMotorPower = turnError * turnkP + turnDerivative * turnkD + turnTotalError * turnkI;
LeftFrontDrive.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits::volt);
LeftMiddleDrive.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits::volt);
LeftBackDrive.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits::volt);
RightFrontDrive.spin(forward, lateralMotorPower - turnMotorPower, voltageUnits::volt);
RightMiddleDrive.spin(forward, lateralMotorPower - turnMotorPower, voltageUnits::volt);
RightBackDrive.spin(forward, lateralMotorPower - turnMotorPower, voltageUnits::volt);
prevError = error;
turnPrevError = turnError;
vex::task::sleep(20);
}
return 1;
}
//Autonomous
void autonomous(void){
vex::task wooooo(drivePID);
resetDriveSensors = true;
desiredValue = 300;
desiredTurnValue = 600;
vex::task::sleep(1000);
resetDriveSensors = true;
desiredValue = 300;
desiredTurnValue = 600;
vex::task::sleep(1000);
}
//User Control
void usercontrol(void) {
Controller1.rumble(rumbleShort);
enableDrivePID = false;
while(Competition.isDriverControl()){
LeftFrontDrive.spin(fwd, Controller1.Axis3.value() + (Controller1.Axis1.value()*.45), pct);
LeftMiddleDrive.spin(fwd, Controller1.Axis3.value() + (Controller1.Axis1.value()*.45), pct);
LeftBackDrive.spin(fwd, Controller1.Axis3.value() + (Controller1.Axis1.value()*.45), pct);
RightFrontDrive.spin(fwd, Controller1.Axis3.value() - (Controller1.Axis1.value()*.45), pct);
RightMiddleDrive.spin(fwd, Controller1.Axis3.value() - (Controller1.Axis1.value()*.45), pct);
RightBackDrive.spin(fwd, Controller1.Axis3.value() - (Controller1.Axis1.value()*.45), pct);
bool R1Button = Controller1.ButtonR1.pressing();
bool R2Button = Controller1.ButtonR2.pressing();
bool L1Button = Controller1.ButtonL1.pressing();
bool L2Button = Controller1.ButtonL2.pressing();
//Mobile Goal Lift (R1/R2)
if(R1Button){
LeftFrontDrive.spin(fwd, 100, pct),
LeftMiddleDrive.spin(reverse, 100, pct);
RightFrontDrive.spin(fwd, 100, pct);
RightMiddleDrive.spin(reverse, 100, pct);
}
else if(R2Button){
LeftFrontDrive.spin(fwd, -100, pct);
LeftMiddleDrive.spin(reverse, 100, pct);
RightFrontDrive.spin(fwd, -100, pct);
RightMiddleDrive.spin(reverse, 100, pct);
}
//Conveyor Belt (L1/L2)
if(L1Button){
ConveyorBelt.spin(fwd, 12, volt);
}
else if (L2Button){
ConveyorBelt.spin(fwd, -12, volt);
}
else{
ConveyorBelt.spin(fwd, 0, volt);
}
}
}
//Main
int main() {
Competition.drivercontrol(usercontrol);
while (true) {
wait(1, msec);
}
}
//-------------------------
I think something might be fighting it (probably the drive control) but I’ve tried a lot and I’m not sure what to do.
Thanks!
-Omnom